卡尔曼滤波与组合导航原理（六）信息Kalman滤波与信息融合

一、信息滤波

1、模型

函数模型
{ X k = Φ k / k − 1 X k − 1 + Γ k − 1 W k − 1 Z k = H k X k + V k \left\{\begin{array}{l} \boldsymbol{X}_{k}=\boldsymbol{\Phi}_{k / k-1} \boldsymbol{X}_{k-1}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} \\ \boldsymbol{Z}_{k}=\boldsymbol{H}_{k} \boldsymbol{X}_{k}+\boldsymbol{V}_{k} \end{array}\right. {Xk​=Φk/k−1​Xk−1​+Γk−1​Wk−1​Zk​=Hk​Xk​+Vk​​
随机模型
{ E [ W k ] = 0 , E [ W k W j T ] = Q k δ k j E [ V k ] = 0 , E [ V k V j T ] = R k δ k j E [ W k V j T ] = 0 \left\{\begin{array}{lr} \mathrm{E}\left[\boldsymbol{W}_{k}\right]=\mathbf{0}, & \mathrm{E}\left[\boldsymbol{W}_{k} \boldsymbol{W}_{j}^{\mathrm{T}}\right]=\boldsymbol{Q}_{k} \delta_{k j} \\ \mathrm{E}\left[\boldsymbol{V}_{k}\right]=\mathbf{0}, & \mathrm{E}\left[\boldsymbol{V}_{k} \boldsymbol{V}_{j}^{\mathrm{T}}\right]=\boldsymbol{R}_{k} \delta_{k j} \\ \mathrm{E}\left[\boldsymbol{W}_{k} \boldsymbol{V}_{j}^{\mathrm{T}}\right]=\mathbf{0} & \end{array}\right. ⎩ ⎨ ⎧​E[Wk​]=0,E[Vk​]=0,E[Wk​VjT​]=0​E[Wk​WjT​]=Qk​δkj​E[Vk​VjT​]=Rk​δkj​​

"信息"的含义：信息和方差是互逆的，即 I k = p k − 1 I_k=p_k^{-1} Ik​=pk−1​ ；估计越准，方差越小，信息量越大：
P k = E [ ( X k − X ^ k ) ( X k − X ^ k ) T ] X ^ k − X k → 0 ， P k → 0 ( P k − 1 → ∞ ) X ^ k − X k → ∞ ， P k → ∞ ( P k − 1 → 0 ) \begin{array}{ll}\boldsymbol{P}_{k}=\mathrm{E}\left[\left(\boldsymbol{X}_{k}-\hat{\boldsymbol{X}}_{k}\right)\left(\boldsymbol{X}_{k}-\hat{\boldsymbol{X}}_{k}\right)^{\mathrm{T}}\right] \\ \hat{\boldsymbol{X}}_{k}-\boldsymbol{X}_{k} \rightarrow 0 ， \boldsymbol{P}_{k} \rightarrow \mathbf{0}\left(\boldsymbol{P}_{k}^{-1} \rightarrow \infty\right) \\ \hat{\boldsymbol{X}}_{k}-\boldsymbol{X}_{k} \rightarrow \infty ， \boldsymbol{P}_{k} \rightarrow \infty\left(\boldsymbol{P}_{k}^{-1} \rightarrow \mathbf{0}\right)\end{array} Pk​=E[(Xk​−X^k​)(Xk​−X^k​)T]X^k​−Xk​→0，Pk​→0(Pk−1​→∞)X^k​−Xk​→∞，Pk​→∞(Pk−1​→0)​
用 I k I_k Ik​ 替换原始Kalman滤波中的 p k − 1 p_k^{-1} pk−1​ ，得信息滤波：
K F { P k / k − 1 = Φ k / k − 1 P k − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 F k − 1 T P k = ( I − K k H k ) P k / k − 1 K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 X ^ k / k − 1 = Φ k / k − 1 X ^ k − 1 X ^ k = X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) ⟹ I K F { I k / k − 1 = ( Φ k / k − 1 I k − 1 − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T ) − 1 I k = I k / k − 1 + H k T R k − 1 H k K k = I k − 1 H k T R k − 1 X ^ k / k − 1 = Φ k / k − 1 X ^ k − 1 X ^ k = X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) \mathrm{KF}\left\{\begin{array}{l}\boldsymbol{P}_{k / k-1}=\boldsymbol{\Phi}_{k / k-1} \boldsymbol{P}_{k-1} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{F}_{k-1}^{\mathrm{T}} \\ \boldsymbol{P}_{k}=\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right) \boldsymbol{P}_{k / k-1} \\ \boldsymbol{K}_{k}=\boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)^{-1} \\ \hat{\boldsymbol{X}}_{k / k-1}=\boldsymbol{\Phi}_{k / k-1} \hat{\boldsymbol{X}}_{k-1} \\ \hat{\boldsymbol{X}}_{k}=\hat{\boldsymbol{X}}_{k / k-1}+\boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}\right)\end{array} \Longrightarrow \mathrm{IKF}\left\{\begin{array}{l}\boldsymbol{I}_{k / k-1}=\left(\boldsymbol{\Phi}_{k / k-1} \boldsymbol{I}_{k-1}^{-1} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}}\right)^{-1} \\ \boldsymbol{I}_{k}=\boldsymbol{I}_{k / k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k} \\ \boldsymbol{K}_{k}=\boldsymbol{I}_{k}^{-1} \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \\ \hat{\boldsymbol{X}}_{k / k-1}=\boldsymbol{\Phi}_{k / k-1} \hat{\boldsymbol{X}}_{k-1} \\ \hat{\boldsymbol{X}}_{k}=\hat{\boldsymbol{X}}_{k / k-1}+\boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}\right)\end{array}\right.\right. KF⎩ ⎨ ⎧​Pk/k−1​=Φk/k−1​Pk−1​Φk/k−1T​+Γk−1​Qk−1​Fk−1T​Pk​=(I−Kk​Hk​)Pk/k−1​Kk​=Pk/k−1​HkT​(Hk​Pk/k−1​HkT​+Rk​)−1X^k/k−1​=Φk/k−1​X^k−1​X^k​=X^k/k−1​+Kk​(Zk​−Hk​X^k/k−1​)​⟹IKF⎩ ⎨ ⎧​Ik/k−1​=(Φk/k−1​Ik−1−1​Φk/k−1T​+Γk−1​Qk−1​Γk−1T​)−1Ik​=Ik/k−1​+HkT​Rk−1​Hk​Kk​=Ik−1​HkT​Rk−1​X^k/k−1​=Φk/k−1​X^k−1​X^k​=X^k/k−1​+Kk​(Zk​−Hk​X^k/k−1​)​

