卡尔曼滤波公式及其详细推导（不涉及矩阵求导）

卡尔曼滤波公式及推导

1 前言

卡尔曼滤波 (Kalman Filter) 是一种关于线性离散系统滤波问题的递推算法。其使用递推的形式对系统的状态进行估计，以测量中产生的误差为依据对估计值进行校正，使被估计的状态不断接近真实值。

卡尔曼滤波的基本思想：根据系统的状态空间方程，利用前一时刻系统状态的估计值和当前时刻系统的观测值对状态变量进行最优估计，求出当前时刻系统状态的估计值。

假设线性离散系统的状态空间方程如下：

{ X ( k + 1 ) = A X ( k ) + v ( k ) Y ( k ) = H X ( k ) + w ( k ) (1.1) \left\{ \begin{aligned} X(k+1) &= AX(k) + v(k) \\ Y(k) &= HX(k) + w(k) \tag{1.1} \end{aligned} \right. {X(k+1)Y(k)​=AX(k)+v(k)=HX(k)+w(k)​(1.1)

式中， A A A 为状态矩阵， H H H 为输出矩阵。 X ( k ) X(k) X(k) 为 k k k 时刻的系统状态， Y ( k ) Y(k) Y(k) 为 k k k 时刻的系统输出，即系统的观测值。 v ( k ) v(k) v(k) 是过程噪声，服从均值为 0 0 0 ，方差为 Q k Q_k Qk​ 的高斯分布； w ( k ) w(k) w(k) 是传感器测量噪声，服从均值为 0 0 0 ，方差为 R k R_k Rk​ 的高斯分布。 v ( k ) v(k) v(k) 和 w ( k ) w(k) w(k) 相互独立。 Q k Q_k Qk​ 和 R k R_k Rk​ 都是协方差矩阵、对称半正定矩阵。 Q k = E { v ( k ) v T ( k ) } Q_k = E\left\{v(k)v^T(k)\right\} Qk​=E{v(k)vT(k)} ， R k = E { w ( k ) w T ( k ) } R_k = E\left\{w(k)w^T(k)\right\} Rk​=E{w(k)wT(k)} 。

2 卡尔曼滤波公式

2.1 预测

根据 k k k 时刻的最优状态估计预测 k + 1 k+1 k+1 时刻的状态和误差的协方差矩阵。

X ( k + 1 ∣ k ) = A X ( k ∣ k ) (2.1) X(k+1|k) = AX(k|k) \tag{2.1} X(k+1∣k)=AX(k∣k)(2.1)
P ( k + 1 ∣ k ) = A P ( k ∣ k ) A T + Q k (2.2) P(k+1|k) = AP(k|k)A^T + Q_k \tag{2.2} P(k+1∣k)=AP(k∣k)AT+Qk​(2.2)

式 ( 2.1 ) (2.1) (2.1) 是状态预测方程。其中， X ( k ∣ k ) X(k|k) X(k∣k) 是 k k k 时刻系统状态的最优估计， X ( k + 1 ∣ k ) X(k+1|k) X(k+1∣k) 是根据 k k k 时刻的系统状态最优估计 X ( k ∣ k ) X(k|k) X(k∣k) 对 k + 1 k+1 k+1 时刻系统状态的预测。

式 ( 2.2 ) (2.2) (2.2) 是误差的协方差矩阵预测方程。其中， P ( k ∣ k ) P(k|k) P(k∣k) 是 k k k 时刻系统状态的最优估计 X ( k ∣ k ) X(k|k) X(k∣k) 对应的误差协方差矩阵， P ( k + 1 ∣ k ) P(k+1|k) P(k+1∣k) 是系统状态的预测 X ( k + 1 ∣ k ) X(k+1|k) X(k+1∣k) 对应的误差协方差矩阵。 Q k Q_k Qk​ 是 k k k 时刻系统过程噪声的协方差矩阵。

2.2 校正

根据系统 k + 1 k+1 k+1 时刻实际的观测值（系统输出）对上一步得到的 k + 1 k+1 k+1 时刻系统状态的预测进行校正，得到在 k + 1 k+1 k+1 时刻系统状态的最优估计。

X ( k + 1 ∣ k + 1 ) = X ( k + 1 ∣ k ) + K ( k + 1 ) [ Y ( k + 1 ) − H X ( k + 1 ∣ k ) ] (2.3) X(k+1|k+1) = X(k+1|k) + K(k+1) \left[Y(k+1) - HX(k+1|k)\right] \tag{2.3} X(k+1∣k+1)=X(k+1∣k)+K(k+1)[Y(k+1)−HX(k+1∣k)](2.3)

其中， X ( k + 1 ∣ k + 1 ) X(k+1|k+1) X(k+1∣k+1) 是 k + 1 k+1 k+1 时刻系统状态的最优估计； K ( k + 1 ) K(k+1) K(k+1) 是卡尔曼增益矩阵，其计算方法如下：

K ( k + 1 ) = P ( k + 1 ∣ k ) H T [ H P ( k + 1 ∣ k ) H T + R k + 1 ] − 1 (2.4) K(k+1) = P(k+1|k)H^T\left[HP(k+1|k)H^T + R_{k+1}\right]^{-1} \tag{2.4} K(k+1)=P(k+1∣k)HT[HP(k+1∣k)HT+Rk+1​]−1(2.4)

其中， R k + 1 R_{k+1} Rk+1​ 是 k + 1 k+1 k+1 时刻测量噪声的协方差矩阵。

为了能使递推算法继续进行，还需要更新 k + 1 k+1 k+1 时刻的误差协方差矩阵：

P ( k + 1 ∣ k + 1 ) = [ I − K ( k + 1 ) H ] P ( k + 1 ∣ k ) (2.5) P(k+1|k+1) = \left[I - K(k+1)H\right]P(k+1|k) \tag{2.5} P(k+1∣k+1)=[I−K(k+1)H]P(k+1∣k)(2.5)

其中， I I I 为单位矩阵， P ( k + 1 ∣ k + 1 ) P(k+1|k+1) P(k+1∣k+1) 是 k + 1 k+1 k+1 时刻系统状态的最优估计 X ( k + 1 ∣ k + 1 ) X(k+1|k+1) X(k+1∣k+1) 对应的误差协方差矩阵。

以上五个公式在推导过程中的对应关系：

• ( 2.1 ) → ( 3.1 ) (2.1) \rightarrow (3.1) (2.1)→(3.1)
• ( 2.2 ) → ( 3.14 ) (2.2) \rightarrow (3.14) (2.2)→(3.14)
• ( 2.3 ) → ( 3.4 ) (2.3) \rightarrow (3.4) (2.3)→(3.4)
• ( 2.4 ) → ( 3.19 ) (2.4) \rightarrow (3.19) (2.4)→(3.19)
• ( 2.5 ) → ( 3.20 ) (2.5) \rightarrow (3.20) (2.5)→(3.20)

3 推导过程

假设在系统运行的过程中，已经得到了 k k k 时刻系统状态的最优估计 X ( k ∣ k ) X(k|k) X(k∣k) 。那么如何得到 k + 1 k+1 k+1 时刻系统状态的最优估计呢？

根据系统的状态空间方程，式 ( 1.1 ) (1.1) (1.1) ，我们可以使用当前状态的最优估计 X ( k ∣ k ) X(k|k) X(k∣k) 预测下一时刻的系统状态 X ( k + 1 ∣ k ) X(k+1|k) X(k+1∣k) ，即：

X ( k + 1 ∣ k ) = A X ( k ∣ k ) (3.1) X(k+1|k) = AX(k|k) \tag{3.1} X(k+1∣k)=AX(k∣k)(3.1)

同时，也可以预测下一时刻的测量值，即系统输出：

Y ( k + 1 ∣ k ) = H X ( k + 1 ∣ k ) (3.2) Y(k+1|k) = HX(k+1|k) \tag{3.2} Y(k+1∣k)=HX(k+1∣k)(3.2)

当下一时刻到来时，我们可以得到实际的测量值 Y ( k + 1 ) Y(k+1) Y(k+1) 。显然，实际的测量值与预测的测量值之间存在误差：

Y ( k + 1 ) − Y ( k + 1 ∣ k ) = Y ( k + 1 ) − H X ( k + 1 ∣ k ) (3.3) Y(k+1) - Y(k+1|k) = Y(k+1) - HX(k+1|k) \tag{3.3} Y(k+1)−Y(k+1∣k)=Y(k+1)−HX(k+1∣k)(3.3)

根据 1 前言 中卡尔曼滤波的基本思想，我们要用式 ( 3.3 ) (3.3) (3.3) 表示的误差对系统状态的预测 X ( k + 1 ∣ k ) X(k+1|k) X(k+1∣k) 进行校正，从而得到系统状态的最优估计 X ( k + 1 ∣ k + 1 ) X(k+1|k+1) X(k+1∣k+1) ，即：

X ( k + 1 ∣ k + 1 ) = X ( k + 1 ∣ k ) + K ( k + 1 ) [ Y ( k + 1 ) − Y ( k + 1 ∣ k ) ] = X ( k + 1 ∣ k ) + K ( k + 1 ) [ Y ( k + 1 ) − H X ( k + 1 ∣ k ) ] (3.4) \begin{aligned} X(k+1|k+1) &= X(k+1|k) + K(k+1)\left[Y(k+1) - Y(k+1|k)\right] \\ &= X(k+1|k) + K(k+1)\left[Y(k+1) - HX(k+1|k)\right] \tag{3.4} \end{aligned} X(k+1∣k+1)​=X(k+1∣k)+K(k+1)[Y(k+1)−Y(k+1∣k)]=X(k+1∣k)+K(k+1)[Y(k+1)−HX(k+1∣k)]​(3.4)

其中， K ( k + 1 ) K(k+1) K(k+1) 是待求的卡尔曼增益矩阵。

K ( k + 1 ) K(k+1) K(k+1) 的取值应该满足最优估计误差的协方差矩阵 P ( k + 1 ∣ k + 1 ) P(k+1|k+1) P(k+1∣k+1) 最小。

下面我们来求卡尔曼增益矩阵 K ( k + 1 ) K(k+1) K(k+1) 。

设 k + 1 k+1 k+1 时刻的系统状态真实值 X ( k + 1 ) X(k+1) X(k+1) 与系统状态的最优估计 X ( k + 1 ∣ k + 1 ) X(k+1|k+1) X(k+1∣k+1) 之间的误差为 e ( k + 1 ∣ k + 1 ) e(k+1|k+1) e(k+1∣k+1) ；
设 k + 1 k+1 k+1 时刻的系统状态真实值 X ( k + 1 ) X(k+1) X(k+1) 与系统状态的预测值 X ( k + 1 ∣ k ) X(k+1|k) X(k+1∣k) 之间的误差为 e ( k + 1 ∣ k ) e(k+1|k) e(k+1∣k) 。

有：

e ( k + 1 ∣ k + 1 ) = X ( k + 1 ) − X ( k + 1 ∣ k + 1 ) (3.5) e(k+1|k+1) = X(k+1) - X(k+1|k+1) \tag{3.5} e(k+1∣k+1)=X(k+1)−X(k+1∣k+1)(3.5)

e ( k + 1 ∣ k ) = X ( k + 1 ) − X ( k + 1 ∣ k ) (3.6) e(k+1|k) = X(k+1) - X(k+1|k) \tag{3.6} e(k+1∣k)=X(k+1)−X(k+1∣k)(3.6)

将状态空间方程 ( 1.1 ) (1.1) (1.1) 和式 ( 3.4 ) (3.4) (3.4)代入式 ( 3.5 ) (3.5) (3.5) ，得：

e ( k + 1 ∣ k + 1 ) = X ( k + 1 ) − X ( k + 1 ∣ k + 1 ) = A X ( k ) + v ( k ) − X ( k + 1 ∣ k ) − K ( k + 1 ) [ Y ( k + 1 ) − H X ( k + 1 ∣ k ) ] = A X ( k ) + v ( k ) − A X ( k ∣ k ) − K ( k + 1 ) [ H A X ( k ) + H v ( k ) + w ( k + 1 ) − H A X ( k ∣ k ) ] = A [ X ( k ) − X ( k ∣ k ) ] − K ( k + 1 ) H A [ X ( k ) − X ( k ∣ k ) ] + v ( k ) − K ( k + 1 ) H v ( k ) − K ( k + 1 ) w ( k + 1 ) = [ I − K ( k + 1 ) H ] A [ X ( k ) − X ( k ∣ k ) ] + [ I − K ( k + 1 ) H ] v ( k ) − K ( k + 1 ) w ( k + 1 ) = [ I − K ( k + 1 ) H ] A e ( k ∣ k ) + [ I − K ( k + 1 ) H ] v ( k ) − K ( k + 1 ) w ( k + 1 ) (3.7) \begin{aligned} e(k+1|k+1) =& X(k+1) - X(k+1|k+1) \\ =& AX(k) + v(k) - X(k+1|k) - K(k+1)\left[Y(k+1) - HX(k+1|k)\right] \\ =& AX(k) + v(k) - AX(k|k) \\ &- K(k+1)\left[HAX(k) + Hv(k) + w(k+1) - HAX(k|k)\right] \\ =& A\left[X(k) - X(k|k)\right] - K(k+1)HA\left[X(k) - X(k|k)\right] + v(k) \\ &- K(k+1)Hv(k) - K(k+1)w(k+1) \\ =& \left[I - K(k+1)H\right]A\left[X(k) - X(k|k)\right] + \left[I - K(k+1)H\right]v(k) - K(k+1)w(k+1) \\ =& \left[I - K(k+1)H\right]Ae(k|k) + \left[I - K(k+1)H\right]v(k) - K(k+1)w(k+1) \tag{3.7} \end{aligned} e(k+1∣k+1)======​X(k+1)−X(k+1∣k+1)AX(k)+v(k)−X(k+1∣k)−K(k+1)[Y(k+1)−HX(k+1∣k)]AX(k)+v(k)−AX(k∣k)−K(k+1)[HAX(k)+Hv(k)+w(k+1)−HAX(k∣k)]A[X(k)−X(k∣k)]−K(k+1)HA[X(k)−X(k∣k)]+v(k)−K(k+1)Hv(k)−K(k+1)w(k+1)[I−K(k+1)H]A[X(k)−X(k∣k)]+[I−K(k+1)H]v(k)−K(k+1)w(k+1)[I−K(k+1)H]Ae(k∣k)+[I−K(k+1)H]v(k)−K(k+1)w(k+1)​(3.7)

将状态空间方程 ( 1.1 ) (1.1) (1.1) 和式 ( 3.1 ) (3.1) (3.1) 代入式 ( 3.6 ) (3.6) (3.6) ，得：

e ( k + 1 ∣ k ) = X ( k + 1 ) − X ( k + 1 ∣ k ) = A X ( k ) + v ( k ) − A X ( k ∣ k ) = A [ X ( k ) − X ( k ∣ k ) ] + v ( k ) = A e ( k ∣ k ) + v ( k ) (3.8) \begin{aligned} e(k+1|k) &= X(k+1) - X(k+1|k) \\ &= AX(k) + v(k) - AX(k|k) \\ &= A\left[X(k)- X(k|k)\right] + v(k) \\ &= Ae(k|k) + v(k) \tag{3.8} \end{aligned} e(k+1∣k)​=X(k+1)−X(k+1∣k)=AX(k)+v(k)−AX(k∣k)=A[X(k)−X(k∣k)]+v(k)=Ae(k∣k)+v(k)​(3.8)

设：
系统过程噪声 v ( k ) v(k) v(k) 的协方差矩阵
Q k = E { v ( k ) v T ( k ) } (3.9) Q_k = E\left\{v(k)v^T(k)\right\} \tag{3.9} Qk​=E{v(k)vT(k)}(3.9)
测量噪声 w ( k ) w(k) w(k) 的协方差矩阵
R k = E { w ( k ) w T ( k ) } (3.10) R_k = E\left\{w(k)w^T(k)\right\} \tag{3.10} Rk​=E{w(k)wT(k)}(3.10)

因为卡尔曼滤波算法假设：系统状态最优估计误差 e ( k ∣ k ) e(k|k) e(k∣k) 、系统过程噪声 v ( k ) v(k) v(k) 、测量噪声 w ( k + 1 ) w(k+1) w(k+1) 两两不相关，即协方差矩阵为 0 0 0 ，有：

E { e ( k ∣ k ) v T ( k ) } = 0 E\left\{e(k|k)v^T(k)\right\} = 0 E{e(k∣k)vT(k)}=0
E { e ( k ∣ k ) w T ( k + 1 ) } = 0 E\left\{e(k|k)w^T(k+1)\right\} = 0 E{e(k∣k)wT(k+1)}=0
E { v ( k ) w T ( k + 1 ) } = 0 E\left\{v(k)w^T(k+1)\right\} = 0 E{v(k)wT(k+1)}=0

设最优估计误差的协方差矩阵：

P ( k + 1 ∣ k + 1 ) = E { e ( k + 1 ∣ k + 1 ) e T ( k + 1 ∣ k + 1 ) } (3.11) P(k+1|k+1) = E\left\{e(k+1|k+1)e^T(k+1|k+1)\right\} \tag{3.11} P(k+1∣k+1)=E{e(k+1∣k+1)eT(k+1∣k+1)}(3.11)

将式 ( 3.7 ) (3.7) (3.7) 、式 ( 3.9 ) (3.9) (3.9) 、式 ( 3.10 ) (3.10) (3.10) 代入式 ( 3.11 ) (3.11) (3.11) ，所有交叉项中期望部分都为 0 0 0 ，整理得：

P ( k + 1 ∣ k + 1 ) = E { e ( k + 1 ∣ k + 1 ) e T ( k + 1 ∣ k + 1 ) } = [ I − K ( k + 1 ) H ] A E { e ( k ∣ k ) e T ( k ∣ k ) } A T [ I − K ( k + 1 ) H ] T + [ I − K ( k + 1 ) H ] E { v ( k ) v T ( k ) } [ I − K ( k + 1 ) H ] T + K ( k + 1 ) E { w ( k + 1 ) w T ( k + 1 ) } K T ( k + 1 ) = [ I − K ( k + 1 ) H ] A P ( k ∣ k ) A T [ I − K ( k + 1 ) H ] T + [ I − K ( k + 1 ) H ] Q k [ I − K ( k + 1 ) H ] T + K ( k + 1 ) R k + 1 K T ( k + 1 ) = [ I − K ( k + 1 ) H ] [ A P ( k ∣ k ) A T + Q k ] [ I − K ( k + 1 ) H ] T + K ( k + 1 ) R k + 1 K T ( k + 1 ) (3.12) \begin{aligned} P(k+1|k+1) =& E\left\{e(k+1|k+1)e^T(k+1|k+1)\right\} \\ =& \left[I - K(k+1)H\right]AE\left\{e(k|k)e^T(k|k)\right\}A^T\left[I - K(k+1)H\right]^T \\ &+ \left[I - K(k+1)H\right]E\left\{v(k)v^T(k)\right\}\left[I - K(k+1)H\right]^T \\ &+ K(k+1)E\left\{w(k+1)w^T(k+1)\right\}K^T(k+1) \\ =& \left[I - K(k+1)H\right]AP(k|k)A^T\left[I - K(k+1)H\right]^T \\ &+ \left[I - K(k+1)H\right]Q_k\left[I - K(k+1)H\right]^T \\ &+ K(k+1)R_{k+1}K^T(k+1) \\ =& \left[I - K(k+1)H\right]\left[AP(k|k)A^T + Q_k\right]\left[I - K(k+1)H\right]^T \\ &+ K(k+1)R_{k+1}K^T(k+1) \tag{3.12} \end{aligned} P(k+1∣k+1)====​E{e(k+1∣k+1)eT(k+1∣k+1)}[I−K(k+1)H]AE{e(k∣k)eT(k∣k)}AT[I−K(k+1)H]T+[I−K(k+1)H]E{v(k)vT(k)}[I−K(k+1)H]T+K(k+1)E{w(k+1)wT(k+1)}KT(k+1)[I−K(k+1)H]AP(k∣k)AT[I−K(k+1)H]T+[I−K(k+1)H]Qk​[I−K(k+1)H]T+K(k+1)Rk+1​KT(k+1)[I−K(k+1)H][AP(k∣k)AT+Qk​][I−K(k+1)H]T+K(k+1)Rk+1​KT(k+1)​(3.12)

设预测误差的协方差矩阵：

P ( k + 1 ∣ k ) = E { e ( k + 1 ∣ k ) e T ( k + 1 ∣ k ) } (3.13) P(k+1|k) = E\left\{e(k+1|k)e^T(k+1|k)\right\} \tag{3.13} P(k+1∣k)=E{e(k+1∣k)eT(k+1∣k)}(3.13)

将式 ( 3.8 ) (3.8) (3.8) 、式 ( 3.9 ) (3.9) (3.9) 、式 ( 3.11 ) (3.11) (3.11) 代入式 ( 3.13 ) (3.13) (3.13) ，所有交叉项中期望部分都为 0 0 0 ，整理得：

P ( k + 1 ∣ k ) = E { e ( k + 1 ∣ k ) e T ( k + 1 ∣ k ) } = A E { e ( k ∣ k ) e T ( k ∣ k ) } A T + E { v ( k ) v T ( k ) } = A P ( k ∣ k ) A T + Q k (3.14) \begin{aligned} P(k+1|k) &= E\left\{e(k+1|k)e^T(k+1|k)\right\} \\ &= AE\left\{e(k|k)e^T(k|k)\right\}A^T + E\left\{v(k)v^T(k)\right\} \\ &= AP(k|k)A^T + Q_k \tag{3.14} \end{aligned} P(k+1∣k)​=E{e(k+1∣k)eT(k+1∣k)}=AE{e(k∣k)eT(k∣k)}AT+E{v(k)vT(k)}=AP(k∣k)AT+Qk​​(3.14)

再将式 ( 3.14 ) (3.14) (3.14) 代入式 ( 3.12 ) (3.12) (3.12) ，得：

P ( k + 1 ∣ k + 1 ) = [ I − K ( k + 1 ) H ] P ( k + 1 ∣ k ) [ I − K ( k + 1 ) H ] T + K ( k + 1 ) R k + 1 K T ( k + 1 ) (3.15) P(k+1|k+1) = \left[I - K(k+1)H\right]P(k+1|k)\left[I - K(k+1)H\right]^T + K(k+1)R_{k+1}K^T(k+1) \tag{3.15} P(k+1∣k+1)=[I−K(k+1)H]P(k+1∣k)[I−K(k+1)H]T+K(k+1)Rk+1​KT(k+1)(3.15)

将式 ( 3.15 ) (3.15) (3.15) 展开并整理，得：

P ( k + 1 ∣ k + 1 ) = P ( k + 1 ∣ k ) − K ( k + 1 ) H P ( k + 1 ∣ k ) − P ( k + 1 ∣ k ) H T K T ( k + 1 ) + K ( k + 1 ) H P ( k + 1 ∣ k ) H T K T ( k + 1 ) + K ( k + 1 ) R k + 1 K T ( k + 1 ) = P ( k + 1 ∣ k ) − K ( k + 1 ) H P ( k + 1 ∣ k ) − P ( k + 1 ∣ k ) H T K T ( k + 1 ) + K ( k + 1 ) [ H P ( k + 1 ∣ k ) H T + R k + 1 ] K T ( k + 1 ) (3.16) \begin{aligned} P(k+1|k+1) =& P(k+1|k) - K(k+1)HP(k+1|k) - P(k+1|k)H^TK^T(k+1) \\ &+ K(k+1)HP(k+1|k)H^TK^T(k+1) + K(k+1)R_{k+1}K^T(k+1) \\ =& P(k+1|k) - K(k+1)HP(k+1|k) - P(k+1|k)H^TK^T(k+1) \\ &+ K(k+1)\left[HP(k+1|k)H^T + R_{k+1}\right]K^T(k+1) \tag{3.16} \end{aligned} P(k+1∣k+1)==​P(k+1∣k)−K(k+1)HP(k+1∣k)−P(k+1∣k)HTKT(k+1)+K(k+1)HP(k+1∣k)HTKT(k+1)+K(k+1)Rk+1​KT(k+1)P(k+1∣k)−K(k+1)HP(k+1∣k)−P(k+1∣k)HTKT(k+1)+K(k+1)[HP(k+1∣k)HT+Rk+1​]KT(k+1)​(3.16)

P ( k + 1 ∣ k + 1 ) P(k+1|k+1) P(k+1∣k+1) 是关于 K ( k + 1 ) K(k+1) K(k+1) 的二次函数。

因为 H P ( k + 1 ∣ k ) H T + R k + 1 HP(k+1|k)H^T + R_{k+1} HP(k+1∣k)HT+Rk+1​ 为对称正定矩阵，所以存在非奇异矩阵 M M M ，使得 M M T = H P ( k + 1 ∣ k ) H T + R k + 1 MM^T = HP(k+1|k)H^T + R_{k+1} MMT=HP(k+1∣k)HT+Rk+1​ 。

于是式 ( 3.16 ) (3.16) (3.16) 可化为：

P ( k + 1 ∣ k + 1 ) = P ( k + 1 ∣ k ) − K ( k + 1 ) H P ( k + 1 ∣ k ) − P ( k + 1 ∣ k ) H T K T ( k + 1 ) + K ( k + 1 ) M M T K T ( k + 1 ) (3.17) \begin{aligned} P(k+1|k+1) =& P(k+1|k) - K(k+1)HP(k+1|k) - P(k+1|k)H^TK^T(k+1) \\ &+ K(k+1)MM^TK^T(k+1) \tag{3.17} \end{aligned} P(k+1∣k+1)=​P(k+1∣k)−K(k+1)HP(k+1∣k)−P(k+1∣k)HTKT(k+1)+K(k+1)MMTKT(k+1)​(3.17)

令 N = P ( k + 1 ∣ k ) H T M − T N = P(k+1|k)H^TM^{-T} N=P(k+1∣k)HTM−T ，对式 ( 3.17 ) (3.17) (3.17) 配方，有：

P ( k + 1 ∣ k + 1 ) = P ( k + 1 ∣ k ) + [ K ( k + 1 ) M − N ] [ K ( k + 1 ) M − N ] T − N N T (3.18) P(k+1|k+1) = P(k+1|k) + \left[K(k+1)M - N\right]\left[K(k+1)M - N\right]^T - NN^T \tag{3.18} P(k+1∣k+1)=P(k+1∣k)+[K(k+1)M−N][K(k+1)M−N]T−NNT(3.18)

因为式 ( 3.18 ) (3.18) (3.18) 中 P ( k + 1 ∣ k ) P(k+1|k) P(k+1∣k) ， N N T NN^T NNT 两项均与 K ( k + 1 ) K(k+1) K(k+1) 无关，所以当 K ( k + 1 ) = N M − 1 K(k+1) = NM^{-1} K(k+1)=NM−1 时， P ( k + 1 ∣ k + 1 ) P(k+1|k+1) P(k+1∣k+1) 最小。

即：

K ( k + 1 ) = P ( k + 1 ∣ k ) H T M − T M − 1 = P ( k + 1 ∣ k ) H T ( M M T ) − 1 = P ( k + 1 ∣ k ) H T [ H P ( k + 1 ∣ k ) H T + R k + 1 ] − 1 (3.19) \begin{aligned} K(k+1) &= P(k+1|k)H^TM^{-T}M^{-1} \\ &= P(k+1|k)H^T(MM^T)^{-1} \\ &= P(k+1|k)H^T\left[HP(k+1|k)H^T + R_{k+1}\right]^{-1} \tag{3.19} \end{aligned} K(k+1)​=P(k+1∣k)HTM−TM−1=P(k+1∣k)HT(MMT)−1=P(k+1∣k)HT[HP(k+1∣k)HT+Rk+1​]−1​(3.19)

将式 ( 3.19 ) (3.19) (3.19) 代入式 ( 3.16 ) (3.16) (3.16) ，得：

P ( k + 1 ∣ k + 1 ) = P ( k + 1 ∣ k ) − K ( k + 1 ) H P ( k + 1 ∣ k ) − P ( k + 1 ∣ k ) H T K T ( k + 1 ) + K ( k + 1 ) [ H P ( k + 1 ∣ k ) H T + R k + 1 ] K T ( k + 1 ) = P ( k + 1 ∣ k ) − K ( k + 1 ) H P ( k + 1 ∣ k ) − P ( k + 1 ∣ k ) H T K T ( k + 1 ) + P ( k + 1 ∣ k ) H T K T ( k + 1 ) = [ I − K ( k + 1 ) H ] P ( k + 1 ∣ k ) (3.20) \begin{aligned} P(k+1|k+1) =& P(k+1|k) - K(k+1)HP(k+1|k) - P(k+1|k)H^TK^T(k+1) \\ &+ K(k+1)\left[HP(k+1|k)H^T + R_{k+1}\right]K^T(k+1) \\ =& P(k+1|k) - K(k+1)HP(k+1|k) - P(k+1|k)H^TK^T(k+1) \\ &+ P(k+1|k)H^TK^T(k+1) \\ =& \left[I - K(k+1)H\right]P(k+1|k) \tag{3.20} \end{aligned} P(k+1∣k+1)===​P(k+1∣k)−K(k+1)HP(k+1∣k)−P(k+1∣k)HTKT(k+1)+K(k+1)[HP(k+1∣k)HT+Rk+1​]KT(k+1)P(k+1∣k)−K(k+1)HP(k+1∣k)−P(k+1∣k)HTKT(k+1)+P(k+1∣k)HTKT(k+1)[I−K(k+1)H]P(k+1∣k)​(3.20)

回顾公式对应关系：

• ( 2.1 ) → ( 3.1 ) (2.1) \rightarrow (3.1) (2.1)→(3.1)
• ( 2.2 ) → ( 3.14 ) (2.2) \rightarrow (3.14) (2.2)→(3.14)
• ( 2.3 ) → ( 3.4 ) (2.3) \rightarrow (3.4) (2.3)→(3.4)
• ( 2.4 ) → ( 3.19 ) (2.4) \rightarrow (3.19) (2.4)→(3.19)
• ( 2.5 ) → ( 3.20 ) (2.5) \rightarrow (3.20) (2.5)→(3.20)