扩展卡尔曼滤波公式
1 前言
在上文《卡尔曼滤波公式及其推导》中，提到了卡尔曼滤波算法适用于线性离散系统。当系统是非线性离散系统时，就需要扩展卡尔曼滤波算法 (Extended Kalman Filter) 对系统状态进行最优估计。
其主要思想是将非线性系统进行线性化，引入雅可比矩阵。
假设非线性离散系统的状态空间方程如下：
\[\left\{ \begin{align*} X(k+1) &= f\left(X(k), u(k)\right) + v(k) \\ Y(k) &= h\left(X(k)\right) + w(k) \end{align*} \right. \tag{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\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) = f\left(X(k|k), u(k)\right) \tag{2.1}\] \[P(k+1|k) = F(k)P(k|k)F^T(k) + Q_k \tag{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] \tag{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} \tag{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) \tag{2.5}\]其中，$I$ 为单位矩阵， $P(k+1|k+1)$ 是 $k+1$ 时刻系统状态的最优估计 $X(k+1|k+1)$ 对应的误差协方差矩阵。
3 小结
扩展卡尔曼滤波公式就是把卡尔曼滤波公式中状态矩阵 $A$ 换成了状态方程的雅可比矩阵 $F(k)$ ，输出矩阵 $H$ 换成了输出方程的雅可比矩阵 $H(k)$ 。
- $A \rightarrow F(k)$
- $H \rightarrow H(k)$