扩展卡尔曼滤波公式

扩展卡尔曼滤波公式

1 前言

在上文《卡尔曼滤波公式及其推导》中，提到了卡尔曼滤波算法适用于线性离散系统。当系统是非线性离散系统时，就需要扩展卡尔曼滤波算法 (Extended Kalman Filter) 对系统状态进行最优估计。

其主要思想是将非线性系统进行线性化，引入雅可比矩阵。

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

$$\{\begin{array}{rl}X(k+1)& =f\left(X(k),u(k)\right)+v(k)\\ Y(k)& =h\left(X(k)\right)+w(k)\end{array}\text{\hspace{1em}(1.1)}$$

式中，$f(\cdot )$ 为状态方程，$h(\cdot )$ 为输出方程。$X(k)$ 为 $k$ 时刻的系统状态，$u(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)=f\left(X(k|k),u(k)\right)\text{\hspace{1em}(2.1)}$$
$$P(k+1|k)=F(k)P(k|k)F^T(k)+Q_k\text{\hspace{1em}(2.2)}$$

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

式 $(2.2)$ 是误差的协方差矩阵预测方程。其中，$P(k|k)$ 是 $k$ 时刻系统状态的最优估计 $X(k|k)$ 对应的误差协方差矩阵，$P(k+1|k)$ 是系统状态的预测 $X(k+1|k)$ 对应的误差协方差矩阵。$F(k)$ 是方程 $f\left(X(k|k),u(k)\right)$ 的雅可比矩阵。$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)-h\left(X(k+1|k)\right)\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(k){\left[H(k)P(k+1|k)H^T(k)+R_{k+1}\right]}^{-1}\text{\hspace{1em}(2.4)}$$

其中，$H(k)$ 是方程 $h\left(X(k)\right)$ 的雅可比矩阵。$R_{k+1}$ 是 $k+1$ 时刻测量噪声的协方差矩阵。

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

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

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

3 小结

扩展卡尔曼滤波公式就是把卡尔曼滤波公式中状态矩阵 $A$ 换成了状态方程的雅可比矩阵 $F(k)$ ，输出矩阵 $H$ 换成了输出方程的雅可比矩阵 $H(k)$ 。

• $A\to F(k)$
• $H\to H(k)$