三、卡尔曼滤波

1、随机系统状态空间模型

$$\{\begin{array}{l}{\mathbf{X}}_k={\mathbf{\Phi }}_{k/k-1}{\mathbf{X}}_{k-1}+{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}\\ {\mathbf{Z}}_k={\mathbf{H}}_k{\mathbf{X}}_k+{\mathbf{V}}_k\end{array}$$

关于噪声的基本假设：系统噪声 ${\mathbf{W}}_k$ 和量测噪声 ${\mathbf{V}}_k$ 都是零均值高斯白噪声，且不相关；系统噪声协方差阵 ${\mathbf{Q}}_k$ 对称非负定，观测噪声协方差阵 ${\mathbf{R}}_k$ 对称正定。

• 其实改系统噪声前面系数 ${\Gamma }_k$ 可以使 ${\mathbf{Q}}_k$ 总正定。
• 必须是高斯白噪声，多个高斯白噪声线性组合还是高斯白噪声，零均值均匀分布噪声线性组合不一定还是零均值均匀分布噪声。

$$\begin{array}{l}\{\begin{array}{l}\mathrm{E}\left[{\mathbf{W}}_k\right]=0,\mathrm{E}\left[{\mathbf{W}}_k{\mathbf{W}}_j^{\mathrm{T}}\right]={\mathbf{Q}}_k{\delta }_{kj}\\ \mathrm{E}\left[{\mathbf{V}}_k\right]=0,\mathrm{E}\left[{\mathbf{V}}_k{\mathbf{V}}_j^{\mathrm{T}}\right]={\mathbf{R}}_k{\delta }_{kj}\\ \mathrm{E}\left[{\mathbf{W}}_k{\mathbf{V}}_j^{\mathrm{T}}\right]=0\end{array}\\ {\mathbf{Q}}_k\ge 0,{\mathbf{R}}_k>0，或{\mathbf{Q}}_k>0,{\mathbf{R}}_k>0\end{array}$$

2、状态预测

**状态一步预测：**预测下一时刻最有可能的那个值，由于高斯白噪声对状态预测的一阶矩不起作用（线性最小方差估计）：
$${\hat{\mathbf{X}}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}{\hat{\mathbf{X}}}_{k-1}$$
状态预测误差：真值-预测
$$\begin{array}{r}{\tilde{\mathbf{X}}}_{k/k-1}={\mathbf{X}}_k-{\hat{\mathbf{X}}}_{k/k-1}=\left({\mathbf{\Phi }}_{k/k-1}{\mathbf{X}}_{k-1}+{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}\right)-{\mathbf{\Phi }}_{k/k-1}{\hat{\mathbf{X}}}_{k-1}\\ ={\mathbf{\Phi }}_{k/k-1}\left({\mathbf{X}}_{k-1}-{\hat{\mathbf{X}}}_{k-1}\right)+{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}={\mathbf{\Phi }}_{k/k-1}{\tilde{\mathbf{X}}}_{k-1}+{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}\end{array}$$
状态预测方差阵：一步预测误差相乘的期望，乘出来有四项，由于两时刻噪声不相关，其中两项为0。所以预测的方差就是上一时刻的方差传递过来 ${\mathbf{\Phi }}_{k/k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}$，加上当前误差 ${\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}$
$$\begin{array}{rl}{\mathbf{P}}_{k/k-1}& =\mathrm{E}\left[{\tilde{\mathbf{X}}}_{k/k-1}{\tilde{\mathbf{X}}}_{k/k-1}^{\mathrm{T}}\right]\\ & =\mathrm{E}\left[\left({\mathbf{\Phi }}_{k/k-1}{\tilde{\mathbf{X}}}_{k-1}+{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}\right){\left({\mathbf{\Phi }}_{k/k-1}{\tilde{\mathbf{X}}}_{k-1}+{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}\right)}^{\mathrm{T}}\right]\\ & ={\mathbf{\Phi }}_{k/k-1}\mathrm{E}\left[{\tilde{\mathbf{X}}}_{k-1}{\tilde{\mathbf{X}}}_{k-1}^{\mathrm{T}}\right]{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}\mathrm{E}\left[{\mathbf{W}}_{k-1}{\mathbf{W}}_{k-1}^{\mathrm{T}}\right]{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}\\ & ={\mathbf{\Phi }}_{k/k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}\end{array}$$

3、状态量测

量测：量测值带入量测方程，同样忽略高斯白噪声的影响：
$${\hat{\mathbf{Z}}}_{k/k-1}={\mathbf{H}}_k{\hat{\mathbf{X}}}_{k/k-1}$$
量测误差：
$$\begin{array}{rl}{\tilde{\mathbf{Z}}}_{k/k-1}& ={\mathbf{Z}}_k-{\hat{\mathbf{Z}}}_{k/k-1}=\left({\mathbf{H}}_k{\mathbf{X}}_k+{\mathbf{V}}_k\right)-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k/k-1}={\mathbf{H}}_k{\tilde{\mathbf{X}}}_{k/k-1}+{\mathbf{V}}_k\end{array}$$
量测方差阵：
$$\begin{array}{rl}{\mathbf{P}}_{ZZ,k/k-1}& =\mathrm{E}\left[{\tilde{\mathbf{Z}}}_{k/k-1}{\tilde{\mathbf{Z}}}_{k/k-1}^{\mathrm{T}}\right]\\ & =\mathrm{E}\left[\left({\mathbf{H}}_k{\tilde{\mathbf{X}}}_{k/k-1}+{\mathbf{V}}_k\right){\left({\mathbf{H}}_k{\tilde{\mathbf{X}}}_{k/k-1}+{\mathbf{V}}_k\right)}^{\mathrm{T}}\right]\\ & ={\mathbf{H}}_k\mathrm{E}\left[{\tilde{\mathbf{X}}}_{k/k-1}{\tilde{\mathbf{X}}}_{k/k-1}^{\mathrm{T}}\right]{\mathbf{H}}_k^{\mathrm{T}}+\mathrm{E}\left[{\mathbf{V}}_k{\mathbf{V}}_k^{\mathrm{T}}\right]\\ & ={\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\end{array}$$
量测互协方差：X 与 Z 之间，状态的一步预测误差和量测之间的互协方差

一般不为0，为0就没有滤波的必要了

$$\begin{array}{l}{\mathbf{P}}_{XZ,k/k-1}=\mathrm{E}\left[{\tilde{\mathbf{X}}}_{k/k-1}{\tilde{\mathbf{Z}}}_{k/k-1}^{\mathrm{T}}\right]\\ =\mathrm{E}\left[{\tilde{\mathbf{X}}}_{k/k-1}{\left({\mathbf{H}}_k{\tilde{\mathbf{X}}}_{k/k-1}+{\mathbf{V}}_k\right)}^{\mathrm{T}}\right]\\ ={\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}\end{array}$$

4、增益矩阵K与状态估计

假设滤波方程可以写成 状态预测+K量测修正 形式，$K$ 矩阵先待定

状态估计：
$${\hat{\mathbf{X}}}_k={\hat{\mathbf{X}}}_{k/k-1}+{\mathbf{K}}_k\widetilde{{\mathbf{Z}}_k}={\hat{\mathbf{X}}}_{k/k-1}+{\mathbf{K}}_k\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k/k-1}\right)$$
状态估计误差：
$$\begin{array}{rl}{\tilde{\mathbf{X}}}_k& ={\mathbf{X}}_k-{\hat{\mathbf{X}}}_k={\mathbf{X}}_k-\left[{\hat{\mathbf{X}}}_{k/k-1}+{\mathbf{K}}_k\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k/k-1}\right)\right]\\ & ={\tilde{\mathbf{X}}}_{k/k-1}-{\mathbf{K}}_k\left({\mathbf{H}}_k{\mathbf{X}}_k+{\mathbf{V}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k/k-1}\right)=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\tilde{\mathbf{X}}}_{k/k-1}-{\mathbf{K}}_k{\mathbf{V}}_k\end{array}$$
状态估计方差阵：
$$\begin{array}{rl}{\mathbf{P}}_k& =\mathrm{E}\left[{\tilde{\mathbf{X}}}_k{\tilde{\mathbf{X}}}_k^{\mathrm{T}}\right]\\ & =\mathrm{E}\left\{\left[\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\tilde{\mathbf{X}}}_{k/k-1}-{\mathbf{K}}_k{\mathbf{V}}_k\right]{\left[\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\tilde{\mathbf{X}}}_{k/k-1}-{\mathbf{K}}_k{\mathbf{V}}_k\right]}^{\mathrm{T}}\right\}\\ & =\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right)\mathrm{E}\left[{\tilde{\mathbf{X}}}_{k/k-1}{\tilde{\mathbf{X}}}_{k/k-1}^{\mathrm{T}}\right]{\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right)}^{\mathrm{T}}+{\mathbf{K}}_k\mathrm{E}\left[{\mathbf{V}}_k{\mathbf{V}}_k^{\mathrm{T}}\right]{\mathbf{K}}_k^{\mathrm{T}}\\ & =\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k/k-1}{\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right)}^{\mathrm{T}}+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\mathrm{T}}\end{array}$$
到此，都是假设，在待定 $K$ 的情况下进行状态预测，并算出方差阵，下面就是要求$K$ ：

