卡尔曼滤波与组合导航原理笔记（一）卡尔曼滤波方程的推导 第一部分

文章目录

• • 一、滤波的基本概念
  • • 1、传统数字滤波器
    • 2、现代控制中的状态观测器
    • 3、最优估计的含义
    • 4、温度估计的例子
  • 二、递推最小二乘

一、滤波的基本概念

1、传统数字滤波器

主要是基于频带来设计，单输入单输出系统。认为数字滤波器处理的是确定性信号，有用的信号在确定的频带内，要去除的噪声在频带之外；噪声在频带内，就没法处理

两种方法：①模拟转数字②直接数字滤波器

MATLAB工具箱fdatool，可以很方便的设置数字滤波器：

2、现代控制中的状态观测器

• 如果参数都知道，上下两部分就只差了一个观测器矩阵E，类似与Kalman滤波中的K矩阵
• 现代控制中处理的是确定性信号，可以用函数来描述，而估计器处理的是带误差的信号

3、最优估计的含义

每一个分量的二阶矩都达到最小：
E [ ( X ~ k ( 1 ) ) 2 ] + E [ ( X ~ k ( 2 ) ) 2 ] + ⋯ + E [ ( X ~ k ( n ) ) 2 ] = min ⁡ \mathrm{E}\left[\left(\tilde{X}{k}{(1)}\right){2}\right]+\mathrm{E}\left[\left(\tilde{X}{k}{(2)}\right){2}\right]+\cdots+\mathrm{E}\left[\left(\tilde{X}_{k}{(n)}\right){2}\right]=\min E[(X~k(1))2]+E[(X~k(2))2]+⋯+E[(X~k​(n))2]=min
即： E [ X ~ k T X ~ k ] = min ⁡ \mathrm{E}\left[\tilde{\boldsymbol{X}}_{k}^{\mathrm{T}} \tilde{\boldsymbol{X}}_{k}\right]=\min E[X~kT​X~k​]=min：
E [ X ~ k X ~ k T ] = [ E [ ( X k ( 1 ) ) 2 ] E [ X k ( 1 ) X k ( 2 ) ] ⋯ E [ X k ( 1 ) X k ( n ) ] E [ X ~ k ( 2 ) X ~ k ( 1 ) ] E [ ( X ~ k ( 2 ) ) 2 ] ⋯ E [ X ~ k ( 2 ) X ~ k ( n ) ] ⋮ ⋮ ⋱ ⋮ E [ X ~ k ( n ) X ~ k ( 1 ) ] E [ X ~ k ( n ) X ~ k ( 2 ) ] ⋯ E [ ( X ~ k ( n ) ) 2 ] ] \mathrm{E}\left[\tilde{\boldsymbol{X}}_{k} \tilde{\boldsymbol{X}}_{k}^{\mathrm{T}}\right]=\left[\begin{array}{cccc}\mathrm{E}\left[\left(X_{k}^{(1)}\right)^{2}\right] & \mathrm{E}\left[X_{k}^{(1)} X_{k}^{(2)}\right] & \cdots & \mathrm{E}\left[X_{k}^{(1)} X_{k}^{(n)}\right] \\ \mathrm{E}\left[\tilde{X}_{k}^{(2)} \tilde{X}_{k}^{(1)}\right] & \mathrm{E}\left[\left(\tilde{X}_{k}^{(2)}\right)^{2}\right] & \cdots & \mathrm{E}\left[\tilde{X}_{k}^{(2)} \tilde{X}_{k}^{(n)}\right] \\ \vdots & \vdots & \ddots & \vdots \\ \mathrm{E}\left[\tilde{X}_{k}^{(n)} \tilde{X}_{k}^{(1)}\right] & \mathrm{E}\left[\tilde{X}_{k}^{(n)} \tilde{X}_{k}^{(2)}\right] & \cdots & \mathrm{E}\left[\left(\tilde{X}_{k}^{(n)}\right)^{2}\right]\end{array}\right] E[X~k​X~kT​]= ​E[(Xk(1)​)2]E[X~k(2)​X~k(1)​]⋮E[X~k(n)​X~k(1)​]​E[Xk(1)​Xk(2)​]E[(X~k(2)​)2]⋮E[X~k(n)​X~k(2)​]​⋯⋯⋱⋯​E[Xk(1)​Xk(n)​]E[X~k(2)​X~k(n)​]⋮E[(X~k(n)​)2]​ ​
只需要关注对角线上的元素，即求迹：
tr ⁡ ( P k ) = tr ⁡ ( E [ X ~ k X ~ k T ] ) = min ⁡ \operatorname{tr}\left(\boldsymbol{P}_{k}\right)=\operatorname{tr}\left(\mathrm{E}\left[\tilde{\boldsymbol{X}}_{k} \tilde{\boldsymbol{X}}_{k}^{\mathrm{T}}\right]\right)=\min tr(Pk​)=tr(E[X~k​X~kT​])=min

