06-卡尔曼滤波原理（六）信息Kalman滤波与信息融合.md

[TOC]

一、信息滤波

1、模型

函数模型 $$ \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. $$ 随机模型 $$ \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. $$

选择合适的系统噪声分配矩阵 $\boldsymbol{\Gamma}{k}$，总可以保证系统噪声方差阵正定，$\boldsymbol{Q}{k}>0$

"信息"的含义：信息和方差是互逆的，即 $I_k=p_k^{-1}$ ；估计越准，方差越小，信息量越大： $$ \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} $$ 用 $I_k$ 替换原始Kalman滤波中的 $p_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. $$

两个问题：

• 右边公式缺点是相比左边公式求逆公式更多。
• 滤波开始时，对初始参数 $X_0$ 的状态一无所知，方差 $P_0$ 应该取无穷大，无穷大不好表示，且无穷大分之一等于 $0$ ，两边的公式都无法执行。

2、信息滤波公式改写

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

信息预测改写

提取出 $\color{green}M_{k-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} $$

$$ \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} $$

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

量测更新改写

将卡尔曼滤波量测更新改写为预测和量测加权平均的形式： $$ \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} $$ 两边除以 $P_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} $$ 用 $I_k$ 替换 $p_k^{-1}$ ，将 $\boldsymbol{I}{k} \hat{\boldsymbol{X}}{k}$ 记为 $\hat{\boldsymbol{S}}{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-1}$ 到 $S_k$ 的递推为： $$ \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} $$

3、IKF公式汇总

滤波流程转换成了 $S$ 和 $I$ 的递推：$\hat{\boldsymbol{S}}{k-1}, \boldsymbol{I}{k-1}, \boldsymbol{Z}{k} \longrightarrow \hat{\boldsymbol{S}}{k}, \boldsymbol{I}{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. $$

• $I_0=0$ 可以实现计算了，解决了初始方差阵无穷大的问题。
• 矩阵求逆也变少了，状态转移矩阵的求逆可以认为没有：$\phi=I+FT_S \Longrightarrow \phi^{-1}=I-FT_S$
• 最终输出：$\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.$，在前几次滤波递推中，可能 $I_k$ 不可逆，得不到均值方差。

4、KF与IKF的对偶关系

$$ \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时间更新使状态估计误差增大，IKF量测更新使信息量增大；
• KF量测更新使状态估计误差减小，IKF时间更新使信息量减小；
• 系统噪声使状态估计误差增大，使状态估计信息量减小；
• 量测噪声使状态估计误差减小，使状态估计信息量增大。

二、信息融合

1、信息融合方法

可以看成特殊 N 组量测最小二乘问题，H是单位阵，从很多方面对参数进行量测，每一个量测有一个噪声，所有噪声之间不相关。需要将这些信息融合，得到最优的参数估计。 $$ \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. $$ 可以用递推最小二乘RLS解决，也可以用信息递推最小二乘IRLS解决： $$ \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} $$ 一直往前递推，得到总/全局信息量级状态估计： $$ \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} $$ 即信息融合公式： $$ \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} $$ 本质上就是加权平均，特别的，当 $N=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} $$

2、信息融合推导Kalman滤波

状态估计融合 $$ \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} $$ 均方差阵融合 $$ \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} $$