Kalman Filter in SLAM (3) ——Extended Kalman Filter (EKF, 扩展卡尔曼滤波)

---

1. 线性系统的 Kalman Filter 回顾

• 系统状态空间方程：
  $$\begin{array}{ll}{x}_k=A{x}_{k-1}+B{U}_{k-1}+w_{k-1}& P(\omega )\sim N(0,Q)\\ z_k=H{x}_k+H_k& P(v)\sim N(0,R)\end{array}$$

• 预测：
  $$\begin{array}{rl}& {\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}\\ & P_k^{-}=A{P}_{k-1}{A}^{\top }+Q\end{array}$$

• 矫正（更新）：
  $$\begin{array}{rl}& K_k=\frac{P_k^{-}{H}^{\top }}{H{P}_k^{-}{H}^{\top }+R}\\ & {\hat{Y}}_k={\hat{X}}_k^{-}+K_k\left(Z_k-H{\hat{X}}_k^{-}\right)\\ & P_k=\left(I-K_{k}H\right)P_k^{-}\end{array}$$

2. Extended Kalman Filter 之 DR_CAN讲解笔记

2.1. 非线性系统

对于非线性系统来说，系统数学模型和观测模型无法用状态空间方程来表达，因为状态空间方程是线性方程，而非线性系统的这两个模型都是非线性的。可以写成如下形式：
$$\begin{array}{rlr}& x_k=f\left(x_{k-1},u_{k-1},{\omega }_{k-1}\right)& P(\omega )\sim N(0,Q)\\ & z_k=h\left(x_k,v_k\right)& P(v)\sim N(0,R)\end{array}$$

注意：上述的高斯噪声是在非线性函数里面的，这样即使噪声原本是高斯的，但是经过非线性系统之后，它的分布也不再是高斯分布了，如下图所示。

可以发现，Kalman Filter的两个前提条件：（1）线性系统；（2）高斯噪声，已经全都不满足了。

所以对于非线性系统应用 Kalman Filter 的最好方法就是对系统在工作点/线性化点 进行线性化，因为只有线性化之后我们才能写成线性的形式，才能得到系数矩阵$A$和$H$，进而计算Kalman Gain，最后进行数据融合。

2.2. 非线性系统线性化

线性化需要在一个线性化点进行，对于非线性系统的Kalman Filter来说，最准确的地方就是在真实状态点进行线性化。

但是真实状态点到底是多少我们永远都不知道，因为如果知道了就不需要做 Kalman Filter了。所以我们只能在我们知道的工作点处进行线性化，我们知道的工作点有两个：

• 上一次的最优估计状态${\hat{x}}_{k-1}$
• 这一次的先验估计状态${\hat{x}}_k^{-}$

2.2.1. 状态方程$f(x_k)$在上一次的最优估计状态${\hat{x}}_{k-1}$处线性化

2.2.2. 观测方程$h(x_k)$在这一次的预测状态${\tilde{x}}_k$处线性化

2.3. Extended Kalman Filter

3. Extended Kalman Filter 之 我的理解与详细解释

既然是卡尔曼滤波，自然就是两个方程：状态方程和观测方程。由于卡尔曼滤波是离散的，所以下面我们先给出IMU的离散状态空间方程和状态观测方程如下：

$$\begin{gathered} {\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1}) \\ {\mathbf{z}}_k=\mathbf{h}({\mathbf{x}}_k,{\mathbf{v}}_k) \end{gathered}$$

由于这两个方程都是非线性方程，所以为了使用使用卡尔曼滤波，必须对他们进行线性化。并且由于线性化必须知道线性化工作点，而我们实际知道的都是名义状态值，这里的名义值包括 上一次的状态后验值 ${\check{\mathbf{x}}}_{k-1}$ 和 这一次的状态先验值 ${\hat{\mathbf{x}}}_k$。所以对上面两个公式进行线性化也都是在 名义状态 的地方进行线性化，得到如下的公式：