$K$ 矩阵可以取某个值，使状态估计方差 ${\mathbf{P}}_k$ 达到最小，且保持 ${\mathbf{P}}_k$ 对称正定。

要理解矩阵最小的含义，得先理解 $A$ 矩阵小于 $B$ 矩阵的含义：$B-A$ 正定

由最优估计的含义可知，等价于求 ${\mathbf{P}}_k$ 的迹，最小，先展开：
$$\begin{array}{rl}{\mathbf{P}}_k& =\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k/k-1}{\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right)}^{\mathrm{T}}+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\mathrm{T}}\\ & ={\mathbf{P}}_{k/k-1}-{\mathbf{K}}_k{\mathbf{H}}_k{\mathbf{P}}_{k/k-1}-{\left({\mathbf{K}}_k{\mathbf{H}}_k{\mathbf{P}}_{k/k-1}\right)}^{\mathrm{T}}+{\mathbf{K}}_k\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right){\mathbf{K}}_k^{\mathrm{T}}\end{array}$$
再求迹，分解开，就是一个单输入多输出的多元函数：
$$\begin{array}{rl}\mathrm{tr}\left({\mathbf{P}}_k\right)=& \mathrm{tr}\left({\mathbf{P}}_{k/k-1}\right)-\mathrm{tr}\left({\mathbf{K}}_k{\mathbf{H}}_k{\mathbf{P}}_{k/k-1}\right)\\ & -\mathrm{tr}\left({\left({\mathbf{K}}_k{\mathbf{H}}_k{\mathbf{P}}_{k/k-1}\right)}^{\mathrm{T}}\right)+\mathrm{tr}\left({\mathbf{K}}_k\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right){\mathbf{K}}_k^{\mathrm{T}}\right)\end{array}$$
要求多元函数的极值，就是找求导等于 $0$ 的点，由求导规则：
$$\begin{array}{l}\frac{\mathrm{d}}{\mathrm{d}\mathbf{X}}[\mathrm{tr}(\mathbf{X}\mathbf{B})]=\frac{\mathrm{d}}{\mathrm{d}\mathbf{X}}\left[\mathrm{tr}\left((\mathbf{X}\mathbf{B}{)}^{\mathrm{T}}\right)\right]={\mathbf{B}}^{\mathrm{T}}\\ \frac{\mathrm{d}}{\mathrm{d}\mathbf{X}}\left[\mathrm{tr}\left(\mathbf{X}\mathbf{A}{\mathbf{X}}^{\mathrm{T}}\right)\right]=2\mathbf{X}\mathbf{A}，A为对称阵时\end{array}$$
求导得：
$$\frac{\mathrm{d}}{\mathrm{d}{\mathbf{K}}_k}\left[\mathrm{tr}\left({\mathbf{P}}_k\right)\right]=0-{\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}\right)}^{\mathrm{T}}-{\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}\right)}^{\mathrm{T}}+2{\mathbf{K}}_k\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)=0$$
绿色的部分为移到左边，两边除以 2 得：
$${\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}={\mathbf{K}}_k\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)$$