2、信息滤波公式改写

针对这种情况对右边的公式修改：

信息预测改写

提取出 M k − 1 \color{green}M_{k-1} Mk−1​ ，红色部分用矩阵求逆引理得：

I k / k − 1 = ( Φ k / k − 1 I k − 1 − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T ) − 1 M k − 1 = Φ k / k − 1 − T I k − 1 Φ k / k − 1 − 1 = ( I + M k − 1 Γ k − 1 Q k − 1 Γ k − 1 T ) − 1 M k − 1 = [ I − M k − 1 Γ k − 1 ( Q k − 1 − 1 + Γ k − 1 T M k − 1 Γ k − 1 ) − 1 Γ k − 1 T ] M k − 1 = ( I − N ^ k − 1 ) M k − 1 \begin{aligned} \boldsymbol{I}_{k / k-1} & =\left(\boldsymbol{\Phi}_{k / k-1} \boldsymbol{I}_{k-1}^{-1} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}}\right)^{-1} \quad {\color{green}\boldsymbol{M}_{k-1}=\boldsymbol{\Phi}_{k / k-1}^{-\mathrm{T}} \boldsymbol{I}_{k-1} \boldsymbol{\Phi}_{k / k-1}^{-1}} \\ & \left.={\color{red}\left(\boldsymbol{I}+\boldsymbol{M}_{k-1} \boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}}\right)^{-1}} \boldsymbol{M}_{k-1} \\=[I-\boldsymbol{\boldsymbol { M } _ { k - 1 }} \boldsymbol{\Gamma}_{k-1}\left(\boldsymbol{Q}_{k-1}^{-1}+\boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} \boldsymbol{M}_{k-1} \boldsymbol{\Gamma}_{k-1}\right)^{-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}}\right] \boldsymbol{M}_{k-1} \\ & =\left(\boldsymbol{I}-\hat{N}_{k-1}\right) \boldsymbol{M}_{k-1} \end{aligned} Ik/k−1​​=(Φk/k−1​Ik−1−1​Φk/k−1T​+Γk−1​Qk−1​Γk−1T​)−1Mk−1​=Φk/k−1−T​Ik−1​Φk/k−1−1​=(I+Mk−1​Γk−1​Qk−1​Γk−1T​)−1Mk−1​=[I−Mk−1​Γk−1​(Qk−1−1​+Γk−1T​Mk−1​Γk−1​)−1Γk−1T​]Mk−1​=(I−N^k−1​)Mk−1​​

