卡尔曼滤波公式及推导

1 前言

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

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

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

\[\left\{ \begin{align*} X(k+1) &= AX(k) + v(k) \\ Y(k) &= HX(k) + w(k) \end{align*} \tag{1.1} \right.\]

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

2 卡尔曼滤波公式

2.1 预测

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

\[X(k+1|k) = AX(k|k) \tag{2.1}\] \[P(k+1|k) = AP(k|k)A^T + Q_k \tag{2.2}\]

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

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

2.2 校正

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

\[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)$ 是 $k+1$ 时刻系统状态的最优估计； $K(k+1)$ 是卡尔曼增益矩阵，其计算方法如下：

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

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

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

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

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

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

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

3 推导过程

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

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

\[X(k+1|k) = AX(k|k) \tag{3.1}\]

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

\[Y(k+1|k) = HX(k+1|k) \tag{3.2}\]

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

\[Y(k+1) - Y(k+1|k) = Y(k+1) - HX(k+1|k) \tag{3.3}\]

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

\[\begin{align*} 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] \end{align*} \tag{3.4}\]

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

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

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

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

有：

\[e(k+1|k+1) = X(k+1) - X(k+1|k+1) \tag{3.5}\] \[e(k+1|k) = X(k+1) - X(k+1|k) \tag{3.6}\]

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

\[\begin{align*} 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) \end{align*} \tag{3.7}\]

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

\[\begin{align*} 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) \end{align*} \tag{3.8}\]

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

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

\[E\left\{e(k|k)v^T(k)\right\} = 0\] \[E\left\{e(k|k)w^T(k+1)\right\} = 0\] \[E\left\{v(k)w^T(k+1)\right\} = 0\]

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

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

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

\[\begin{align*} 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) \end{align*} \tag{3.12}\]

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

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

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

\[\begin{align*} 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 \end{align*} \tag{3.14}\]

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

\[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}\]

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

\[\begin{align*} 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) \end{align*} \tag{3.16}\]

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

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

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

\[\begin{align*} 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) \end{align*} \tag{3.17}\]

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

\[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}\]

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

即：

\[\begin{align*} 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} \end{align*} \tag{3.19}\]

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

\[\begin{align*} 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) \end{align*} \tag{3.20}\]

回顾公式对应关系：

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