system: [SYSTEM]

user: 3. 算法设计 3.1. 卡尔曼滤波器原理 卡尔曼滤波(Kalman filter)是一种高效率的递归滤波器(自回归滤波器)，也是一种递推线性最小方差估计，它能够从一系列的不完全包含噪声的测量中，估计动态系统的状态 [ 2 ] 。 不考虑控制系统作用，设随机线性离散系统的状态方程和观测方程为 [ 3 ] ： 图1. 嵌入式组合导航系统框图 X k = Φ k ， k − 1 Χ k − 1 + Γ k ， k − 1 W k − 1 (1) Ζ k = Η k Χ k + V k (2) 式中： Χ k 是系统的n维状态向量； Ζ k 是系统的m维观测序列； W k 是p维系统过程噪声序列； V k 是m维的系统观测噪声序列； Φ k ， k − 1 是系统的 n × n 维状态转移矩阵； Γ k ， k − 1 是 n × p 维系统过程噪声输入序列； Η k 是 m × n 维观测矩阵。 假设过程噪声 W k 和观测噪声 V k 是不相关的、白噪声序列，其统计特性如下： { Ε [ W k ] = 0 ， Ε [ W k W j T ] = Q k δ k j Ε [ V k ] = 0 ， Ε [ V k V j T ] = R k δ k j Ε [ W k V k T ] = 0 (3) 式中： Q k 是系统过程噪声 W k 的 p × p 维对称非负定方差矩阵； R k 是系统观测噪声 V k 的 m × m 维对称正定方差阵；而 δ k j 是Kronecker- δ 函数。 常规kalman滤波方程，可描述如下： 状态一步预测： Χ ^ k ， k − 1 = Φ k ， k − 1 Χ ^ k − 1 (4) 状态估计： Χ ^ k = Χ ^ k ， k − 1 + Κ k [ Ζ − k Η k Χ k − 1 ] (5) 一步预测误差阵： Ρ k ， k − 1 = Φ k ， k − 1 Ρ k − 1 Φ k ， k − 1 T + T k ， k − 1 Q k − 1 T k ， k − 1 T (6) 增益矩阵： K k = P k ， k − 1 H k T ( H k P k − 1 H k T + R k ) − 1 (7) 估计误差方阵： Ρ k = [ Ι − Κ k Η k ] Ρ k ， k − 1 [ Ι − Κ k Η k ] T + Κ k R k Η k T (8) 3.2. 扩展卡尔曼滤波原理 前面介绍的离散Kalman滤波器只适用于线性系统，但是很多实际系统，如本设计所采用的组合导航系统的航姿系统是一个非线性系统，其状态方程和观测方程中存在一定的非线性，所以本文拟采用非线性滤波器，即扩展卡尔曼滤波(Extended Kalman Filter，简称EKF) [ 4 ] 。 假设一个非线性系统如下： X k = f ( X k − 1 ， k − 1 ) + Γ ( X k − 1 ， k − 1 ) W k − 1 (9) Z k = h ( X k ， k ) + V k (10) 式中： f ( · ) 是n维的向量函数，是自变量的非线性函数且对 X k 是可微的； h ( · ) 是m维向量函数，是自变量的非线性函数； W k − 1 和 V k 分别为p维系统噪声向量和m维观测噪声向量；假设 W k 和 V k 为零均值且互不相关的白噪声序列。 如果真实状态 X k 与标称状态 X k ∗ 之差(状态偏差)足够小，可以围绕滤波值 将系统状态方程(9)中的非线性函数 f ( · ) 进行泰勒级数展开，并略去二阶及以上的项可得： X k ≈ f ( X ^ ， k − 1 ) + ∂ f ( X ^ k − 1 ， k − 1 ) ∂ X ^ k − 1 T ⋅ ( X k − 1 − X ^ k − 1 ) + Γ ( X ^ k − 1 ， k − 1 ) W k − 1 (11) 令 且 f ( X ^ ， k − 1 ) − ∂ f ( X k − 1 ， k − 1 ) ∂ X k − 1 T | X k − 1 = X ^ k − 1 X ^ k − 1 = φ k − 1 则状态方程为： X k = Φ k ， k − 1 + Γ ( X ^ k − 1 ， k − 1 ) W k − 1 + φ k − 1 (12) 针对系统观测方程(10)，将非线性函数 h ( · ) 围绕滤波值 X ^ k ， k − 1 展成泰勒级数，省略二阶及以上的项可得： Z k ≈ h ( X ^ k | k − 1 ， k ) + ∂ h ( X ^ k | k − 1 ， k ) ∂ X ^ k | k − 1 T ⋅ ( X k − X ^ k | k − 1 ) + V k (13) 令 A k ， k − 1 = ∂ f ( X k − 1 ， k − 1 ) ∂ X k − 1 T | X k − 1 = X ^ k − 1 且 H k = ∂ h ( X k | k − 1 ， k ) ∂ X k | k − 1 T | X k | k − 1 = X ^ k | k − 1 则观测方程为： Z k = H k X k + y k + V k (14) 利用3.1节中推导的线性离散时间系统卡尔曼滤波基本方程，我们可以得到非线性离散时间系统的扩展卡尔曼滤波的一组递推公式。 状态一步预测： X ^ k | k − 1 = f [ X ^ k − 1 ， k − 1 ] (15) 估计误差方差阵： P k | k − 1 = A k | k − 1 P k − 1 A k | k − 1 T + B k − 1 Q k − 1 B k − 1 T (16) 状态估计： X ^ k = X ^ k | k − 1 + K k [ Z k − h ( X k | k − 1 ， k ) ] (17) 卡尔曼滤波增益矩阵： K k = P k | k − 1 H k T [ H k P k | k − 1 H k T + R k ] − 1 (18) 估计误差方差阵： P k = ( I − K k H k ) P k | k − 1 (19) 3.3. MINS/BDS组合方案 本文设计的是一种MEMS-SINS/BDS的微型组合导航系统，由MEMS-SINS系统和北斗接收机构成。BDS/SINS系统组合原理框图如图2所示，采用两级组合的方式，MEMS惯性导航系统能提供姿态角、速度、位置等导航参数，而BDS系统能提供航向、姿态和位置信息，建立扩展卡尔曼滤波方程，可以完成信息融合 [ 5 ] 。第一级组合是由MEMS加速度计和陀螺仪组合完成姿态的测量，在静态下，用MEMS加速度计和陀螺仪通过积分测得姿态角，三天线北斗接收机可利用差分信号测得初始的航向，将两者信息进行组合来完成系统的初始对准，得到组合导航系统的初始姿态角和方位信息，并将这三个量作为量测值，来估计MEMS陀螺的姿态测量误差；第二级在这种组合水平上，组合卡尔曼滤波器以及扩展卡尔曼滤波器对INS误差建模 [ 6 ] ，为MEMS-SINS系统误差提供最小方差估计，利用这些误差的估计值去修正MEMS-SINS系统，减少组合导航系统的误差，将BDS输出的位置、速度与INS输出的位置、速度之差作为滤波器的量测，相互补偿、反馈最终得到最优组合解 [ 7 ] 。在位置组合的基础上，加上速度组合，可以提高速度误差、方位误差和惯性器件误差的估计精度和收敛速度，增强INS的地面导航性能 [ 8 ] 。 图2. 组合导航系统原理图