信息矩阵的更新就无需求逆了。

量测更新改写

将卡尔曼滤波量测更新改写为预测和量测加权平均的形式：
X ^ k = X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) = ( I − K k H k ) X ^ k / k − 1 + K k Z k = P k P k / k − 1 − 1 X ^ k / k − 1 + P k H k T R k − 1 Z k \begin{aligned} \hat{\boldsymbol{X}}_{k} & =\hat{\boldsymbol{X}}_{k / k-1}+\boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}\right) \\ & =\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right) \hat{\boldsymbol{X}}_{k / k-1}+\boldsymbol{K}_{k} \boldsymbol{Z}_{k} \\ & =\boldsymbol{P}_{k} \boldsymbol{P}_{k / k-1}^{-1} \hat{\boldsymbol{X}}_{k / k-1}+\boldsymbol{P}_{k} \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}\end{aligned} X^k​​=X^k/k−1​+Kk​(Zk​−Hk​X^k/k−1​)=(I−Kk​Hk​)X^k/k−1​+Kk​Zk​=Pk​Pk/k−1−1​X^k/k−1​+Pk​HkT​Rk−1​Zk​​
两边除以 P k P_k Pk​ 得：
P k − 1 X ^ k = P k / k − 1 − 1 X ^ k / k − 1 + H k T R k − 1 Z k \boldsymbol{P}_{k}^{-1} \hat{\boldsymbol{X}}_{k}=\boldsymbol{P}_{k / k-1}^{-1} \hat{\boldsymbol{X}}_{k / k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k} Pk−1​X^k​=Pk/k−1−1​X^k/k−1​+HkT​Rk−1​Zk​
用 I k I_k Ik​ 替换 p k − 1 p_k^{-1} pk−1​ ，将 I k X ^ k \boldsymbol{I}_{k} \hat{\boldsymbol{X}}_{k} Ik​X^k​ 记为 S ^ k \hat{\boldsymbol{S}}_{k} S^k​，得：
S ^ k = I k X ^ k = I k / k − 1 X ^ k / k − 1 + H k T R k − 1 Z k = S ^ k / k − 1 + H k T R k − 1 Z k \hat{\boldsymbol{S}}_{k}=\boldsymbol{I}_{k} \hat{\boldsymbol{X}}_{k}=\boldsymbol{I}_{k / k-1} \hat{\boldsymbol{X}}_{k / k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}=\hat{\boldsymbol{S}}_{k / k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k} S^k​=Ik​X^k​=Ik/k−1​X^k/k−1​+HkT​Rk−1​Zk​=S^k/k−1​+HkT​Rk−1​Zk​
S k − 1 S_{k-1} Sk−1​ 到 S k S_k Sk​ 的递推为：
S ^ k / k − 1 = I k / k − 1 X ^ k / k − 1 = ( I − N k − 1 ) M k − 1 Φ k / k − 1 X ^ k − 1 = ( I − N k − 1 ) Φ k / k − 1 − T I k − 1 Φ k / k − 1 − 1 Φ k / k − 1 X ^ k − 1 = ( I − N k − 1 ) Φ k / k − 1 − T S ^ k − 1 \begin{array}{l} \hat{\boldsymbol{S}}_{k / k-1}=\boldsymbol{I}_{k / k-1} \hat{\boldsymbol{X}}_{k / k-1}=\left(\boldsymbol{I}-\boldsymbol{N}_{k-1}\right) \boldsymbol{M}_{k-1} \boldsymbol{\Phi}_{k / k-1} \hat{\boldsymbol{X}}_{k-1} \\ =\left(\boldsymbol{I}-\boldsymbol{N}_{k-1}\right) \boldsymbol{\Phi}_{k / k-1}^{-\mathrm{T}} \boldsymbol{I}_{k-1} \boldsymbol{\Phi}_{k / k-1}^{-1} \boldsymbol{\Phi}_{k / k-1} \hat{\boldsymbol{X}}_{k-1}=\left(\boldsymbol{I}-\boldsymbol{N}_{k-1}\right) \boldsymbol{\Phi}_{k / k-1}^{-\mathrm{T}} \hat{\boldsymbol{S}}_{k-1} \\ \end{array} S^k/k−1​=Ik/k−1​X^k/k−1​=(I−Nk−1​)Mk−1​Φk/k−1​X^k−1​=(I−Nk−1​)Φk/k−1−T​Ik−1​Φk/k−1−1​Φk/k−1​X^k−1​=(I−Nk−1​)Φk/k−1−T​S^k−1​​

