局部估计自适应两阶段平方根容积滤波的方法技术

本发明专利技术公开了一种局部估计自适应两阶段平方根容积滤波的方法，包括：首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积滤波算法；然后提出自适应两阶段平方根容积滤波算法；在两个阶段的滤波器中分别估计该阶段的统计特性未知的噪声，然后分别改善各自滤波器的估计效果，再用新得到的系统状态和误差协方差分别再次修正各自滤波器的噪声统计特征，形成两个滤波器各自的递推循环。本发明专利技术通过在局部估计自适应两阶段平方根容积Kalman滤波算法(ATSCKF‑G)中，对于量测噪声统计特性的估计是采用Sage‑Husa滤波器直接进行估计，然后将估计出来的统计特性值作为已知条件。

【技术实现步骤摘要】
局部估计自适应两阶段平方根容积滤波的方法
本专利技术涉及滤波估计领域，具体地说，特别涉及一种局部估计自适应两阶段平方根容积滤波的方法。
技术介绍
非线性滤波算法是在非线性系统模型下利用离散的传感器观测量来估计目标的连续状态并滤去随机噪声的过程。目前几种常见的非线性Kalman滤波有各自的优缺点。扩展Kalman滤波(EKF)将非线性函数进行泰勒展开，忽略高维项进行线性化，但这种方法只适合系统模型足够平滑的弱非线性函数，若系统是强非线性系统，则会因为滤波误差较大而失去有效性，同时在计算时需要计算雅克比矩阵，计算量较大。无迹Kalman滤波(UKF)不需要计算Jacobian矩阵，计算量与EKF相当，在强非线性系统下也有较高的滤波精度。但是UKF没有经过严格的数学推导，在状态维数较高时，滤波精度会下降。采用三度Spherical-Radial容积规则近似非线性函数传递的后验均值和协方差的容积Kalman滤波，经过了严密的数学推导，在理论上具有严格的保证。在滤波迭代过程中，有些因素的存在会导致误差协方差矩阵负定，比如数值计算中的舍入误差、初始值误差较大和观测噪声较大等，从而导致滤波器不稳定，甚至无法工作。
技术实现思路
为了解决现有技术的问题，本专利技术实施例提供了一种局部估计自适应两阶段平方根容积滤波的方法。所述技术方案如下：一方面，提供了一种局部估计自适应两阶段平方根容积滤波的方法，包括：首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积Kalman滤波算法；然后基于Sage-Husa滤波算法，提出自适应两阶段平方根容积Kalman滤波算法；在两阶段容积Kalman滤波的过程中，整体的对噪声未知统计特性进行估计，将估计出的噪声统计特性作为已知条件进行递推估计。进一步的，所述平方根两阶段容积Kalman滤波算法中的系统模型具体为；带有随机偏差的非线性高斯系统模型可表示为：其中k是离散时间序列，xk∈Rn×1是系统的状态向量，bk∈Rp×1是系统偏差向量，zk∈Rm×1是量测向量，fk(·)和hk(·)为已知的非线性状态转移函数和量测函数且在xk处连续可微，是系统噪声序列、是偏差噪声序列，υk是量测噪声序列，均为高斯白噪声序列。其中均值为E(υk)＝r，方差和Rk满足如下条件：令Hk(Xk)＝hk(xk)+Fkbk系统模型可以改写为如下形式：Xk+1＝Fk(Xk)+ωkZk＝Hk(Xk)+υk其统计特性满足如下公式(5-2)所示：初始状态x0、b0与ωk、υk无关，且满足进一步的，所述平方根两阶段容积Kalman滤波算法步骤如下：Step1时间更新:⑴计算容积点Xi,k-|k-1和通过状态转移函数计算得到的传播容积点⑵估计k时刻的状态预测值和⑶估计k时刻的状态误差协方差平方根因子其中定义如公式(5-4)所示：⑷借助Uk估计k时刻的状态误差协方差和其中Step2量测更新:⑴计算容积点Xi,k|k-1和通过观测函数计算得到的传播容积点Zi,k|k-1；⑵估计k时刻的量测预测值⑶估计k时刻量测误差协方差的平方根因子Szz,k|k-1＝Tria([∈k|k-1SR，k])(5-5)其中∈k|k-1定义如公式(5-4)所示：⑷估计k时刻的量测误差协方差⑸估计k时刻的交叉协方差⑹估计卡尔曼增益和其中⑺计算k时刻状态估计值和⑻计算k时刻状态误差协方差估计值和Vk。进一步的，所述系统模型如公式(5-1)所示，具有如公式(5-2)所示的噪声统计特性，在噪声统计特性未知或不准确的情况下，基于Sage-Husa算法可得次优常值噪声估计器：其中是j-1时刻的状态估计值经过非线性状态函数f(·)传播后得到的状态预测值，是j时刻的状态估计值经过非线性量测函数h(·)传播后得到的量测预测值，在容积Kalman滤波算法中有Xi,j-1|j-1与Xi,j|j-1分别表示j-1与j时刻状态估计值与计算得到的容积点。采用MAP估计器，得到噪声统计特性未知的无偏估计器，结合容积Kalman滤波算法，可得无偏常值噪声估计器：其中表示新息向量。进一步的，所述系统模型如公式(5-1)所示，公式(5-2)为其噪声统计特性，如果其噪声为时变的情况，需要减弱或消除陈旧量测信息，加大最近量测信息的作用，因此对求和式中的每一项引入不同的加权系数；采用渐消记忆指数加权的方式对时变噪声进行估计，可得无偏时变噪声估计器：其中dk-1＝(1-b)/(1-bk)，b为遗忘因子，用来限制滤波器的记忆长度，通常b的取值范围为0.95＜b＜0.99，b的值越大越加重最近量测信息的作用，当噪声变化快时，b的取值应偏大，反之则偏小。进一步的，所述系统模型形如公式(5-1)所示，在实际应用中，量测噪声容易受到量测误差和环境扰动等方面的影响，因此在公式(5-2)所示的噪声统计特性中，系统噪声ωk为均值为qk，方差为Qk的高斯白噪声序列，量测噪声υk也为高斯白噪声序列，但是统计特性未知；结合Sage-Husa算法，首先采用局部估计的方式，对未知的量测噪声统计特性进行局部估计，然后再根据两阶段方法，对滤波器进行分解；进一步的，所述在两个阶段的滤波器中分别估计该阶段的统计特性未知的噪声，然后分别改善各自滤波器的估计效果，再用新得到的系统状态和误差协方差分别再次修正各自滤波器的噪声统计特征，形成两个滤波器各自的递推循环具体为：局部估计自适应两阶段平方根容积Kalman滤波算法(ATSCKF-L)中均以上角标L标明以示区分，算法步骤如下：初始化：Step1时间更新：⑴计算容积点Xi,k-|k-1和通过状态转移函数计算得到的传播容积点⑵由于系统噪声均值不为0，故有其中⑶估计k时刻的状态预测值和得到⑷估计k时刻的状态误差协方差平方根因子其中定义如公式(5-25)所示：⑸估计k时刻的状态误差协方差和其中⑹借助Uk计算和其中Step2量测更新：⑴计算容积点Xi，k|k-1和通过观测函数计算得到的传播容积点⑵由于测量噪声的均值也不为0，估计k时刻的量测预测值其中⑶估计k时刻的量测误差协方差的平方根因子其中⑷估计k时刻的量测误差协方差⑸估计k时刻的交叉协方差⑹估计卡尔曼增益和其中⑺计算k时刻状态估计值和⑻计算k时刻状态误差协方差估计值和Vk；Step3噪声估计：⑴分别估计k时刻状态噪声的均值和由公式(5-18)可得即得到⑵分别估计k时刻状态噪声的方差由公式(5-19)可得其中即得到Step4：如果k<N，N为跟踪时间，转到Step1。进一步的，在局部估计自适应两阶段平方根容积Kalman滤波算法中，计算系统噪声统计特性时，通过得到系统噪声统计特性，并在两个阶段滤波中各自的表示方法，然后采用Sage-Husa滤波器分别进行估计，将估计出来的统计特性值加入到下一个时刻的递推估计中。本专利技术实施例提供的技术方案带来的有益效果是：本专利技术通过采用Saga-Husa估计器实时估计噪声的未知统计特性，再将实时更新的统计特性带入Kalman滤波算法，改善噪声统计特性未知系统中的滤波估计效果，再用新得到的系统状态和误差协方差再次修正噪声统计特征，以此形成递推循环；在局部估计自适应两阶段平方根容积Kalman滤波算法(ATSCKF-G)中，对于量测噪声统计特性的估计是采用Sage-H本文档来自技高网...

