一、信息滤波
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−1Xk−1+Γk−1Wk−1Zk=HkXk+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[WkVjT]=0E[WkWjT]=QkδkjE[VkVjT]=Rkδkj
选择合适的系统噪声分配矩阵 Γ k \boldsymbol{\Gamma}_{k} Γk，总可以保证系统噪声方差阵正定， Q k > 0 \boldsymbol{Q}_{k}>0 Qk>0
"信息"的含义：信息和方差是互逆的，即 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−1Pk−1Φk/k−1T+Γk−1Qk−1Fk−1TPk=(I−KkHk)Pk/k−1Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1X^k/k−1=Φk/k−1X^k−1X^k=X^k/k−1+Kk(Zk−HkX^k/k−1)⟹IKF⎩
⎨
⎧Ik/k−1=(Φk/k−1Ik−1−1Φk/k−1T+Γk−1Qk−1Γk−1T)−1Ik=Ik/k−1+HkTRk−1HkKk=Ik−1HkTRk−1X^k/k−1=Φk/k−1X^k−1X^k=X^k/k−1+Kk(Zk−HkX^k/k−1)
两个问题：
- 右边公式缺点是相比左边公式求逆公式更多。
- 滤波开始时，对初始参数 X 0 X_0 X0 的状态一无所知，方差 P 0 P_0 P0 应该取无穷大，无穷大不好表示，且无穷大分之一等于 0 0 0 ，两边的公式都无法执行。
2、信息滤波公式改写
针对这种情况对右边的公式修改：
信息预测改写
提取出 M k − 1 \color{green}M_{k-1} Mk−1 ，红色部分用矩阵求逆引理得：
( A 11 − A 12 A 22 − 1 A 21 ) − 1 = A 11 − 1 + A 11 − 1 A 12 ( A 22 − A 21 A 11 − 1 A 12 ) − 1 A 21 A 11 − 1 = [ I + A 11 − 1 A 12 ( A 22 − A 21 A 11 − 1 A 12 ) − 1 A 21 ] A 11 − 1 \begin{array}{l}\left(A_{11}-A_{12} A_{22}^{-1} A_{21}\right)^{-1}=A_{11}^{-1}+A_{11}^{-1} A_{12}\left(A_{22}-A_{21} A_{11}^{-1} A_{12}\right)^{-1} A_{21} A_{11}^{-1} \\=\left[I+A_{11}^{-1} A_{12}\left(A_{22}-A_{21} A_{11}^{-1} A_{12}\right)^{-1} A_{21}\right] A_{11}^{-1}\end{array} (A11−A12A22−1A21)−1=A11−1+A11−1A12(A22−A21A11−1A12)−1A21A11−1=[I+A11−1A12(A22−A21A11−1A12)−1A21]A11−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−1Ik−1−1Φk/k−1T+Γk−1Qk−1Γk−1T)−1Mk−1=Φk/k−1−TIk−1Φk/k−1−1=(I+Mk−1Γk−1Qk−1Γk−1T)−1Mk−1=[I−Mk−1Γk−1(Qk−1−1+Γk−1TMk−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−HkX^k/k−1)=(I−KkHk)X^k/k−1+KkZk=PkPk/k−1−1X^k/k−1+PkHkTRk−1Zk
两边除以 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−1X^k=Pk/k−1−1X^k/k−1+HkTRk−1Zk
用 I k I_k Ik 替换 p k − 1 p_k^{-1} pk−1 ，将 I k X ^ k \boldsymbol{I}_{k} \hat{\boldsymbol{X}}_{k} IkX^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=IkX^k=Ik/k−1X^k/k−1+HkTRk−1Zk=S^k/k−1+HkTRk−1Zk
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−1X^k/k−1=(I−Nk−1)Mk−1Φk/k−1X^k−1=(I−Nk−1)Φk/k−1−TIk−1Φk/k−1−1Φk/k−1X^k−1=(I−Nk−1)Φk/k−1−TS^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−1Ik−1−1Φk/k−1T+Γk−1Qk−1Γk−1T)−1Ik=Ik/k−1+HkTRk−1HkKk=Ik−1HkTRk−1X^k/k−1=Φk/k−1X^k−1X^k−1=X^k/k−1+Kk(Zk−HkX^k/k−1)⟹⎩
⎨
⎧Mk−1=Φk/k−1−TIk−1Φk/k−1−1Nk−1=Mk−1Γk−1(Γk−1TMk−1Γk−1+Qk−1−1)−1Γk−1T)Ik/k−1=(I−Nk−1)Mk−1Ik=Ik/k−1+HkTRk−1HkS^k/k−1=(I−Nk−1)Φk/k−1−TS^k−1S^k=S^k/k−1+HkTRk−1Zk
- 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−1S^k=IkX^k⇒{ Pk=Ik−1X^k=PkS^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−1Pk−1Φk/k−1T+Γk−1Qk−1Γk−1TPk=[I−Pk/k−1HkT(HkPk/k−1HkT+Rk)−1Hk]Pk/k−1 IKF :⎩ ⎨ ⎧Ik/k−1=[I−Mk−1Γk−1(Γk−1TMk−1Γk−1+Qk−1−1)−1Γk−1T]Mk−1Ik==Ik/k−1+HkTRk−1Hk
- 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+V1X(2)=X+V2⋮X(N)=X+VNE[Vi]=0,E[ViVjT]=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+HkTRk−1HkPk−1X^k=Pk−1−1X^k−1+HkTRk−1Zk IRLS {
Ik=Ik−1+HkTRk−1HkS^k=S^k−1+HkTRk−1Zk
一直往前递推，得到总/全局信息量级状态估计：
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−1S^g=S^N−1+PN−1X^(N)=(S^N−2+PN−1−1X^(N−1))+PN−1X^(N)⋯=(S^1+P2−1X^(2))+⋯+PN−1−1X^(N−1)+PN−1X^(N)=P1−1X^(1)+P2−1X^(2)+⋯+PN−1−1X^(N−1)+PN−1X^(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−1S^g=P1−1X^(1)+P2−1X^(2)+⋯+PN−1−1X^(N−1)+PN−1X^(N)⟹⎩
⎨
⎧Pg−1=∑k=1NPk−1Pg−1X^g=∑k=1NPk−1X^(k)⟹⎩
⎨
⎧Pg=(∑k=1NPk−1)−1X^g=Pg∑k=1NPk−1X^(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−1X(1)+P2−1X(2))=P1+P2P2X(1)+P1+P2P1X(2)
2、信息融合推导Kalman滤波
![[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-2k3jSiLx-1686018047371)(卡尔曼滤波与组合导航原理（六）信息滤波与信息融合.assets/1686017767586.png)]](https://img-blog.csdnimg.cn/63d198b0e14549a19ee29a1518deee93.png)
状态估计融合
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+(HkTRk−1Hk)−1(HkTRk−1Hk)−1Φk/k−1X^k−1+Pk/k−1+(HkTRk−1Hk)−1Pk/k−1(HkTRk−1Hk)−1HkTRk−1Zk
均方差阵融合
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+HkTRk−1Hk)−1