3、IKF公式汇总

滤波流程转换成了 S S S 和 I I I 的递推： S ^ k − 1 , I k − 1 , Z k ⟶ S ^ k , I k \hat{\boldsymbol{S}}_{k-1}, \boldsymbol{I}_{k-1}, \boldsymbol{Z}_{k} \longrightarrow \hat{\boldsymbol{S}}_{k}, \boldsymbol{I}_{k} S^k−1​,Ik−1​,Zk​⟶S^k​,Ik​
{ I k / k − 1 = ( Φ k / k − 1 I k − 1 − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T ) − 1 I k = I k / k − 1 + H k T R k − 1 H k K k = I k − 1 H k T R k − 1 X ^ k / k − 1 = Φ k / k − 1 X ^ k − 1 X ^ k − 1 = X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) ⟹ { M k − 1 = Φ k / k − 1 − T I k − 1 Φ k / k − 1 − 1 N k − 1 = M k − 1 Γ k − 1 ( Γ k − 1 T M k − 1 Γ k − 1 + Q k − 1 − 1 ) − 1 Γ k − 1 T ) I k / k − 1 = ( I − N k − 1 ) M k − 1 I k = I k / k − 1 + H k T R k − 1 H k S ^ k / k − 1 = ( I − N k − 1 ) Φ k / k − 1 − T S ^ k − 1 S ^ k = S ^ k / k − 1 + H k T R k − 1 Z k \left\{\begin{array}{l}\boldsymbol{I}_{k / k-1}=\left(\boldsymbol{\Phi}_{k / k-1} \boldsymbol{I}_{k-1}^{-1} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}}\right)^{-1} \\ \boldsymbol{I}_{k}=\boldsymbol{I}_{k / k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k} \\ \boldsymbol{K}_{k}=\boldsymbol{I}_{k}^{-1} \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \\ \hat{\boldsymbol{X}}_{k / k-1}=\boldsymbol{\Phi}_{k / k-1} \hat{\boldsymbol{X}}_{k-1} \\ \hat{\boldsymbol{X}}_{k-1}=\hat{\boldsymbol{X}}_{k / k-1}+\boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}\right)\end{array} \quad \Longrightarrow\left\{\begin{array}{l}\boldsymbol{M}_{k-1}=\boldsymbol{\Phi}_{k / k-1}^{-\mathrm{T}} \boldsymbol{I}_{k-1} \boldsymbol{\Phi}_{k / k-1}^{-1} \\ \left.\boldsymbol{N}_{k-1}=\boldsymbol{M}_{k-1} \boldsymbol{\Gamma}_{k-1}\left(\boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} \boldsymbol{M}_{k-1} \boldsymbol{\Gamma}_{k-1}+\boldsymbol{Q}_{k-1}^{-1}\right)^{-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}}\right) \\ \boldsymbol{I}_{k / k-1}=\left(\boldsymbol{I}-\boldsymbol{N}_{k-1}\right) \boldsymbol{M}_{k-1} \\ \boldsymbol{I}_{k}=\boldsymbol{I}_{k / k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k} \\ \hat{\boldsymbol{S}}_{k / k-1}=\left(\boldsymbol{I}-\boldsymbol{N}_{k-1}\right) \boldsymbol{\Phi}_{k / k-1}^{-\mathrm{T}} \hat{\boldsymbol{S}}_{k-1} \\ \hat{\boldsymbol{S}}_{k}=\hat{\boldsymbol{S}}_{k / k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}\end{array}\right.\right. ⎩ ⎨ ⎧​Ik/k−1​=(Φk/k−1​Ik−1−1​Φk/k−1T​+Γk−1​Qk−1​Γk−1T​)−1Ik​=Ik/k−1​+HkT​Rk−1​Hk​Kk​=Ik−1​HkT​Rk−1​X^k/k−1​=Φk/k−1​X^k−1​X^k−1​=X^k/k−1​+Kk​(Zk​−Hk​X^k/k−1​)​⟹⎩ ⎨ ⎧​Mk−1​=Φk/k−1−T​Ik−1​Φk/k−1−1​Nk−1​=Mk−1​Γk−1​(Γk−1T​Mk−1​Γk−1​+Qk−1−1​)−1Γk−1T​)Ik/k−1​=(I−Nk−1​)Mk−1​Ik​=Ik/k−1​+HkT​Rk−1​Hk​S^k/k−1​=(I−Nk−1​)Φk/k−1−T​S^k−1​S^k​=S^k/k−1​+HkT​Rk−1​Zk​​

