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

卡尔曼滤波公式及推导

1 前言

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

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

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

$$\{\begin{array}{rl}X(k+1)& =AX(k)+v(k)\\ Y(k)& =HX(k)+w(k)\end{array}\text{\hspace{1em}(1.1)}$$

式中，$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\{v(k)v^T(k)\right\}$ ，$R_k=E\left\{w(k)w^T(k)\right\}$ 。

2 卡尔曼滤波公式

2.1 预测

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

$$X(k+1|k)=AX(k|k)\text{\hspace{1em}(2.1)}$$
$$P(k+1|k)=AP(k|k)A^T+Q_k\text{\hspace{1em}(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]\text{\hspace{1em}(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}\text{\hspace{1em}(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)\text{\hspace{1em}(2.5)}$$

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

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

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