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

KF与EKF
本文假定读者已熟悉KF,若不熟悉请参考卡尔曼滤波简介。
KF与EKF的区别如下:
- 预测未来:x′=Fx+u用x′=f(x,u)代替;其余F用Fj代替。
- 修正当下:将状态映射到测量的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:极坐标系。可检测到距离,角度,速度信息,但是精度较低。其测量值

,图示如下。

传感器融合步骤

步骤图如上所示,包括:
- 收到第一个测量值,对状态xx进行初始化。
- 预测未来
- 修正当下
初始化
初始化,指在收到第一个测量值后,对状态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的主要有:
- 状态空间到测量空间的非线性映射f(x)
- 非线性映射线性化后的Jacob矩阵
- radar的

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

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

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

传感器融合实例
多传感器融合的示例如下,需要注意的有:
- lidar和radar的预测部分是完全相同的
- lidar和radar的参数更新部分是不同的,不同的原因是不同传感器收到的测量值是不同的
- 当收到lidar或radar的测量值,依次执行预测、更新步骤
- 当同时收到lidar和radar的测量值,依次执行预测、更新1、更新2步骤

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

扩展卡尔曼滤波EKF与多传感器融合的更多相关文章
- Google Cardboard的九轴融合算法——基于李群的扩展卡尔曼滤波
Google Cardboard的九轴融合算法 --基于李群的扩展卡尔曼滤波 极品巧克力 前言 九轴融合算法是指通过融合IMU中的加速度计(三轴).陀螺仪(三轴).磁场计(三轴),来获取物体姿态的方法 ...
- 扩展卡尔曼滤波(MRPT)
EKF relies on a linearisation of the evolution and observation functions which are good approximatio ...
- Sensor fusion(传感器融合)
From Wikipedia, the free encyclopedia 来自维基百科,免费的百科Sensor fusion is combining of sensory data or data ...
- 车载多传感器融合定位方案:GPS +IMU+MM
导读 高德定位业务包括云上定位和端上定位两大模块.其中,云上定位主要解决Wifi指纹库.AGPS定位.轨迹挖掘和聚类等问题:端上定位解决手机端和车机端的实时定位问题.近年来,随着定位业务的发展,用户对 ...
- Sensor Fusion-based Exploration in Home Environments using Information, Driving and Localization Gains(基于传感器融合的使用信息、驾驶和定位增益在家庭环境中的探索)
Authors: Joong-Tae Park, Jae-Bok Song Department:Department of Mechanical Engineering, Korea Un ...
- 突破短板,传统桌面程序 使用webapi 扩展迎合web和移动端融合的需求
传统桌面程序不能完全被web和移动端替代,但是需要改造.这里要说的是巧用webapi把以前用dll和com组件,ocx等方式做接口,做分布式开发的方式,改成restful 风格api的方式实现跨平台, ...
- SLAM+语音机器人DIY系列:(三)感知与大脑——2.带自校准九轴数据融合IMU惯性传感器
摘要 在我的想象中机器人首先应该能自由的走来走去,然后应该能流利的与主人对话.朝着这个理想,我准备设计一个能自由行走,并且可以与人语音对话的机器人.实现的关键是让机器人能通过传感器感知周围环境,并通过 ...
- 卡尔曼滤波—Simple Kalman Filter for 2D tracking with OpenCV
之前有关卡尔曼滤波的例子都比较简单,只能用于简单的理解卡尔曼滤波的基本步骤.现在让我们来看看卡尔曼滤波在实际中到底能做些什么吧.这里有一个使用卡尔曼滤波在窗口内跟踪鼠标移动的例子,原作者主页:http ...
- 四元数运动学笔记(5)IMU驱动的运动误差方程
1.扩展卡尔曼滤波EKF1.1线性化卡尔曼滤波1.2偏差微分方程的推导1.3线性化卡尔曼滤波的流程1.4 离散EKF2.误差状态的运动方程2.1连续时间的IMU系统动态方程2.1.1相关变量2.1.2 ...
随机推荐
- 前端之HTML补充
一.列表 (1).无序列表<ul> <body> <ul type="disc"> <li>属性一</li> <l ...
- 【MFC】对话框自带滚动条的使用
对话框自带滚动条的使用 摘自 http://wenku.baidu.com/link?url=aZe1zgBSBsf9xCYNpcz2fNGljmKxg372OVIGeJ7p6iRCWbbsertS7 ...
- Python基本特殊方法之__format__
__format__()方法 __format__()传参方法:someobject.__format__(specification) specification为指定格式,当应用程序中出现&quo ...
- 5款实用的硬盘、SSD固态硬盘、U盘、储存卡磁盘性能测试工具绿色版
http://www.iplaysoft.com/disk-benchmark-tools.html/comment-page-1#comment-149425
- Python学习-数据运算
在Python中有丰富的算术运算,这使得Python在科学计算领域有着很高的地位,Python可以提供包括四则运算在内的各种算术运算. a = 10 b = 20 print(a-b) #-10 pr ...
- VS 生成 dll、exe 版本号与SVN版本号一致
1.VS 可自动生成版本号 注释掉以下两行代码 [assembly: AssemblyVersion("1.0.0.0")][assembly: AssemblyFileVersi ...
- Maven根据不同的环境打包不同的配置
前言: 在开发过程中,我们的软件会面对不同的运行环境,比如开发环境.测试环境.生产环境,而我们的软件在不同的环境中,有的配置可能会不一样,比如数据源配置.日志文件配置等等. 那么就需要借助maven提 ...
- openfaas k8s 集成
备注 k8s 1.6 以下版本与k8s 1.6 以上版本会有一些简单的区别 1. 克隆k8s 部署文档 https://github.com/openfaas/faas-netes 2. 创 ...
- [转]Mac上的抓包工具Charles
$*********************************************************************************************$ 博主推荐 ...
- php跨域问题
http://www.cnblogs.com/xiezn/p/5651093.html