第4章 优化｜EKF滤波器

4.1 卡尔曼滤波的引出

$$
P(\boldsymbol{x}_k|\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{1:k-1})=\int P(\boldsymbol{x}_k|\boldsymbol{x}_{k-1},\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{1:k-1}) P(\boldsymbol{x}_{k-1}|\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{1:k-1})d{\boldsymbol{x}_{k-1}}
$$

马尔可夫性：$ k $时刻状态只与$ k-1 $时刻状态有关。将如下公式简化：

• 右侧第一部分：对应k时刻的运动方程。

$$
P(\boldsymbol{x}_k|\boldsymbol{x}_{k-1},\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{1:k-1}) = P(\boldsymbol{x}_k|\boldsymbol{x}_{k-1},\boldsymbol{u}_k)
$$

• 右侧第二部分：k-1时刻的状态分布。

$$
P(\boldsymbol{x}_{k-1}|\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{1:k-1}) = P(\boldsymbol{x}_{k-1}|\boldsymbol{x}_0,\boldsymbol{u}_{1:k-1},\boldsymbol{z}_{1:k-1})
$$

因此，KF、EKF：把$ k-1 $时刻的状态分布推导至$ k $时刻。（程序运行期间，只要维护一个状态量，对它进行不断地迭代和更新即可）

4.2 线性系统和KF

线性系统：运动、观测方程可由线性方程表示：

$$
\begin{cases}
\boldsymbol{x}_k = \boldsymbol{A}_k \boldsymbol{x}_{k-1} + \boldsymbol{u}_k + \boldsymbol{\omega}_k, \quad \boldsymbol{\omega}～N(\boldsymbol{0,R}) \
\boldsymbol{z}_k = \boldsymbol{C}_k \boldsymbol{x}_k + \boldsymbol{v}_k, \quad \boldsymbol{v}_k～N(\boldsymbol{0,Q})
\end{cases}
$$

KF目标：假设已知$ k-1 $时刻的后验状态估计：$ \hat{\boldsymbol{x}}_{k-1} $和它的协方差$ \hat{\boldsymbol{P}}_{k-1} $。根据$ k $时刻的输入和观测数据，确定的后验分布$ \hat{\boldsymbol{x}}_k $。（记：$ \hat{\boldsymbol{x}}_k $后验，$ \overline{\boldsymbol{x}} $先验，$ \hat{\boldsymbol{P}}_k $协方差）

4.2.1 推导

预测：

从运动方程出发，根据高斯分布性质：

$$
P(\boldsymbol{x}_k|\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{1:k-1}) = N(\boldsymbol{A}_k \hat{\boldsymbol{x}}_{k-1} + \boldsymbol{u}_k, \boldsymbol{A}_k \hat{P}_{k-1} \boldsymbol{A}_k^T + \boldsymbol{R})
$$

从上一个时刻的状态$ \hat{\boldsymbol{x}}_{k-1} $，根据输入信息（有噪声），推断当前时刻的状态分布$ \overline{\boldsymbol{x}}_k $。得到预测方程：

$$
\overline{\boldsymbol{x}} = \boldsymbol{A}_k \hat{\boldsymbol{x}}_{k-1} + \boldsymbol{u}_k, \quad \overline{\boldsymbol{P}} = \boldsymbol{A}_k \hat{\boldsymbol{P}}_{k-1} \boldsymbol{A}_k^T + \boldsymbol{R}
$$

更新：

从观测方程出发，可计算在某个状态下，应该产生怎样的观测数据。

$$
P(\boldsymbol{z}_k|\boldsymbol{x}_k) = N(\boldsymbol{C}_k\boldsymbol{x}_k,\boldsymbol{Q})
$$

4.2.2 总结

![]()

4.3 非线性系统和EKF

SLAM中运动、观测方程通常是非线性的，非线性系统：