【技术保护点】
1.一种局部估计自适应两阶段平方根容积滤波的方法，其特征在于，包括：首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积Kalman滤波算法；然后基于Sage‑Husa滤波算法，提出自适应两阶段平方根容积Kalman滤波算法；在两个阶段的滤波器中分别估计该阶段的统计特性未知的噪声，然后分别改善各自滤波器的估计效果，再用新得到的系统状态和误差协方差分别再次修正各自滤波器的噪声统计特征，形成两个滤波器各自的递推循环。

【技术特征摘要】
1.一种局部估计自适应两阶段平方根容积滤波的方法，其特征在于，包括：首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积Kalman滤波算法；然后基于Sage-Husa滤波算法，提出自适应两阶段平方根容积Kalman滤波算法；在两个阶段的滤波器中分别估计该阶段的统计特性未知的噪声，然后分别改善各自滤波器的估计效果，再用新得到的系统状态和误差协方差分别再次修正各自滤波器的噪声统计特征，形成两个滤波器各自的递推循环。2.如权利要求1所述的方法，其特征在于，所述平方根两阶段容积Kalman滤波算法中的系统模型具体为；带有随机偏差的非线性高斯系统模型可表示为：其中k是离散时间序列，xk∈Rn×1是系统的状态向量，bk∈Rp×1是系统偏差向量，zk∈Rm×1是量测向量，fk(·)和hk(·)为已知的非线性状态转移函数和量测函数且在xk处连续可微，是系统噪声序列、是偏差噪声序列，υk是量测噪声序列，均为高斯白噪声序列；其中均值为E(υk)＝r，方差和Rk满足如下条件：令Hk(Xk)＝hk(xk)+Fkbk系统模型可以改写为如下形式：Xk+1＝Fk(Xk)+ωkZk＝Hk(Xk)+υk其统计特性满足如下公式(5-2)所示：初始状态x0、b0与ωk、υk无关，且满足3.如权利要求2所述的方法，其特征在于，所述平方根两阶段容积Kalman滤波算法步骤如下：Step1时间更新:⑴计算容积点Xi,k-|k-1和通过状态转移函数计算得到的传播容积点⑵估计k时刻的状态预测值和⑶估计k时刻的状态误差协方差平方根因子其中定义如公式(5-4)所示：⑷借助Uk估计k时刻的状态误差协方差和其中Step2量测更新:⑴计算容积点Xi,k|k-1和通过观测函数计算得到的传播容积点Zi,k|k-1；⑵估计k时刻的量测预测值⑶估计k时刻量测误差协方差的平方根因子Szz,k|k-1＝Tria([∈k|k-1SR,k])(5-5)其中∈k|k-1定义如公式(5-4)所示：⑷估计k时刻的量测误差协方差⑸估计k时刻的交叉协方差⑹估计卡尔曼增益和其中⑺计算k时刻状态估计值和⑻计算k时刻状态误差协方差估计值和Vk。4.如权利要求3所述的方法，其特征在于，所述系统模型如公式(5-1)所示，具有如公式(5-2)所示的噪声统计特性，在噪声统计特性未知或不准确的情况下，基于Sage-Husa算法可得次优常值噪声估计器：其中是j-1时刻的状态估计值经过非线性状态函数f(·)传播后得到的状态预测值，是j时刻的状态估计值经过非线性量测函数h(·)传播后得到的量测预测值，在容积Kalman滤波算法中有Xi,j-1|j-1与Xi,j|j-1分别表示j-1...

【专利技术属性】
技术研发人员：张露，
申请(专利权)人：衢州学院，
类型：发明
国别省市：浙江,33