4、温度估计的例子

1.问题描述

• 某房间内温度受随机干扰影响——不恒定、波动
• 每小时用温度计测量一次温度——离散观测点
• 试对该房间温度作最佳估计——建模
• 干扰： W ∼ N ( 0 , 0. 4 ∧ 2 ) W \sim N \left(0,0 .{4^{\wedge} 2}\right) W∼N(0,0.4∧2)——实际参数波动
• 温度计误差 ： V ∼ N ( 0 , 0. 3 ∧ 2 ) V \sim N\left(0,0.3^{\wedge} 2\right) V∼N(0,0.3∧2)——观测值噪声

2.分析

• 假设知道上一时刻温度 25℃，可以知道这一小时的温度受到干扰影响；根据高斯分布，67%概率在干扰的范围内 25±0.4℃，

• 如果温度计读数是 25.2℃，根据高斯分布，67%概率温度在 25.2±0.3℃

• 现在有两方面的信息，一个是惯性保持下来的 25±0.4℃，一个是量测得到的 25.2±0.3℃。如何得出此房间的温度？

• 按照最朴素的想法：两个值加权平均。0.4 代表误差大一些，0.3 代表误差小一些，量测信息更准确，占的权重更大。

• 假设两个值不相关，按概率论方法，可得到：

  可以看出，一个值的权重取决于另一个值的误差，另一个值误差很小，这个权重就小

• 再往下一时刻，算法也同样

• 从噪声（方差）的角度看，这其实很类似电路，噪声往里加就像串联电路，量测信息和预测信息的结合就像是并联电路

• 如果干扰 W=0，则上一时刻温度是多少，下一时刻温度还是多少；房间是常温，温度计量测带误差，就相当于对常量进行估计，用递推最小二乘法。

• 如果量测噪声 V=0，温度计很准，量出来多少就是多少，变成确定性系统，不用估计了。

二、递推最小二乘

最小二乘量测模型：
Z k = H k X + V k E [ V k ] = 0 E [ V k V j T ] = R k δ k j \boldsymbol{Z}_{k}=\boldsymbol{H}_{k} \boldsymbol{X}+\boldsymbol{V}_{k} \quad \mathrm{E}\left[\boldsymbol{V}_{k}\right]=\mathbf{0} \quad \mathrm{E}\left[\boldsymbol{V}_{k} \boldsymbol{V}_{j}^{\mathrm{T}}\right]=\boldsymbol{R}_{k} \delta_{k j} Zk​=Hk​X+Vk​E[Vk​]=0E[Vk​VjT​]=Rk​δkj​

每一个观测时刻 K K K ，都有一组量测方程 Z k = H k X + V k \boldsymbol{Z}_{k}=\boldsymbol{H}_{k} \boldsymbol{X}+\boldsymbol{V}_{k} Zk​=Hk​X+Vk​ 。一般认为设计矩阵 H k \boldsymbol{H}_{k} Hk​ 的列数 n 是确定的（代表参数个数不变），行数 m m m 是不确定的（代表观测值个数可变）。可以把很多观测时刻的数据都列拼接在一起：
Z ‾ i = [ Z 1 Z 2 ⋮ Z i ] , H ‾ i = [ H 1 H 2 ⋮ H i ] , V ‾ i = [ V 1 V 2 ⋮ V i ] \overline{\boldsymbol{Z}}_{i}=\left[\begin{array}{c}\boldsymbol{Z}_{1} \\ \boldsymbol{Z}_{2} \\ \vdots \\ \boldsymbol{Z}_{i}\end{array}\right], \quad \overline{\boldsymbol{H}}_{i}=\left[\begin{array}{c}\boldsymbol{H}_{1} \\ \boldsymbol{H}_{2} \\ \vdots \\ \boldsymbol{H}_{i}\end{array}\right], \quad \overline{\boldsymbol{V}}_{i}=\left[\begin{array}{c}\boldsymbol{V}_{1} \\ \boldsymbol{V}_{2} \\ \vdots \\ \boldsymbol{V}_{i}\end{array}\right] Zi​= ​Z1​Z2​⋮Zi​​ ​,Hi​= ​H1​H2​⋮Hi​​ ​,Vi​= ​V1​V2​⋮Vi​​ ​
当 i = k − 1 i=k-1 i=k−1 时刻，对此方程做加权最小二乘估计：
X ^ k − 1 = ( H ‾ k − 1 T R ‾ k − 1 − 1 H ‾ k − 1 ) − 1 H ‾ k − 1 T R ‾ k − 1 − 1 Z ‾ k − 1 = P k − 1 H ‾ k − 1 T R ‾ k − 1 − 1 Z ‾ k − 1 \hat{\boldsymbol{X}}_{k-1} =\left(\overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{H}}_{k-1}\right)^{-1} \overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{Z}}_{k-1} =\boldsymbol{P}_{k-1} \overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{Z}}_{k-1} X^k−1​=(Hk−1T​Rk−1−1​Hk−1​)−1Hk−1T​Rk−1−1​Zk−1​=Pk−1​Hk−1T​Rk−1−1​Zk−1​