要求出 $K$ ，需要 $K$ 后面部分可逆，即量测噪声 ${\mathbf{R}}_k$ 要正定；前面的 ${\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k$ 由于 ${\mathbf{H}}_k$ 有很多 $0$ 元素，可逆会不正定，如果加上一个总是正定 ${\mathbf{R}}_k$，还能维持整体的正定，继续求逆。

$K$ 后面部分可逆乘到两边：
$${\mathbf{K}}_k={\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)}^{-1}$$
将求出的增益矩阵 $K$ 带回到最上面状态估计和方差的的方程中，得出Kalman滤波解。

消除一部分元素，可以得到短一些的方差：${\mathbf{P}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k/k-1}$

5、Kalman滤波公式汇总

1. 状态一步预测：${\hat{\mathbf{X}}}_{k\mid k-1}={\mathbf{\Phi }}_{k\mid k-1}{\hat{\mathbf{X}}}_{k-1}$

2. 状态一步预测均方误差：${\mathbf{P}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}$

3. 滤波增益：${\mathbf{K}}_k={\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)}^{-1}$

4. 状态估计：${\hat{\mathbf{X}}}_k={\hat{\mathbf{X}}}_{k/k-1}+{\mathbf{K}}_k\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k/k-1}\right)$

5. 状态估计均方误差：${\mathbf{P}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k/k-1}$

