无人驾驶技术——扩展Kalman滤波（EKF）

基本的卡尔曼滤波及实现可参考： 无人驾驶技术——初探Kalman滤波器
卡尔曼滤波器介绍还可参考：https://blog.csdn.net/u010720661/article/details/63253509

卡尔曼滤波方程和扩展卡尔曼滤波方程非常相似。主要区别在于：

基本原理

综合预测值，直接测量值，得到更加接近真实的值。

运用范围

运用于线性模型。比如采用恒定速度模型，然后加减速用处理噪声来表示，这一模型用来估算行人的状态其实已经足够了。

以机器人移动为例，推导过程：

1. 以当前的速度和位置，预测下一步的速度和位置，Fk为状态转移矩阵：
   

2. 更新下一状态协方差矩阵，由（4）协方差的更新规则，可以得到（5）的协方差矩阵更新：
   

3. 内部控制量的加入，也会改变状态，比如刹车或加速，若给定的加速度为a, 状态变化如下式所示，Bk为控制矩阵，uk为控制向量：

4. 外部干扰，比如风，或者路滑等一些因素，会干扰预测的准确度，也就是会增加协方差。所以外部干扰Qk可以作为噪声处理，加到协方差上。

注意：此项目中采用另一种思路，即将内部控制量，以噪声Qk的形式加入，推导过程：
由于加速度（内部控制量）存在，状态更新为：

由于加速度未知，同时相融合外部噪音影响，所以将其带来的改变作为噪音项进行处理：

获得噪音协方差矩阵Q，其中加速度a假设为0均值，σ方差的正态分布，互不相关。

5. 更新，用测量值来更新预测值。预测值是具有一定方差的高斯分布，测量值也是具有一定方差的高斯分布。融合这两个高斯分布，可以得到两分布中都发生的的概率，则为最优估计，仍未高斯分布。

融合高斯分布：

以一维分布举例。

得到融合分布后的，均值和方差。

推广到多维分布，得到一般化公式。

案例分析：

对如下两个分布进行高斯融合，前者为预测分布，后者为测量分布：

预测分布为什么要加上观测矩阵（测量矩阵）Hk了，因为传感器读取的数据的单位和尺度，形式（直角坐标，极坐标）有可能与我们要跟踪的状态的单位和尺度不一样，加上Hk将预测值转换到传感器尺度。

根据(14),(15)的规则，可以得到更新后的分布：

将其化简，可以得到更新后的状态和协方差：

将新的状态和协方差，放到下一个预测和更新方程中不断迭代。

卡尔曼滤波器数据流直观图：

扩展卡尔曼滤波器：

卡尔曼滤波器用于处理线性系统，扩展卡尔曼滤波器用于处理非线性系统。将非线性系统转化为线性系统，其他的就和卡尔曼滤波器一样了。

系统的线性和非线性：

线性系统：

线性，指量与量之间按比例、成直线的关系。在空间和时间上代表规则和光滑的运动。

非线性系统：

非线性则指不按比例、不成直线的关系。如问：两个眼睛的视敏度是一个眼睛的几倍？很容易想到的是两倍，可实际是 6－10倍！
主要区别：是否能够采用叠加原理来进行分析。
线性系统的表达式中只有状态变量的一次项，高次、三角函数以及常数项都没有，只要有任意一个非线性环节就是非线性系统。

为什么要采用线性系统：

预测状态是高斯分布，如果将其映射到非线性函数上，则将不会保持高斯分布，这样破坏了高斯分布的基本前提，就不可以用卡尔曼滤波器了。

将非线性系统转化为线性系统：

泰勒展开，去掉一阶项后面的部分，就变成线性系统了。多维泰勒展开，一阶偏导为雅可比（jacobian）矩阵，二阶偏导为hessian矩阵。
雷达案例：需要将预测状态空间，转换到雷达测量空间。

然而h(x)为非线性函数，所以需要将其线性化，根据多维泰勒展开，可得jacobian矩阵Hj：

雅可比矩阵的推导和代码实现可参考：
雅克比矩阵

然后其他部分和卡尔曼部分一样，只是稍作更新：

注意事项1：预测部分，如果是线性的，则不需要更改。如果不是，则和求jacbian矩阵Hj一样，求Fj。
注意事项2：扩展卡尔曼滤波器，y值的求得，直接采用右边式子即可。只是变换了Hj.
注意事项3：上面对比图片公式，和上面卡尔曼推导公式(18),(19)是一样的，只是略作展开。
称K为卡尔曼增益。
案例分析：详细c++代码见GitHub：https://github.com/godloveliang/Extended-Kalman-Filter

If f and h are linear functions, then the Extended Kalman Filter generates exactly the same result as the standard Kalman Filter. Actually, if f and h are linear then the Extended Kalman Filter F_j turns into f and H_j turns into h. All that’s left is the same ol’ standard Kalman Filter!

In our case we have a linear motion model, but a nonlinear measurement model when we use radar observations. So, we have to compute the Jacobian only for the measurement function.