参考:https://blog.csdn.net/young_gy/article/details/78468153

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。

扩展卡尔曼滤波EKF与多传感器融合的更多相关文章

  1. Google Cardboard的九轴融合算法——基于李群的扩展卡尔曼滤波

    Google Cardboard的九轴融合算法 --基于李群的扩展卡尔曼滤波 极品巧克力 前言 九轴融合算法是指通过融合IMU中的加速度计(三轴).陀螺仪(三轴).磁场计(三轴),来获取物体姿态的方法 ...

  2. 扩展卡尔曼滤波(MRPT)

    EKF relies on a linearisation of the evolution and observation functions which are good approximatio ...

  3. Sensor fusion(传感器融合)

    From Wikipedia, the free encyclopedia 来自维基百科,免费的百科Sensor fusion is combining of sensory data or data ...

  4. 车载多传感器融合定位方案:GPS +IMU+MM

    导读 高德定位业务包括云上定位和端上定位两大模块.其中,云上定位主要解决Wifi指纹库.AGPS定位.轨迹挖掘和聚类等问题:端上定位解决手机端和车机端的实时定位问题.近年来,随着定位业务的发展,用户对 ...

  5. 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 ...

  6. 突破短板,传统桌面程序 使用webapi 扩展迎合web和移动端融合的需求

    传统桌面程序不能完全被web和移动端替代,但是需要改造.这里要说的是巧用webapi把以前用dll和com组件,ocx等方式做接口,做分布式开发的方式,改成restful 风格api的方式实现跨平台, ...

  7. SLAM+语音机器人DIY系列:(三)感知与大脑——2.带自校准九轴数据融合IMU惯性传感器

    摘要 在我的想象中机器人首先应该能自由的走来走去,然后应该能流利的与主人对话.朝着这个理想,我准备设计一个能自由行走,并且可以与人语音对话的机器人.实现的关键是让机器人能通过传感器感知周围环境,并通过 ...

  8. 卡尔曼滤波—Simple Kalman Filter for 2D tracking with OpenCV

    之前有关卡尔曼滤波的例子都比较简单,只能用于简单的理解卡尔曼滤波的基本步骤.现在让我们来看看卡尔曼滤波在实际中到底能做些什么吧.这里有一个使用卡尔曼滤波在窗口内跟踪鼠标移动的例子,原作者主页:http ...

  9. 四元数运动学笔记(5)IMU驱动的运动误差方程

    1.扩展卡尔曼滤波EKF1.1线性化卡尔曼滤波1.2偏差微分方程的推导1.3线性化卡尔曼滤波的流程1.4 离散EKF2.误差状态的运动方程2.1连续时间的IMU系统动态方程2.1.1相关变量2.1.2 ...

随机推荐

  1. 前端之HTML补充

    一.列表 (1).无序列表<ul> <body> <ul type="disc"> <li>属性一</li> <l ...

  2. 【MFC】对话框自带滚动条的使用

    对话框自带滚动条的使用 摘自 http://wenku.baidu.com/link?url=aZe1zgBSBsf9xCYNpcz2fNGljmKxg372OVIGeJ7p6iRCWbbsertS7 ...

  3. Python基本特殊方法之__format__

    __format__()方法 __format__()传参方法:someobject.__format__(specification) specification为指定格式,当应用程序中出现&quo ...

  4. 5款实用的硬盘、SSD固态硬盘、U盘、储存卡磁盘性能测试工具绿色版

    http://www.iplaysoft.com/disk-benchmark-tools.html/comment-page-1#comment-149425

  5. Python学习-数据运算

    在Python中有丰富的算术运算,这使得Python在科学计算领域有着很高的地位,Python可以提供包括四则运算在内的各种算术运算. a = 10 b = 20 print(a-b) #-10 pr ...

  6. VS 生成 dll、exe 版本号与SVN版本号一致

    1.VS 可自动生成版本号 注释掉以下两行代码 [assembly: AssemblyVersion("1.0.0.0")][assembly: AssemblyFileVersi ...

  7. Maven根据不同的环境打包不同的配置

    前言: 在开发过程中,我们的软件会面对不同的运行环境,比如开发环境.测试环境.生产环境,而我们的软件在不同的环境中,有的配置可能会不一样,比如数据源配置.日志文件配置等等. 那么就需要借助maven提 ...

  8. openfaas k8s 集成

    备注 k8s 1.6 以下版本与k8s 1.6 以上版本会有一些简单的区别 1. 克隆k8s  部署文档   https://github.com/openfaas/faas-netes   2. 创 ...

  9. [转]Mac上的抓包工具Charles

    $*********************************************************************************************$ 博主推荐 ...

  10. php跨域问题

    http://www.cnblogs.com/xiezn/p/5651093.html