6、Kalman滤波流程图

1.划分为左右两部分（一阶矩和二阶矩）

• 左边一部分滤波计算回路是一阶矩的更新过程，
• 右边增益计算回路是二阶矩的更新
• 左边对右边没有影响，右边通过增益矩阵对左边产生影响。有时候数组不好左边计算都崩溃了，对右边都没有影响，还按照模型来计算

2.划分为上下两部分（时间更新、量测更新）

• 上面是时间更新，之和状态方程有关系。
• 下面是量测更新，只和量测方程有关系。

7、Kalman滤波计算系统回路图

• 总的来说，是一个线性系统，输入 $Z_k$，输出 ${\hat{X}}_k$
• 右边部分是时间更新，以系统上一时刻的输出为输入，输出当前时刻的预测状态
• 左边部分是量测更新
• Kalman滤波对线性系统来说，是最小方差线性无偏估计

8、Kalman编程计算流程图

• 如果不是定常系统，${\mathbf{\Phi }}_{k/k-1},{\mathbf{\Gamma }}_{k-1}$,${\mathbf{Q}}_{k-1}$,${\mathbf{H}}_k,{\mathbf{R}}_k$ 都是时变的，需要给出；定常系统作为常数就行。
• 如果系统非线性，是高阶系统，必须给出初始状态 ${\hat{\mathbf{X}}}_0,{\mathbf{P}}_0$
• 输出一阶矩、二阶矩
  • 一阶矩、二阶矩能代表系统的全部，随机信号需要时正态分布的
  • 二阶矩是为了下一时刻计算权重（增益矩阵 $K$）
• 量测更新和时间更新可能不同步；常见的是量测更新频率小于时间更新频率。

9、Kalman滤波特性

1.状态估计时量测的线性组合