$$\begin{gathered} {\hat{\mathbf{x}}}_k=\mathbf{f}({\hat{\mathbf{x}}}_{k-1},0)+{\mathbf{F}}_{k-1}({\mathbf{x}}_{k-1}-{\check{\mathbf{x}}}_{k-1})+{\mathbf{W}}_{k-1}{\mathbf{w}}_{k-1} \\ {\mathbf{z}}_k=\mathbf{h}({\hat{\mathbf{x}}}_k,0)+{\mathbf{H}}_k({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k)+{\mathbf{V}}_k{\mathbf{v}}_k \end{gathered}$$

注意：

（1） 时刻记住我们线性化的关键目的是什么：是为了得到系数矩阵，从而变成线性系统，可以使用典型的线性系统卡尔曼滤波。

（2） 上面的方程线性化出来的两个固定函数值 $\mathbf{f}({\hat{\mathbf{x}}}_{k-1},0)$ 和 $\mathbf{h}({\hat{\mathbf{x}}}_k,0)$ 对我们使用卡尔曼滤波没有影响，因为我们计算协方差矩阵的时候是使用系数矩阵来计算的。

（3）假设传感器观测到的值是${\mathbf{z}}_m$，这里省略掉了时间下标$k$，然后用下标$m$表示是 观测的测量值。然后用$\mathbf{z}$代表预测观测值，也就是我们把IMU预测得到的状态（先验状态）${\hat{\mathbf{x}}}_k$ 带入到观测方程中得到的 计算出来的观测值。因为看上面的典型的卡尔曼滤波公式就知道，我们是把IMU 先验状态 带入观测方程中得到一个 计算出来的预测观测值，然后和真正的观测传感器的测量值作差，再乘以卡尔曼增益进行校正。所以这里计算的 预测观测值 表达公式如下（因为我们在计算，噪声不知道是多少，所以简化为0）：
$$\mathbf{z}=\mathbf{h}({\hat{\mathbf{x}}}_k,0)+{\mathbf{H}}_k({\hat{\mathbf{x}}}_k-{\hat{\mathbf{x}}}_k)+{\mathbf{V}}_{k}0=\mathbf{h}({\hat{\mathbf{x}}}_k,0)$$

（4）重要：上面的公式中，${\mathbf{x}}_{k-1}$ 和 ${\mathbf{x}}_k$ 是 自变量，${\hat{\mathbf{x}}}_k$ 和 ${\mathbf{z}}_k$ 是 函数值，所以我们带入不同的自变量值会得到不同的函数值。比如我们实际计算的时候，在预测方程中带入的是 上一次的后验状态值${\check{\mathbf{x}}}_{k-1}$，那么得到的就是 本次的先验状态值 ${\hat{\mathbf{x}}}_k=\mathbf{f}({\hat{\mathbf{x}}}_{k-1},0)$；我们在观测方程中带入 本次的先验状态值，得到的就是 本次的先验预测观测值${\mathbf{z}}_k=\mathbf{h}({\hat{\mathbf{x}}}_k,0)$。

（5）注意：在我们计算卡尔曼滤波的时候，我们只能按照（4）中所说的那样带入 上一次的先验状态到状态方程，然后带入 这一次的先验状态到观测方程。因为在典型的线性卡尔曼滤波中就是这么做的，我们就是要融合状态方程和观测方程。

最后，给出EKF的卡尔曼滤波方程为：

预测公式：
$$\begin{gathered} 带入上一时刻的后验状态到自变量中，即{\mathbf{x}}_{k-1}\leftarrow {\check{\mathbf{x}}}_{k-1}: \\ {\hat{\mathbf{x}}}_k=\mathbf{f}({\hat{\mathbf{x}}}_{k-1},0)+{\mathbf{F}}_{k-1}({\check{\mathbf{x}}}_{k-1}-{\check{\mathbf{x}}}_{k-1})=\mathbf{f}({\hat{\mathbf{x}}}_{k-1},0) \\ {\hat{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\check{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^T+\mathbf{Q} \end{gathered}$$

校正公式：
$$\begin{gathered} 带入这一时刻的先验状态到自变量中，即{\mathbf{x}}_k\leftarrow {\hat{\mathbf{x}}}_k: \\ {\mathbf{z}}_k=\mathbf{h}({\hat{\mathbf{x}}}_k,0)+{\mathbf{H}}_k({\hat{\mathbf{x}}}_k-{\hat{\mathbf{x}}}_k)=\mathbf{h}({\hat{\mathbf{x}}}_k,0) \\ \begin{array}{c}{\mathbf{K}}_k=\frac{{\hat{\mathbf{P}}}_k{\mathbf{H}}^T}{\mathbf{H}{\hat{\mathbf{P}}}_k{\mathbf{H}}^T+\mathbf{R}}\\ {\check{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{z}}_m-\mathbf{h}({\hat{\mathbf{x}}}_k,0)\right)\\ {\check{\mathbf{P}}}_k=\left(\mathbf{I}-{\mathbf{K}}_k\mathbf{H}\right){\hat{\mathbf{P}}}_k\end{array} \end{gathered}$$

---