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

原创 2017年11月07日 17:49:10

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

这里写图片描述

KF与EKF

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

KF与EKF的区别如下:

  1. 预测未来:x=Fx+ux=f(x,u)代替;其余FFj代替。
  2. 修正当下:将状态映射到测量的Hxh(x)代替;其余HHj代替。

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

FjHj计算方式如下:

Fjb=f(x,u)x=h(x)x

这里写图片描述

为什么要用EKF

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

将非线性系统线性化

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

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

f(x)f(μ)+f(μ)x(xμ)

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

T(x)f(a)+(xa)TDf(a)

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

多传感器融合

lidar与radar

本文将以汽车跟踪为例,目标是知道汽车时刻的状态x=(px,py,vx,vy)。已知的传感器有lidar、radar。

  • lidar:笛卡尔坐标系。可检测到位置,没有速度信息。其测量值z=(px,py)
  • radar:极坐标系。可检测到距离,角度,速度信息,但是精度较低。其测量值z=(ρ,ϕ,ρ˙),图示如下。

这里写图片描述

传感器融合步骤

这里写图片描述

步骤图如上所示,包括:

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

初始化

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

对于radar来说,

pxpyvxvy=10000100[pxpy]

对于radar来说,

pxpyvxvy=ρcosϕρsinϕρ˙cosϕρ˙sinϕ

预测未来

预测主要涉及的公式是:

xP=Fx=FPFT+Q

需要求解的有三个变量:FPQ


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

Fx=10000100dt0100dt01pxpyvxvy


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

P=1000010000100000001000


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

v=axdt22aydt22axdtaydt=dt220dt00dt220dt[axay]=Ga

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

Q=E[vvT]=E[GaaTGT]=GE[aaT]GT=G[σ2ax00σ2ay]GT=dt44σ2ax0dt32σ2ax00dt44σ2ay0dt32σ2aydt32σ2ax0dt2σ2ax00dt32σ2ay0dt2σ2ay

修正当下

lidar

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

ySKxP=zHx=HPHT+R=PHTS1=x+Ky=(IKH)P

需要求解的有两个变量:HR


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

Hx=[10010000]pxpyvxvy


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

Rlaser=[0.0225000.0225]

radar

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

ySKxP=zf(x)=HjPHTj+R=PHTjS1=x+Ky=(IKHj)P

区别与上面lidar的主要有:

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

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

f(x)=ρϕρ˙=p2x+p2yarctanpypxpxvx+pyvyp2x+p2y


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

Hj=f(x)x=ρpxϕpxρ˙pxρpyϕpyρ˙pyρvxϕvxρ˙vxρvyϕvyρ˙vy


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

Rlaser=0.090000.00090000.09

传感器融合实例

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

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

这里写图片描述

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

这里写图片描述

无损卡尔曼滤波UKF与多传感器融合

非线性系统状态估计是一大难点。KF(Kalman Filter)只适用于线性系统。EKF(Extended Kalman Filter)利用泰勒展开将非线性系统线性化。可是,EKF在强非线性系统下的误...
  • Young_Gy
  • Young_Gy
  • 2017年11月16日 17:01
  • 564

卡尔曼滤波(KF)与扩展卡尔曼滤波(EKF)的一种理解思路及相应推导(1)

前言: 从上个世纪卡尔曼滤波理论被提出,卡尔曼滤波在控制论与信息论的连接上做出了卓越的贡献。为了得出准确的下一时刻状态真值,我们常常使用卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波等等方法,这...
  • qq_18163961
  • qq_18163961
  • 2016年09月11日 19:22
  • 12342

卡尔曼滤波原理二:扩展卡尔曼

1、理论部分       上一篇介绍了线性卡尔曼滤波器,当系统为线性高斯模型时,滤波器能给出最优的估计,但是实际系统总是存在不同程度的非线性,如平方、三角关系、开方等。对于非线性系统,可以采用的一种...
  • u012936940
  • u012936940
  • 2017年08月16日 16:18
  • 3536

卡尔曼/扩展卡尔曼滤波器的原理及应用

卡尔曼滤波器的原理及应用 应用前提算法详细介绍应用举例下一步 原文地址:http://blog.csdn.net/lizilpl/article/details/45289541 1.应...
  • zhangmh2011
  • zhangmh2011
  • 2016年12月08日 10:12
  • 7397

卡尔曼滤波器学习笔记(二)

扩展卡尔曼滤波器的原理及应用经典的卡尔曼滤波只适用于线性且满足高斯分布的系统,但实际工程中并不是这么简单,比如飞行器在水平运动时有可能伴随着自身的自旋,此时的系统并不是线性的,这时就需要应用扩展卡尔曼...
  • lizilpl
  • lizilpl
  • 2015年04月26日 16:55
  • 33879

初学者的卡尔曼滤波——扩展卡尔曼滤波(一)

简介  已经历经了半个世纪的卡尔曼滤波至今仍然是研究的热点,相关的文章不断被发表。其中许多文章是关于卡尔曼滤波器的新应用,但也不乏改善和扩展滤波器算法的研究。而对算法的研究多着重于将卡尔曼滤波应用于非...
  • gzj2013
  • gzj2013
  • 2017年05月10日 10:59
  • 1089

开源飞控OpenPilot的扩展卡尔曼滤波EKF学习笔记(二)

接着说,往下看filter(stateFilter *self, stateEstimation *state)函数,filter函数执行具体的滤波功能,从程序的架构上可以看出,init函数只执行一次...
  • romepop1
  • romepop1
  • 2014年10月18日 16:01
  • 4296

扩展卡尔曼滤波(EKF)

首先进行文档下载 仔细阅读文档,理解文档中所述内容。 我对文档的matlab代码进行了简单调整如下: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%...
  • u010703122
  • u010703122
  • 2015年10月12日 15:33
  • 7255

卡尔曼滤波器、扩展卡尔曼滤波器、无向卡尔曼滤波器的详细推导

这段时间做轴承故障诊断和预测的时候,需要一个针对已经获取了特征向量的工具来对轴承故障状态进行估计和预测。卡尔曼滤波器可以实现对过去、当前和未来目标位置的估计,所以想通过卡尔曼滤波器的设计思路找到一些灵...
  • u013102281
  • u013102281
  • 2017年03月01日 15:49
  • 1951

扩展卡尔曼滤波的实现

扩展卡尔曼滤波(Extended Kalman Filter )与KF的最大的不同,是允许系统模型和测量模型非线性的存在,它的实现较为简单,参照Wikipedia,我把代码贴出来,方便学习交流。 ...
  • todayq
  • todayq
  • 2014年12月02日 14:45
  • 1761
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:扩展卡尔曼滤波EKF与多传感器融合
举报原因:
原因补充:

(最多只允许输入30个字)