上式利用到了前面所有时刻的观测值。同理当 i = k i=k i=k 时的最小二乘估计：
X ^ k = ( H ‾ k T R ‾ k − 1 H ‾ k ) − 1 H ‾ l T R ‾ k − 1 Z ‾ k = P k ( [ H ‾ k − 1 T H k T ] [ R ‾ k − 1 − 1 0 0 R k − 1 ] [ Z ‾ k − 1 $ Z k ] ) = P k ( H ‾ Σ − 1 T R ‾ k − 1 − 1 Z ‾ k − 1 + H k T R 1 − 1 Z k ) \begin{array}{l}\hat{\boldsymbol{X}}_{k}=\left(\overline{\boldsymbol{H}}_{k}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k}^{-1} \overline{\boldsymbol{H}}_{k}\right)^{-1} \overline{\boldsymbol{H}}_{l}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k}^{-1} \overline{\boldsymbol{Z}}_{k} \\ =\boldsymbol{P}_{k}\left(\left[\begin{array}{ll}\overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} & \boldsymbol{H}_{k}^{\mathrm{T}}\end{array}\right]\left[\begin{array}{cc}\overline{\boldsymbol{R}}_{k-1}^{-1} \mathbf{0} & \\ \mathbf{0} & \boldsymbol{R}_{k}^{-1}\end{array}\right]\left[\begin{array}{c}\overline{\boldsymbol{Z}}_{k-1} \\ \$ \boldsymbol{Z}_{k}\end{array}\right]\right) \\ =\boldsymbol{P}_{k}\left(\overline{\boldsymbol{H}}_{\mathrm{\Sigma}-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{Z}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{1}^{-1} \boldsymbol{Z}_{k}\right) \\\end{array} X^k​=(HkT​Rk−1​Hk​)−1HlT​Rk−1​Zk​=Pk​([Hk−1T​​HkT​​][Rk−1−1​00​Rk−1​​][Zk−1​$Zk​​])=Pk​(HΣ−1T​Rk−1−1​Zk−1​+HkT​R1−1​Zk​)​
递推目标函数：知道前一时刻的估计值 X ^ k − 1 \hat{\boldsymbol{X}}_{k-1} X^k−1​、 P k − 1 \boldsymbol{P}_{k-1} Pk−1​ 矩阵，和当前时刻测量得来的信息 H k , R k , Z k \boldsymbol{H}_{k}, \boldsymbol{R}_{k}, \boldsymbol{Z}_{k} Hk​,Rk​,Zk​ ，求得当前时刻的 H k , R k , Z k \boldsymbol{H}_{k}, \boldsymbol{R}_{k}, \boldsymbol{Z}_{k} Hk​,Rk​,Zk​
( H k , R k , Z k ) = f ( X ^ k − 1 , P k − 1 , H k , R k , Z k ) \left(\boldsymbol{H}_{k}, \boldsymbol{R}_{k}, \boldsymbol{Z}_{k}\right)=f\left(\hat{\boldsymbol{X}}_{k-1}, \boldsymbol{P}_{k-1}, \boldsymbol{H}_{k}, \boldsymbol{R}_{k}, \boldsymbol{Z}_{k}\right) (Hk​,Rk​,Zk​)=f(X^k−1​,Pk−1​,Hk​,Rk​,Zk​)
方差阵递推（协方差传播定律）：
P k = ( H ‾ k T R ‾ k − 1 H ‾ k ) − 1 = ( [ H ‾ k − 1 T H k T ] [ R ‾ k − 1 − 1 0 0 R k − 1 ] [ H ‾ k − 1 H k ] ) − 1 = ( H ‾ k − 1 T R ‾ k − 1 − 1 H ‾ k − 1 + H k T R k − 1 H k ) − 1 = ( P k − 1 − 1 + H k T R k − 1 H k ) − 1 \begin{aligned} \boldsymbol{P}_{k} & =\left(\overline{\boldsymbol{H}}_{k}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k}^{-1} \overline{\boldsymbol{H}}_{k}\right)^{-1} \\ & =\left(\left[\begin{array}{ll}\overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} & \boldsymbol{H}_{k}^{\mathrm{T}}\end{array}\right]\left[\begin{array}{cc}\overline{\boldsymbol{R}}_{k-1}^{-1} & \mathbf{0} \\ \mathbf{0} & \boldsymbol{R}_{k}^{-1}\end{array}\right]\left[\begin{array}{c}\overline{\boldsymbol{H}}_{k-1} \\ \boldsymbol{H}_{k}\end{array}\right]\right)^{-1} \\ & =\left(\overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{H}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1} \\ & =\left(\boldsymbol{P}_{k-1}^{-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1}\end{aligned} Pk​​=(HkT​Rk−1​Hk​)−1=([Hk−1T​​HkT​​][Rk−1−1​0​0Rk−1​​][Hk−1​Hk​​])−1=(Hk−1T​Rk−1−1​Hk−1​+HkT​Rk−1​Hk​)−1=(Pk−1−1​+HkT​Rk−1​Hk​)−1​
上式求逆太多，可以写为下面逆的形式：
P k − 1 = P k − 1 − 1 + H k T R k − 1 H k \boldsymbol{P}_{k}^{-1}=\boldsymbol{P}_{k-1}^{-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k} Pk−1​=Pk−1−1​+HkT​Rk−1​Hk​
状态估计递推：
X ^ k = P k ( H ‾ k − 1 T R ‾ k − 1 − 1 Z ‾ k − 1 + H k T R k − 1 Z k ) = P k ( P k − 1 − 1 P k − 1 H ‾ k − 1 T R ‾ k − 1 − 1 Z ‾ k − 1 + H k T R k − 1 Z k ) = P k ( P k − 1 − 1 X ^ k − 1 + H k T R k − 1 Z k ) = P k [ ( P k − 1 − H k T R k − 1 H k ) X ^ k − 1 + H k T R k − 1 Z k ] = X ^ k − 1 + P k H k T R k − 1 ( Z k − H k X ^ k − 1 ) \begin{aligned} \hat{\boldsymbol{X}}_{k} & =\boldsymbol{P}_{k}\left(\overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{Z}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}\right)=\boldsymbol{P}_{k}\left(\boldsymbol{P}_{k-1}^{-1} \boldsymbol{P}_{k-1} \overline{\boldsymbol{H}}_{k-1}^{\mathrm{T}} \overline{\boldsymbol{R}}_{k-1}^{-1} \overline{\boldsymbol{Z}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}\right) \\ & =\boldsymbol{P}_{k}\left(\boldsymbol{P}_{k-1}^{-1} \hat{\boldsymbol{X}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}\right)=\boldsymbol{P}_{k}\left[\left(\boldsymbol{P}_{k}^{-1}-\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right) \hat{\boldsymbol{X}}_{k-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{Z}_{k}\right] \\ & =\hat{\boldsymbol{X}}_{k-1}+\boldsymbol{P}_{k} \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right) \end{aligned} X^k​​=Pk​(Hk−1T​Rk−1−1​Zk−1​+HkT​Rk−1​Zk​)=Pk​(Pk−1−1​Pk−1​Hk−1T​Rk−1−1​Zk−1​+HkT​Rk−1​Zk​)=Pk​(Pk−1−1​X^k−1​+HkT​Rk−1​Zk​)=Pk​[(Pk−1​−HkT​Rk−1​Hk​)X^k−1​+HkT​Rk−1​Zk​]=X^k−1​+Pk​HkT​Rk−1​(Zk​−Hk​X^k−1​)​
上式已经是递推最小二乘了，但为了更接近Kalman滤波，还可以继续向下推导。由于求逆特别多，引入矩阵求逆引理：

