[发明专利]两阶段高阶容积信息滤波方法

权利要求书

1.一种两阶段高阶容积信息滤波方法，其特征在于，包括：

在进行多传感器融合时，将容积Kalman滤波嵌入到扩展信息滤波算法的框架中得到容积信息滤波算法；

所述容积信息滤波算法根据五阶Spherical-Radial容积规则，得到高阶容积信息滤波算法；

根据高阶容积信息滤波算法的特点，其进行两阶段时不能直接套用两阶段扩展Kalman滤波框架，引入动态矩阵分块方法，得出两阶段高阶容积信息滤波方法；

所述扩展信息滤波算法步骤如下：

Step 1：初始化状态x₀，一阶线性化非线性函数f(·)和h(·)，求出矩阵F_k-1和H_k的值；

Setp 2：计算状态一步预测值

Step 3：计算状态一步预测协方差P_k|k-1；

Step 4：计算预测信息矩阵Y_k|k-1和预测信息向量

Step 5：计算信息矩阵贡献I_k和信息向量贡献i_k；

Step 6：计算信息更新矩阵Y_k|k和信息更新向量

Step7：根据信息更新向量求出状态估计值根据信息更新矩阵求出估计误差协方差P_k|k，完成一次迭代滤波过程；

所述扩展信息滤波算法具体为：

非线性系统模型如公式(1)所示

x_k＝f_k-1(x_k-1)+ω_k-1

z_k＝h_k(x_k)+υ_k (1)

其中k是离散时间点，为x_k为n×1的状态向量，z_k为m×1的对应于状态向量的观测向量f_k-1:R^n×1→R^n×1；和h_k：R^m×1→R^n×1均为已知可微非线性函数；过程噪声ω_k-1和测量噪声υ_k为不相关的零均值高斯白噪声过程且方差满足如下条件：

初始状态x₀满足且与ω_k、υ_k不相关；

扩展Kalman滤波算法如公式(2)所示：

其中是非线性函数f(·)的雅各比矩阵，是非线性函数h(·)的雅各比矩阵；

扩展信息滤波是扩展Kalman滤波的信息表示形式，定义Fisher信息矩阵Y_k-1|k-1和信息向量如公式(3)和(4)所示：

由公式(3)和(4)可得信息预测矩阵Y_k|k-1和信息预测向量为：

信息更新矩阵Y_k|k和信息更新向量为：

其中为信息贡献矩阵，为信息贡献向量，为新息向量；

由公式(3)和(4)可得：

所述容积信息滤波算法具体为：

根据Fisher信息矩阵Y_k-1|k-1和信息向量将容积Kalman滤波嵌入扩展信息滤波框架中；

所述容积信息滤波算法的步骤如下：

初始化状态条件：

Q₀＝Q₀,R₀＝R₀

for k＝1,2,…,N do

Step1时间更新:

1)分解P_k-1|k-1得到S_k-1|k-1其中

2)计算容积点和传播容积点

3)估计状态一步预测值

4)估计状态一步预测协方差

5)计算预测信息矩阵Yk|k-1(5)

6)计算预测信息向量

Step2量测更新

1)分解P_k|k-1得到S_k|k-1，其中

2)计算容积点和通过量测方程计算得到的传播容积点Z_i,k|k-1＝h(X_i,k|k-1,u_k)

3)估计量测预测值

4)估计交叉协方差

5)计算信息更新矩阵Y_k|k(7)

6)计算信息更新向量

7)计算状态估计值根据信息更新矩阵求出估计误差协方差P_k|k(9)；

所述五阶Spherical-Radial容积规则具体为：

根据Genz积分方法，五阶球面规则为：

其中e_j为空间Rⁿ中第j个元素为1的单位向量，和为：

权值w_s1和w_s2为：

为单位球面表面积；

五阶径向规则的点和权值为：

由公式(11)-(15)可得五阶Spherical-Radial容积规则下的高斯标准加权积分近似为：

其中N_r＝2，N_s＝2n²，五阶Spherical-Radial容积规则使用2n²+1个容积点；

