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

矫情吗；* 2021-12-11 19:23 653阅读 0赞

### 文章目录 ###

*  基本原理
 *  运用范围
 *  以机器人移动为例，推导过程：
 *  融合高斯分布：
 *  案例分析：
 *  卡尔曼滤波器数据流直观图：
 *  扩展卡尔曼滤波器：
 *  系统的线性和非线性：
 *   *  线性系统：
     *  非线性系统：
     *  为什么要采用线性系统：
     *  将非线性系统转化为线性系统：

基本的卡尔曼滤波及实现可参考： [无人驾驶技术——初探Kalman滤波器][Kalman]  
卡尔曼滤波器介绍还可参考：https://blog.csdn.net/u010720661/article/details/63253509  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70]  
卡尔曼滤波方程和扩展卡尔曼滤波方程非常相似。主要区别在于：  
![在这里插入图片描述][20190902152422291.png]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 1]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 2]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 3]

# 基本原理 #

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

# 运用范围 #

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

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

1.  以当前的速度和位置，预测下一步的速度和位置，Fk为状态转移矩阵：  
    ![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 4]
2.  更新下一状态协方差矩阵，由（4）协方差的更新规则，可以得到（5）的协方差矩阵更新：  
    ![在这里插入图片描述][20190709165419722.png]
3.  内部控制量的加入，也会改变状态，比如刹车或加速，若给定的加速度为a, 状态变化如下式所示，Bk为控制矩阵，uk为控制向量：

![在这里插入图片描述][20190709165430874.png]  
4. 外部干扰，比如风，或者路滑等一些因素，会干扰预测的准确度，也就是会增加协方差。所以外部干扰Qk可以作为噪声处理，加到协方差上。  
![在这里插入图片描述][20190709165440786.png]  
**注意**：此项目中采用另一种思路，即将内部控制量，以噪声Qk的形式加入，推导过程：  
由于加速度（内部控制量）存在，状态更新为：

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

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

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

# 融合高斯分布： #

以一维分布举例。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 5]  
得到融合分布后的，均值和方差。  
![在这里插入图片描述][20190709165516556.png]  
推广到多维分布，得到一般化公式。  
![在这里插入图片描述][201907091655275.png]

# 案例分析： #

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

![在这里插入图片描述][20190709165537816.png]  
预测分布为什么要加上**观测矩阵（测量矩阵）Hk**了，因为传感器读取的数据的单位和尺度，形式（直角坐标，极坐标）有可能与我们要跟踪的状态的单位和尺度不一样，加上**Hk将预测值转换到传感器尺度**。  
![在这里插入图片描述][20190709165552505.png]  
根据(14),(15)的规则，可以得到更新后的分布：

![在这里插入图片描述][20190709165614378.png]  
将其化简，可以得到更新后的状态和协方差：  
![在这里插入图片描述][20190709165623398.png]  
将新的状态和协方差，放到下一个预测和更新方程中不断迭代。

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

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 6]

# 扩展卡尔曼滤波器： #

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

# 系统的线性和非线性： #

## 线性系统： ##

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

## 非线性系统： ##

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

## 为什么要采用线性系统： ##

预测状态是高斯分布，如果将其映射到非线性函数上，则将不会保持高斯分布，这样破坏了高斯分布的基本前提，就不可以用卡尔曼滤波器了。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 7]

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

泰勒展开，去掉一阶项后面的部分，就变成线性系统了。多维泰勒展开，一阶偏导为雅可比（jacobian）矩阵，二阶偏导为hessian矩阵。  
雷达案例：需要将预测状态空间，转换到雷达测量空间。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 8]  
然而h(x)为非线性函数，所以需要将其线性化，根据多维泰勒展开，可得jacobian矩阵Hj：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 9]  
雅可比矩阵的推导和代码实现可参考：  
[雅克比矩阵][Link 1]

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

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 11]

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.

[Kalman]: https://blog.csdn.net/xiao_lxl/article/details/92969611
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70]: /images/20211211/ea14c3cc088945f8bee261589bbb174a.png
[20190902152422291.png]: /images/20211211/8532afc9bf73472d90badfcf9aade975.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 1]: /images/20211211/0ba9dba0aab645828ecbaee61ed0644c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 2]: /images/20211211/f1171da68ee648799712fe7e93f8e0bf.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 3]: /images/20211211/c61f7233909d4c46a4aae417695a3609.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 4]: /images/20211211/2aa26409be2d44d1a5b3fa2b0e852b91.png
[20190709165419722.png]: /images/20211211/62fd5001b557473098bcc98a5d87c101.png
[20190709165430874.png]: /images/20211211/1131b03881c5499ebe04f23fb22a31cd.png
[20190709165440786.png]: /images/20211211/1b405917107341d59f5853d7b32a1a77.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 5]: /images/20211211/d320840cca274dc1b0eccd33cb23e0cc.png
[20190709165516556.png]: /images/20211211/a4c4f60748a94e21a5fc0b412f3a906c.png
[201907091655275.png]: /images/20211211/86e1228b87fd4207a7858197127248b4.png
[20190709165537816.png]: /images/20211211/b0030ab755b049f0a0ae9cb68c802e53.png
[20190709165552505.png]: /images/20211211/35d45e76f4bd4c94a1e25b81c0d005d0.png
[20190709165614378.png]: /images/20211211/63934baa975941a3852b956fee04a09b.png
[20190709165623398.png]: /images/20211211/a0e365412c534a3a9f00d187f7fcefc9.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 6]: /images/20211211/8f7a4d4455cd4dc0936518d4643841ef.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 7]: /images/20211211/64de23504c5646aa975fe0fc8906a693.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 8]: /images/20211211/6ff908f3334944f089e6e160a4ed2bc9.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 9]: /images/20211211/4afa39f08be34778ac34e2abc64b1b15.png
[Link 1]: https://blog.csdn.net/xiao_lxl/article/details/100289075
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 10]: /images/20211211/9ea6b80cff3e49fa92d4cd3e7b6a3f86.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 11]: /images/20211211/3b2a08a0291b4ed28824b66118559e23.png