扩展卡尔曼滤波EKF与多传感器融合

Extended Kalman Filter(扩展卡尔曼滤波)是卡尔曼滤波的非线性版本。在状态转移方程确定的情况下,EKF已经成为了非线性系统状态估计的事实标准。本文将简要介绍EKF,并介绍其在无人驾驶多传感器融合上的应用。

KF与EKF

本文假定读者已熟悉KF,若不熟悉请参考卡尔曼滤波简介

KF与EKF的区别如下:

  1. 预测未来:x′=Fx+u用x′=f(x,u)代替;其余F用Fj代替。
  2. 修正当下:将状态映射到测量的Hx′用h(x′)代替;其余H用Hj代替。

其中,非线性函数f(x,u),h(x′)用非线性得到了更精准的状态预测值、映射后的测量值;线性变换Fj,,Hj通过线性变换使得变换后的x,z仍满足高斯分布的假设。

Fj,Hj计算方式如下:

为什么要用EKF

KF的假设之一就是高斯分布的x预测后仍服从高斯分布,高斯分布的x变换到测量空间后仍服从高斯分布。可是,假如F、H是非线性变换,那么上述条件则不成立。

将非线性系统线性化

既然非线性系统不行,那么很自然的解决思路就是将非线性系统线性化。

对于一维系统,采用泰勒一阶展开即可得到:

对于多维系统,仍旧采用泰勒一阶展开即可得到:

其中,Df(a)是Jacobian矩阵。

多传感器融合

lidar与radar

本文将以汽车跟踪为例,目标是知道汽车时刻的状态

。已知的传感器有lidar、radar。

  • lidar:笛卡尔坐标系。可检测到位置,没有速度信息。其测量值

  • radar:极坐标系。可检测到距离,角度,速度信息,但是精度较低。其测量值

,图示如下。

传感器融合步骤

步骤图如上所示,包括:

  1. 收到第一个测量值,对状态xx进行初始化。
  2. 预测未来
  3. 修正当下

初始化

初始化,指在收到第一个测量值后,对状态x进行初始化。初始化如下,同时加上对时间的更新。

对于radar来说,

对于radar来说,

预测未来

预测主要涉及的公式是:

需要求解的有三个变量:F、P、Q。

F表明了系统的状态如何改变,这里仅考虑线性系统,F易得:

P表明了系统状态的不确定性程度,用x的协方差表示,这里自己指定为:

Q表明了x′=Fx未能刻画的其他外界干扰。本例子使用线性模型,因此加速度变成了干扰项。x′=Fx中未衡量的额外项目v为:

v服从高斯分布N(0,Q)。

修正当下

lidar

lidar使用了KF。修正当下这里牵涉到的公式主要是:

需要求解的有两个变量:H、R。

H表示了状态空间到测量空间的映射。

R表示了测量值的不确定度,一般由传感器的厂家提供,这里lidar参考如下:

radar

radar使用了EKF。修正当下这里牵涉到的公式主要是:

区别与上面lidar的主要有:

  1. 状态空间到测量空间的非线性映射f(x)
  2. 非线性映射线性化后的Jacob矩阵
  3. radar的

状态空间到测量空间的非线性映射f(x)如下

非线性映射线性化后的Jacob矩阵Hj

R表示了测量值的不确定度,一般由传感器的厂家提供,这里radar参考如下:

传感器融合实例

多传感器融合的示例如下,需要注意的有:

  1. lidar和radar的预测部分是完全相同的
  2. lidar和radar的参数更新部分是不同的,不同的原因是不同传感器收到的测量值是不同的
  3. 当收到lidar或radar的测量值,依次执行预测、更新步骤
  4. 当同时收到lidar和radar的测量值,依次执行预测、更新1、更新2步骤

多传感器融合的效果如下图所示,红点和蓝点分别表示radar和lidar的测量位置,绿点代表了EKF经过多传感器融合后获取到的测量位置,取得了较低的RMSE。

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

发表于

我来说两句

0 条评论
登录 后参与评论

相关文章

来自专栏大数据挖掘DT机器学习

R语言 使用BP神经网络进行银行客户信用评估

一、学习R语言AMORE包中的newff函数 这是个前馈神经网络工具包,类似的还有nnet,RSNNS等。AMORE比nnet参数要丰富一些。AMORE...

4078
来自专栏MyBlog

Procedural Noise Adversarial Examples for Black-Box Attacks on Deep Neural Networks 论文笔记(2)

该文章提出一种利用程序化噪声来生成对抗样本的方法, 所提出的方法和那些通过梯度不断修改以至于到达分类器的边界的方法不一样, 上述方法需要对目标的模型有一定的了解...

751
来自专栏磐创AI技术团队的专栏

清华朱军团队包揽三项冠军,NIPS 2017对抗样本攻防竞赛总结

1212
来自专栏琦小虾的Binary

论文翻译:ViBe+算法(ViBe算法的改进版本)

论文翻译:ViBe+算法(ViBe算法的改进版本) 原文地址: 《Background Subtraction: Experiments and Impro...

3599
来自专栏小小挖掘机

推荐系统遇上深度学习(一)--FM模型理论和实践

1、FM背景 在计算广告和推荐系统中,CTR预估(click-through rate)是非常重要的一个环节,判断一个商品的是否进行推荐需要根据CTR预估的点击...

2K10
来自专栏PPV课数据科学社区

【V课堂】R语言十八讲(十四)—几大检验

在统计分析中,我们会听到很多检验,有T检验,卡方检验,秩和检验,F检验,费舍尔检验等等,这么多检验,光听就要晕了,还怎么用啊?哪种检验什么时候能用什么时候不能用...

2887
来自专栏机器之心

入门 | 深度学习模型的简单优化技巧

以下是我与同事和学生就如何优化深度模型进行的对话、消息和辩论的摘要。如果你发现了有影响力的技巧,请分享。

1300
来自专栏机器之心

业界 | 谷歌发布TensorFlow 1.4与TensorFlow Lattice:利用先验知识提升模型准确度

2816
来自专栏YoungGy

卡尔曼滤波简介

卡尔曼滤波是一种在不确定状况下组合多源信息得到所需状态最优估计的一种方法。本文将简要介绍卡尔曼滤波的原理及推导。 ? 什么是卡尔曼滤波 首先定义问题:对于某一系...

3895
来自专栏数据派THU

清华朱军团队包揽三项冠军 | NIPS 2017对抗样本攻防竞赛总结(附学习资料)

4445

扫码关注云+社区

领取腾讯云代金券