所述高阶容积信息滤波算法的步骤如下：

初始化状态条件：

Q₀＝Q₀,R₀＝R₀

for k＝1,2,…,N do

Step1时间更新:

1)分解P_k-1|k-1得到S_k-1|k-1其中

2)使用新的取值点向量ξ_i(20)计算容积点X_i,k-1|k-1 (19)和传播容积点

3)使用新的权值向量w_i(23)估计状态预测值

4)估计状态误差协方差P_k|k-1(24)

5)计算预测信息矩阵Y_k|k-1(5)预测信息向量

Step2量测更新:

1)分解P_k|k-1得到S_k|k-1，其中

2)计算容积点和经过量测方程(1)传播的传播容

4)估计交叉协方差P_xz,k|k-1(26)

5)借助信息贡献矩阵I_k计算信息更新矩阵Y_k|k(17)，借助信息贡献向量计算信息更新向量

6)计算状态估计值和估计误差协方差P_k|k(9)；

所述高阶容积信息滤波算法具体如下：

由公式(3)和(4)的Fisher信息矩阵Y_k-1|k-1和信息向量以及公式(5)和(6)所示的信息预测矩阵Y_k|k-1和信息预测向量可知信息更新矩阵Y_k|k和信息更新向量为：

Y_k|k＝Y_k|k-1+I_k

高阶容积信息滤波算法描述如下：

X_i,k-1|k-1表示容积点，i＝0,1,…,2n²

其中{ξ_i,w_i}为容积点：

其中e_i为空间Rⁿ中第i个元素为1的单位向量，和如公式(10)所示；

表示传播容积点，i＝0,1,…,2n²

是k时刻的状态预测值：

其中

P_k|k-1是k时刻的状态误差协方差

是k时刻的量测预测值：

其中Z_i,k|k-1是经过测量方程传播的传播容积点(i＝1,2,...,2n²):

Z_i,k|k-1＝h(X_i,k|k-1)

P_xz,k|k-1是k时刻的互协方差

公式(18)中新息向量v_k为

2.如权利要求1所述的方法，其特征在于，所述两阶段高阶容积信息滤波算法具体如下：

初始化状态条件：

Q₀＝Q₀,R₀＝R₀

for k＝1,2,…,N do

Step1时间更新：

1)分解P_k-1|k-1得到S_k-1|k-1其中

2)使用新的取值点向量ξ_i(20)计算容积点X_i,k-1|k-1(19)和传播容积点(21)

3)使用新的权值向量w_i(23)估计改进的无偏滤波器的状态预测值(36)和有偏滤波器的(38)

4)借助耦合关系U_k(47)估计改进的无偏滤波器的状态误差协方差(45)和有偏滤波器估计状态误差协方差(46)

5)借助耦合关系U_k和S_k(50)计算改进的无偏滤波器的预测信息矩阵(48)和有偏滤波器的预测信息矩阵(49)

6)借助耦合关系S_k计算改进的无偏滤波器的预测信息向量(41)和有偏滤波器的预测信息向量(42)

Step2量测更新：

1)分解P_k|k-1得到S_k|k-1，其中

2)计算容积点和经过量测方程(1)传播的传播容积点Z_i,k|k-1＝h(X_i,k|k-1,u_k)

3)估计量测预测值(25)and新息向量v_k(27)

4)估计交叉协方差P_xz,k|k-1(26)

5)估计信息贡献向量i_k(18)和信息贡献矩阵I_k(17)

6)借助耦合关系T_k(53)计算改进的无偏滤波器的信息更新矩阵(51)和有偏滤波器的信息更新矩阵(52)

7)计算改进的无偏滤波器的信息更新向量(43)和有偏滤波器的信息更新向量(44)

8)计算改进的无偏滤波器的估计误差协方差(54),有偏滤波器的估计误差协方差(55)和耦合关系V_k(56)

9)借助耦合关系V_k计算改进的无偏滤波器的状态估计值(37)和有偏滤波器的状态估计(39)。