• I 0 = 0 I_0=0 I0​=0 可以实现计算了，解决了初始方差阵无穷大的问题。
• 矩阵求逆也变少了，状态转移矩阵的求逆可以认为没有： ϕ = I + F T S ⟹ ϕ − 1 = I − F T S \phi=I+FT_S \Longrightarrow \phi^{-1}=I-FT_S ϕ=I+FTS​⟹ϕ−1=I−FTS​
• 最终输出： { I k = P k − 1 S ^ k = I k X ^ k ⇒ { P k = I k − 1 X ^ k = P k S ^ k \left\{\begin{array}{l}\boldsymbol{I}_{k}=\boldsymbol{P}_{k}^{-1} \\ \hat{\boldsymbol{S}}_{k}=\boldsymbol{I}_{k} \hat{\boldsymbol{X}}_{k}\end{array} \Rightarrow\left\{\begin{array}{l}\boldsymbol{P}_{k}=\boldsymbol{I}_{k}^{-1} \\ \hat{\boldsymbol{X}}_{k}=\boldsymbol{P}_{k} \hat{\boldsymbol{S}}_{k}\end{array}\right.\right. {Ik​=Pk−1​S^k​=Ik​X^k​​⇒{Pk​=Ik−1​X^k​=Pk​S^k​​，在前几次滤波递推中，可能 I k I_k Ik​ 不可逆，得不到均值方差。

4、KF与IKF的对偶关系

K F : { P k / k − 1 = Φ k / k − 1 P k − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T P k = [ I − P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 H k ] P k / k − 1  IKF  : { I k / k − 1 = [ I − M k − 1 Γ k − 1 ( Γ k − 1 T M k − 1 Γ k − 1 + Q k − 1 − 1 ) − 1 Γ k − 1 T ] M k − 1 I k = = I k / k − 1 + H k T R k − 1 H k \begin{array}{l}\mathrm{KF}:\left\{\begin{array}{l}\boldsymbol{P}_{k / k-1}=\boldsymbol{\Phi}_{k / k-1} \boldsymbol{P}_{k-1} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} \\ \boldsymbol{P}_{k}=\left[\boldsymbol{I}-\boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)^{-1} \boldsymbol{H}_{k}\right] \boldsymbol{P}_{k / k-1}\end{array}\right. \\ \text { IKF }:\left\{\begin{array}{l}\boldsymbol{I}_{k / k-1}=\left[\boldsymbol{I}-\boldsymbol{M}_{k-1} \boldsymbol{\Gamma}_{k-1}\left(\boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} \boldsymbol{M}_{k-1} \boldsymbol{\Gamma}_{k-1}+\boldsymbol{Q}_{k-1}^{-1}\right)^{-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}}\right] \boldsymbol{M}_{k-1} \\ \boldsymbol{I}_{k=}=\boldsymbol{I}_{k / k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\end{array}\right.\end{array} KF:⎩ ⎨ ⎧​Pk/k−1​=Φk/k−1​Pk−1​Φk/k−1T​+Γk−1​Qk−1​Γk−1T​Pk​=[I−Pk/k−1​HkT​(Hk​Pk/k−1​HkT​+Rk​)−1Hk​]Pk/k−1​​ IKF :⎩ ⎨ ⎧​Ik/k−1​=[I−Mk−1​Γk−1​(Γk−1T​Mk−1​Γk−1​+Qk−1−1​)−1Γk−1T​]Mk−1​Ik=​=Ik/k−1​+HkT​Rk−1​Hk​​​

• KF时间更新使状态估计误差增大，IKF量测更新使信息量增大；
• KF量测更新使状态估计误差减小，IKF时间更新使信息量减小；
• 系统噪声使状态估计误差增大，使状态估计信息量减小；
• 量测噪声使状态估计误差减小，使状态估计信息量增大。

二、信息融合

1、信息融合方法

可以看成特殊 N 组量测最小二乘问题，H是单位阵，从很多方面对参数进行量测，每一个量测有一个噪声，所有噪声之间不相关。需要将这些信息融合，得到最优的参数估计。
{ X ( 1 ) = X + V 1 X ( 2 ) = X + V 2 E [ V i ] = 0 , E [ V i V j T ] = P i δ i j ( i , j = 1 , 2 , ⋯   , N ) ⋮ P i > 0 i , j  表示信息来源/渠道  X ( N ) = X + V N \left\{\begin{array}{lrr}\boldsymbol{X}^{(1)}=\boldsymbol{X}+\boldsymbol{V}_{1} & \\ \boldsymbol{X}^{(2)}=\boldsymbol{X}+\boldsymbol{V}_{2} & \mathrm{E}\left[\boldsymbol{V}_{i}\right]=\mathbf{0}, & \mathrm{E}\left[\boldsymbol{V}_{i} \boldsymbol{V}_{j}^{\mathrm{T}}\right]=\boldsymbol{P}_{i} \delta_{i j} \quad(i, j=1,2, \cdots, N) \\ \vdots & &\boldsymbol{P}_{i}>0 \quad i, j \text { 表示信息来源/渠道 } \\ \boldsymbol{X}^{(N)}=\boldsymbol{X}+\boldsymbol{V}_{N} & \end{array}\right. ⎩ ⎨ ⎧​X(1)=X+V1​X(2)=X+V2​⋮X(N)=X+VN​​E[Vi​]=0,​E[Vi​VjT​]=Pi​δij​(i,j=1,2,⋯,N)Pi​>0i,j 表示信息来源/渠道 ​
可以用递推最小二乘RLS解决，也可以用信息递推最小二乘IRLS解决：
RLS ⁡ { P k − 1 = P k − 1 − 1 + H k T R k − 1 H k P k − 1 X ^ k = P k − 1 − 1 X ^ k − 1 + H k T R k − 1 Z k  IRLS  { I k = I k − 1 + H k T R k − 1 H k S ^ k = S ^ k − 1 + H k T R k − 1 Z k \begin{array}{l}\operatorname{RLS}\left\{\begin{array}{l}\boldsymbol{P}_{k}^{-1}=\boldsymbol{P}_{k-1}^{-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k} \\ \boldsymbol{P}_{k}^{-1} \hat{\boldsymbol{X}}_{k}=\boldsymbol{P}_{k-1}^{-1} \hat{\boldsymbol{X}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}\end{array}\right. \\ \text { IRLS }\left\{\begin{array}{l}\boldsymbol{I}_{k}=\boldsymbol{I}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k} \\ \hat{\boldsymbol{S}}_{k}=\hat{\boldsymbol{S}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}\end{array}\right.\end{array} RLS{Pk−1​=Pk−1−1​+HkT​Rk−1​Hk​Pk−1​X^k​=Pk−1−1​X^k−1​+HkT​Rk−1​Zk​​ IRLS {Ik​=Ik−1​+HkT​Rk−1​Hk​S^k​=S^k−1​+HkT​Rk−1​Zk​​​
一直往前递推，得到总/全局信息量级状态估计：
I g = I N − 1 + P N − 1 S ^ g = S ^ N − 1 + P N − 1 X ^ ( N ) = ( I N − 2 + P N − 1 − 1 ) + P N − 1 = ( S ^ N − 2 + P N − 1 − 1 X ^ ( N − 1 ) ) + P N − 1 X ^ ( N ) = ⋯ ⋯ = ( I 1 + P 2 − 1 ) + ⋯ + P N − 1 − 1 + P N − 1 = ( S ^ 1 + P 2 − 1 X ^ ( 2 ) ) + ⋯ + P N − 1 − 1 X ^ ( N − 1 ) + P N − 1 X ^ ( N ) = P 1 − 1 + P 2 − 1 + ⋯ + P N − 1 − 1 + P N − 1 = P 1 − 1 X ^ ( 1 ) + P 2 − 1 X ^ ( 2 ) + ⋯ + P N − 1 − 1 X ^ ( N − 1 ) + P N − 1 X ^ ( N ) \begin{aligned} \boldsymbol{I}_{g} & =\boldsymbol{I}_{N-1}+\boldsymbol{P}_{N}^{-1} & \hat{\boldsymbol{S}}_{g} & =\hat{\boldsymbol{S}}_{N-1}+\boldsymbol{P}_{N}^{-1} \hat{\boldsymbol{X}}^{(N)} \\ & =\left(\boldsymbol{I}_{N-2}+\boldsymbol{P}_{N-1}^{-1}\right)+\boldsymbol{P}_{N}^{-1} & & =\left(\hat{\boldsymbol{S}}_{N-2}+\boldsymbol{P}_{N-1}^{-1} \hat{\boldsymbol{X}}^{(N-1)}\right)+\boldsymbol{P}_{N}^{-1} \hat{\boldsymbol{X}}^{(N)} \\ & =\quad \cdots & & \cdots \\ & =\left(\boldsymbol{I}_{1}+\boldsymbol{P}_{2}^{-1}\right)+\cdots+\boldsymbol{P}_{N-1}^{-1}+\boldsymbol{P}_{N}^{-1} & & =\left(\hat{\boldsymbol{S}}_{1}+\boldsymbol{P}_{2}^{-1} \hat{\boldsymbol{X}}^{(2)}\right)+\cdots+\boldsymbol{P}_{N-1}^{-1} \hat{\boldsymbol{X}}^{(N-1)}+\boldsymbol{P}_{N}^{-1} \hat{\boldsymbol{X}}^{(N)} \\ & =\boldsymbol{P}_{1}^{-1}+\boldsymbol{P}_{2}^{-1}+\cdots+\boldsymbol{P}_{N-1}^{-1}+\boldsymbol{P}_{N}^{-1} & & =\boldsymbol{P}_{1}^{-1} \hat{\boldsymbol{X}}^{(1)}+\boldsymbol{P}_{2}^{-1} \hat{\boldsymbol{X}}^{(2)}+\cdots+\boldsymbol{P}_{N-1}^{-1} \hat{\boldsymbol{X}}^{(N-1)}+\boldsymbol{P}_{N}^{-1} \hat{\boldsymbol{X}}^{(N)}\end{aligned} Ig​​=IN−1​+PN−1​=(IN−2​+PN−1−1​)+PN−1​=⋯=(I1​+P2−1​)+⋯+PN−1−1​+PN−1​=P1−1​+P2−1​+⋯+PN−1−1​+PN−1​​S^g​​=S^N−1​+PN−1​X^(N)=(S^N−2​+PN−1−1​X^(N−1))+PN−1​X^(N)⋯=(S^1​+P2−1​X^(2))+⋯+PN−1−1​X^(N−1)+PN−1​X^(N)=P1−1​X^(1)+P2−1​X^(2)+⋯+PN−1−1​X^(N−1)+PN−1​X^(N)​
即信息融合公式：
I g = P 1 − 1 + P 2 − 1 + ⋯ + P N − 1 − 1 + P N − 1 S ^ g = P 1 − 1 X ^ ( 1 ) + P 2 − 1 X ^ ( 2 ) + ⋯ + P N − 1 − 1 X ^ ( N − 1 ) + P N − 1 X ^ ( N ) ⟹ { P g − 1 = ∑ k = 1 N P k − 1 P g − 1 X ^ g = ∑ k = 1 N P k − 1 X ^ ( k ) ⟹ { P g = ( ∑ k = 1 N P k − 1 ) − 1 X ^ g = P g ∑ k = 1 N P k − 1 X ^ ( k ) \begin{array}{l} \boldsymbol{I}_{g}=\boldsymbol{P}_{1}^{-1}+\boldsymbol{P}_{2}^{-1}+\cdots+\boldsymbol{P}_{N-1}^{-1}+\boldsymbol{P}_{N}^{-1} \\ \hat{\boldsymbol{S}}_{g}=\boldsymbol{P}_{1}^{-1} \hat{\boldsymbol{X}}^{(1)}+\boldsymbol{P}_{2}^{-1} \hat{\boldsymbol{X}}^{(2)}+\cdots+\boldsymbol{P}_{N-1}^{-1} \hat{\boldsymbol{X}}^{(N-1)}+\boldsymbol{P}_{N}^{-1} \hat{\boldsymbol{X}}^{(N)} \\ \Longrightarrow\left\{\begin{array} { l } { \boldsymbol { P } _ { g } ^ { - 1 } = \sum _ { k = 1 } ^ { N } \boldsymbol { P } _ { k } ^ { - 1 } } \\ { \boldsymbol { P } _ { g } ^ { - 1 } \hat { \boldsymbol { X } } _ { g } = \sum _ { k = 1 } ^ { N } \boldsymbol { P } _ { k } ^ { - 1 } \hat { \boldsymbol { X } } ^ { ( k ) } } \end{array} \Longrightarrow \left\{\begin{array}{l} \boldsymbol{P}_{g}=\left(\sum_{k=1}^{N} \boldsymbol{P}_{k}^{-1}\right)^{-1} \\ \hat{\boldsymbol{X}}_{g}=\boldsymbol{P}_{g} \sum_{k=1}^{N} \boldsymbol{P}_{k}^{-1} \hat{\boldsymbol{X}}^{(k)} \end{array}\right.\right. \end{array} Ig​=P1−1​+P2−1​+⋯+PN−1−1​+PN−1​S^g​=P1−1​X^(1)+P2−1​X^(2)+⋯+PN−1−1​X^(N−1)+PN−1​X^(N)⟹⎩ ⎨ ⎧​Pg−1​=∑k=1N​Pk−1​Pg−1​X^g​=∑k=1N​Pk−1​X^(k)​⟹⎩ ⎨ ⎧​Pg​=(∑k=1N​Pk−1​)−1X^g​=Pg​∑k=1N​Pk−1​X^(k)​​
本质上就是加权平均，特别的，当 N = 2 N=2 N=2 时有
P fusion  = ( P 1 − 1 + P 2 − 1 ) − 1 X ^ fusion  = P g ( P 1 − 1 X ( 1 ) + P 2 − 1 X ( 2 ) ) = P 2 P 1 + P 2 X ( 1 ) + P 1 P 1 + P 2 X ( 2 ) \begin{array}{l} \boldsymbol{P}_{\text {fusion }}=\left(\boldsymbol{P}_{1}^{-1}+\boldsymbol{P}_{2}^{-1}\right)^{-1} \\ \hat{\boldsymbol{X}}_{\text {fusion }}=\boldsymbol{P}_{g}\left(\boldsymbol{P}_{1}^{-1} \boldsymbol{X}^{(1)}+\boldsymbol{P}_{2}^{-1} \boldsymbol{X}^{(2)}\right)=\frac{\boldsymbol{P}_{2}}{\boldsymbol{P}_{1}+\boldsymbol{P}_{2}} \boldsymbol{X}^{(1)}+\frac{\boldsymbol{P}_{1}}{\boldsymbol{P}_{1}+\boldsymbol{P}_{2}} \boldsymbol{X}^{(2)} \end{array} Pfusion ​=(P1−1​+P2−1​)−1X^fusion ​=Pg​(P1−1​X(1)+P2−1​X(2))=P1​+P2​P2​​X(1)+P1​+P2​P1​​X(2)​

2、信息融合推导Kalman滤波

状态估计融合
X ^ k = ( H k T R k − 1 H k ) − 1 P k / k − 1 + ( H k T R k − 1 H k ) − 1 Φ k / k − 1 ‾ X ^ k − 1 + P k / k − 1 P k / k − 1 + ( H k T R k − 1 H k ) − 1 ( H k T R k − 1 H k ) − 1 H k T R k − 1 Z k ‾ \begin{aligned} \hat{\boldsymbol{X}}_{k} & =\frac{\left(\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1}}{\boldsymbol{P}_{k / k-1}+\left(\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1}} \underline{\boldsymbol{\Phi}_{k / k-1}} \hat{\boldsymbol{X}}_{k-1}+\frac{\boldsymbol{P}_{k / k-1}}{\boldsymbol{P}_{k / k-1}+\left(\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1}} \underline{\left(\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1} \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}} \\ \end{aligned} X^k​​=Pk/k−1​+(HkT​Rk−1​Hk​)−1(HkT​Rk−1​Hk​)−1​Φk/k−1​​X^k−1​+Pk/k−1​+(HkT​Rk−1​Hk​)−1Pk/k−1​​(HkT​Rk−1​Hk​)−1HkT​Rk−1​Zk​​​
均方差阵融合
P k = [ P k / k − 1 − 1 + ( P k ′ ) − 1 ] − 1 = ( P k / k − 1 − 1 + H k T R k − 1 H k ) − 1 \boldsymbol{P}_{k}=\left[\boldsymbol{P}_{k / k-1}^{-1}+\left(\boldsymbol{P}_{k}^{\prime}\right)^{-1}\right]^{-1}=\left(\boldsymbol{P}_{k / k-1}^{-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1} Pk​=[Pk/k−1−1​+(Pk′​)−1]−1=(Pk/k−1−1​+HkT​Rk−1​Hk​)−1