Kalman滤波是一种递推的公式，，上一时刻可以递推当前时刻，不断递推。以预测和量测加权的方式表示：
$$\begin{array}{rl}{\hat{\mathbf{X}}}_k& ={\hat{\mathbf{X}}}_{k/k-1}+{\mathbf{K}}_k\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k/k-1}\right)\\ & =\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{\Phi }}_{k/k-1}{\hat{\mathbf{X}}}_{k-1}+{\mathbf{K}}_k{\mathbf{Z}}_k={\mathbf{G}}_k{\hat{\mathbf{X}}}_{k-1}+{\mathbf{K}}_k{\mathbf{Z}}_k\\ & ={\mathbf{G}}_k\left({\mathbf{G}}_{k-1}{\hat{\mathbf{X}}}_{k-2}+{\mathbf{K}}_{k-1}{\mathbf{Z}}_{k-1}\right)+{\mathbf{K}}_k{\mathbf{Z}}_k\\ & ={\mathbf{G}}_k{\mathbf{G}}_{k-1}{\hat{\mathbf{X}}}_{k-2}+{\mathbf{G}}_k{\mathbf{K}}_{k-1}{\mathbf{Z}}_{k-1}+{\mathbf{K}}_k{\mathbf{Z}}_k\\ & ={\mathbf{G}}_k{\mathbf{G}}_{k-1}{\mathbf{G}}_{k-2}{\hat{\mathbf{X}}}_{k-3}+{\mathbf{G}}_k{\mathbf{G}}_{k-1}{\mathbf{K}}_{k-2}{\mathbf{Z}}_{k-2}+{\mathbf{G}}_k{\mathbf{K}}_{k-1}{\mathbf{Z}}_{k-1}+{\mathbf{K}}_k{\mathbf{Z}}_k\\ & =\cdots \\ & =\left(\prod _{i=1}^k{\mathbf{G}}_i\right){\hat{\mathbf{X}}}_0+\sum _{i=1}^k\left(\prod _{j=i+1}^k{\mathbf{G}}_j\right){\mathbf{K}}_i{\mathbf{Z}}_i\end{array}$$
如果初值 ${\hat{\mathbf{X}}}_0$ 取值 $0$ ，当前的估计值就是前面所有量测值 ${\mathbf{Z}}_{\mathbf{i}}$ 的线性组合

2.正交投影性质

• Kalman滤波估计就是预测和量测的加权平均。
• 量测是一条矢量、预测也是一条矢量，前面乘上加权系数再相加。
• 无论前面系数是多少，相加得到的估计值都在同一个平面上。
• 真实的状态也是空间中一条矢量。
• 要使估计值和真值直接的误差最小，即在平面上取到平面外一点距离最小的点，就是取正交投影点。

3.新息向量是零矩阵白噪声序列

$$\{\begin{array}{l}\mathrm{E}\left[{\tilde{\mathbf{Z}}}_{k/k-1}\right]=0\\ \mathrm{E}\left[{\tilde{\mathbf{Z}}}_{k/k-1}{\tilde{\mathbf{Z}}}_{j/j-1}^{\mathrm{T}}\right]=\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right){\delta }_{kj}\end{array}$$

10、带确定性输入的Kalman滤波

状态空间模型：
$$\{\begin{array}{l}{\mathbf{X}}_k={\mathbf{\Phi }}_{k/k-1}{\mathbf{X}}_{k-1}+{\mathbf{B}}_{k-1}{\mathbf{u}}_{k-1}+{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}\\ {\mathbf{Z}}_k={\mathbf{H}}_k{\mathbf{X}}_k+{\mathbf{Y}}_k+{\mathbf{V}}_k\end{array}$$
滤波方程，只有和一阶矩有关的部分需要改动：
$$\{\begin{array}{l}{\hat{\mathbf{X}}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}{\hat{\mathbf{X}}}_{k-1}+{\mathbf{B}}_{k-1}{\mathbf{u}}_{k-1}\\ {\mathbf{P}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}\\ {\mathbf{K}}_k={\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)}^{-1}\\ {\hat{\mathbf{X}}}_k={\hat{\mathbf{X}}}_{k/k-1}+{\mathbf{K}}_k\left({\mathbf{Z}}_k-{\mathbf{Y}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k/k-1}\right)\\ {\mathbf{P}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k/k-1}\end{array}$$