Apollo学习笔记（18）EKF

书接上回啊，上一篇详细的介绍了下标准卡尔曼滤波，使用卡尔曼滤波进行更新和预测都是建立在线性系统的基础上，因为高斯分布$x_k$预测后仍然是高斯分布，且高斯分布变换到观测空间后也仍然是符合高斯分布的。

然而，实际工程中，极少有系统是线性系统，那这种情况怎么使用卡尔曼滤波呢？

非线性处理

学过数学的都知道，线性函数可以使用下式进行表达，

$$f(x)=ax+b\text{\hspace{1em}(1)}$$

然而其他的函数，比如，$f(x)=a{x}^2+bx+c$，$f(x)=\sin x$，$f(x)=\ln x$，$f(x)=a^x$等，这些都不是线性函数，按道理来说是都没办法使用卡尔曼滤波的，那工程中如何使用卡尔曼滤波呢？比如无人驾驶中的车辆运动学模型，如何使用卡尔曼滤波对车辆的位置进行跟踪？

大学那会都学过，任何光滑连续的曲线都是可微的，一个光滑的曲线可以用多个连续的短距离的直线来表示，如下图，

这就给我们在非线性系统上应用卡尔曼滤波提供了一种思路，可以使用泰勒级数对非线性系统进行展开，只取其一阶的泰勒级数，使用非线性系统当前位置的一阶泰勒级数进行近似和逼近。

扩展卡尔曼滤波

上面介绍了，如何将非线性系统通过泰勒展开成一阶系统，现在，来看一下EKF的整体计算流程。其实，扩展卡尔曼滤波与的整体流程与标准卡尔曼是大致相同的，如下，

第一步，确定状态预测方程，
$$\begin{gathered} {\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k{\vec{u}}_k \\ {P}_k=F_k{P}_{k-1}{F}_k^T+Q_k\text{\hspace{1em}(2)} \end{gathered}$$

第二步，计算卡尔曼增益，
$$K^{{}^{\prime }}=P_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}\text{\hspace{1em}(3)}$$

第三步，更新系统状态和协方差矩阵，
$$\begin{gathered} {\hat{x}}_k^{{}^{\prime }}={\hat{x}}_k+K^{{}^{\prime }}({\vec{z}}_k-H_k{\hat{x}}_k) \\ {P}_k^{{}^{\prime }}=P_k-K^{{}^{\prime }}{H}_k{P}_k\text{\hspace{1em}(4)} \end{gathered}$$

第四步，将系统状态方程泰勒展开到一阶方程，并用此一阶方程作为下一个计算循环的系统方程，
$$\begin{gathered} F_{k+1}=\frac{\delta f}{\delta {\hat{x}}^{{}^{\prime }}} \\ {\hat{x}}_{k+1}=F_{k+1}{\hat{x}}_k+B_{k+1}{\vec{u}}_{k+1} \\ {P}_{k+1}=F_{k+1}{P}_k{F}_{k+1}^T+Q_{k+1}\text{\hspace{1em}(5)} \end{gathered}$$