回过头看递推最小二乘的公式：
{ P k = ( P k − 1 − 1 + H k T R k − 1 H k ) − 1 X ^ k = X ^ k − 1 + P k H k T R k − 1 ( Z k − H k X ^ k − 1 ) \left\{\begin{array}{l}\boldsymbol{P}_{k}=\left(\boldsymbol{P}_{k-1}^{-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1} \\ \hat{\boldsymbol{X}}_{k}=\hat{\boldsymbol{X}}_{k-1}+\boldsymbol{P}_{k} \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right)\end{array}\right. ⎩ ⎨ ⎧​Pk​=(Pk−1−1​+HkT​Rk−1​Hk​)−1X^k​=X^k−1​+Pk​HkT​Rk−1​(Zk​−Hk​X^k−1​)​
可以看出 P 矩阵的递推和矩阵求逆引理的第一个公式对应，令 A 11 = P k − 1 − 1 \boldsymbol{A}_{11}=\boldsymbol{P}_{k-1}^{-1} A11​=Pk−1−1​, A 12 = − H k T \quad \boldsymbol{A}_{12}=-\boldsymbol{H}_{k}^{\mathrm{T}} A12​=−HkT​, A 22 = R k \quad \boldsymbol{A}_{22}=\boldsymbol{R}_{k} A22​=Rk​, A 21 = H k \quad \boldsymbol{A}_{21}=\boldsymbol{H}_{k} A21​=Hk​ ，则有：
P k = ( P k − 1 − 1 + H k T R k − 1 H k ) − 1 = P k − 1 − P k − 1 H k T ( R k + H k P k − 1 H k T ) − 1 H k P k − 1 \boldsymbol{P}_{k}=\left(\boldsymbol{P}_{k-1}^{-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1}=\boldsymbol{P}_{k-1}-{\color{red}\boldsymbol{P}_{k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{R}_{k}+\boldsymbol{H}_{k} \boldsymbol{P}_{k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\right)^{-1}}\boldsymbol{H}_{k} \boldsymbol{P}_{k-1} Pk​=(Pk−1−1​+HkT​Rk−1​Hk​)−1=Pk−1​−Pk−1​HkT​(Rk​+Hk​Pk−1​HkT​)−1Hk​Pk−1​
从左式到右式，看起来更复杂了，但其实左边要求逆三次，右边只要求逆一次。再仔细看，标红的部分与矩阵求逆引理的第二个公式对应：
P k − 1 H k T ( R k + H k P k − 1 H k T ) − 1 = ( P k − 1 − 1 + H k T R k − 1 H k ) − 1 H k T R k − 1 = P k H k T R k − 1 \boldsymbol{P}_{k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{R}_{k}+\boldsymbol{H}_{k} \boldsymbol{P}_{k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\right)^{-1}={\left(\boldsymbol{P}_{k-1}^{-1}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k}\right)^{-1} \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1}}=\boldsymbol{P}_{k} \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} Pk−1​HkT​(Rk​+Hk​Pk−1​HkT​)−1=(Pk−1−1​+HkT​Rk−1​Hk​)−1HkT​Rk−1​=Pk​HkT​Rk−1​
带入变化之后式子就可以变得很简单，而且发现最后的结果与状态更新中新息向量 ( Z k − H k X ^ k − 1 ) \left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right) (Zk​−Hk​X^k−1​) 前的系数一致，把红色的部分记为 K K K ，得最终公式：
{ K k = P k − 1 H k T ( H k P k − 1 H k T + R k ) − 1 X ^ k = X ^ k − 1 + K k ( Z k − H k X ^ k − 1 ) P k = ( I − K k H k ) P k − 1 \left\{\begin{array}{l}\boldsymbol{K}_{k}=\boldsymbol{P}_{k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)^{-1} \\ \hat{\boldsymbol{X}}_{k}=\hat{\boldsymbol{X}}_{k-1}+\boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right) \\ \boldsymbol{P}_{k}=\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right) \boldsymbol{P}_{k-1}\end{array}\right. ⎩ ⎨ ⎧​Kk​=Pk−1​HkT​(Hk​Pk−1​HkT​+Rk​)−1X^k​=X^k−1​+Kk​(Zk​−Hk​X^k−1​)Pk​=(I−Kk​Hk​)Pk−1​​
状态的更新是上一时刻的状态 X ^ k − 1 \hat{\boldsymbol{X}}_{k-1} X^k−1​ 加上基于当前时刻量测进行的修正 K k ( Z k − H k X ^ k − 1 ) \boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right) Kk​(Zk​−Hk​X^k−1​) 。修正量是量测值与上一时刻的差值 ( Z k − H k X ^ k − 1 ) \left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k-1}\right) (Zk​−Hk​X^k−1​) 乘以增益系数 K k \boldsymbol{K}_{k} Kk​。