[发明专利]局部估计自适应两阶段平方根容积滤波的方法

权利要求书

1.一种局部估计自适应两阶段平方根容积滤波的方法，其特征在于，包括：

首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积Kalman滤波算法；

然后基于Sage-Husa滤波算法，提出自适应两阶段平方根容积Kalman滤波算法；

在两个阶段的滤波器中分别估计该阶段的统计特性未知的噪声，然后分别改善各自滤波器的估计效果，再用新得到的系统状态和误差协方差分别再次修正各自滤波器的噪声统计特征，形成两个滤波器各自的递推循环。

2.如权利要求1所述的方法，其特征在于，所述平方根两阶段容积Kalman滤波算法中的系统模型具体为；

带有随机偏差的非线性高斯系统模型可表示为：

其中k是离散时间序列，x_k∈R^n×1是系统的状态向量，b_k∈R^p×1是系统偏差向量，z_k∈R^m×1是量测向量，f_k(·)和h_k(·)为已知的非线性状态转移函数和量测函数且在x_k处连续可微，是系统噪声序列、是偏差噪声序列，υ_k是量测噪声序列，均为高斯白噪声序列；其中均值为E(υ_k)＝r，方差和R_k满足如下条件：

令

H_k(X_k)＝h_k(x_k)+F_kb_k

系统模型可以改写为如下形式：

X_k+1＝F_k(X_k)+ω_k

Z_k＝H_k(X_k)+υ_k

其统计特性满足如下公式(5-2)所示：

初始状态x₀、b₀与ω_k、υ_k无关，且满足

3.如权利要求2所述的方法，其特征在于，所述平方根两阶段容积Kalman滤波算法步骤如下：

Step1时间更新:

⑴计算容积点X_i,k-|k-1和通过状态转移函数计算得到的传播容积点

⑵估计k时刻的状态预测值和

⑶估计k时刻的状态误差协方差平方根因子

其中定义如公式(5-4)所示：

⑷借助U_k估计k时刻的状态误差协方差和其中

Step2量测更新:

⑴计算容积点X_i,k|k-1和通过观测函数计算得到的传播容积点Z_i,k|k-1；

⑵估计k时刻的量测预测值

⑶估计k时刻量测误差协方差的平方根因子

S_zz,k|k-1＝Tria([∈_k|k-1S_R,k]) (5-5)

其中∈_k|k-1定义如公式(5-4)所示：

⑷估计k时刻的量测误差协方差

⑸估计k时刻的交叉协方差

⑹估计卡尔曼增益和其中

⑺计算k时刻状态估计值和

⑻计算k时刻状态误差协方差估计值和V_k。