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

扩展卡尔曼滤波EKF与多传感器融合ExtendedKalmanFilter(扩展卡尔曼滤波)是卡尔曼滤波的非线性版本。在状态转移方程确定的情况下,EKF已经成为了非线性系统状态估计的事实标准。本文将简要介绍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) 用非线性得到了更精准的状态预测值、映射后的测量值;线性变换 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。

这里写图片描述

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。

发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/149252.html原文链接:https://javaforall.cn

【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛

【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...

(0)
blank

相关推荐

  • 期货软件开发与平台搭建注意事项是什么_手机期货程序化交易软件

    期货软件开发与平台搭建注意事项是什么_手机期货程序化交易软件期货软件开发和期货平台搭建需要注意很多内容,关系到后期运营的是否正常稳定。现在市面上的很多的期货交易系统软件平台,基本都支持支持PC、安卓APP端,微信端、且具备风控系统、杠杆系统、交易系统、在线出入金、后台管理系统、代理系统、股票数据行情等功能。但是行业鱼龙混杂,并不是每一家开发公司都是靠谱的。加wx:“Zhangyoukeji001”发送相关演示版与报价!  作为投资者,要想拥有一个可靠的期货交易系统,需要注意以下几点:前期对期货系统软件的功能规划——针对期货系统软件,要有具体的规划方案,需

  • 基于SSM框架的毕业设计管理系统的设计与实现(附资源下载)

    基于SSM框架的毕业设计管理系统的设计与实现(附资源下载)我的毕业设计就叫毕业设计管理系统的设计与实现,哈哈目录1.摘要2.需求分析3.数据库设计4.功能模块的设计与实现5.项目截图6.项目源码1.摘要毕业设计是本科教育的最后一个环节,整个过程包括课题拟定与审核、师生双选、开题报告、文档审核、中期检查、毕业答辩等多个环节,是一项系统而复杂的工作。开发一个适合本校的毕业设计管理系统,不仅能够极大地减少教务管理人员的…

  • 常见电容的种类_电容工作原理

    常见电容的种类_电容工作原理一、瓷介电容器(CC)1.结构用陶瓷材料作介质,在陶瓷表面涂覆一层金属(银)薄膜,再经高温烧结后作为电极而成。瓷介电容器又分1类电介质(NPO、CCG));2类电介质(X7R、2X1)和3类电介质(Y5V、2F4)瓷介电容器。2.特点1类瓷介电容器具有温度系数小、稳定性高、损耗低、耐压高等优点。最大容量不超过1000pF,常用的有CC1、CC2、CC18A、CC11、…

  • redis在c#中的应用_redis写入数据的同时是否可以读

    redis在c#中的应用_redis写入数据的同时是否可以读一RedisClient存入缓存:(1)redis.exe执行代码和结果127.0.0.1:6379>setToken”4b366348-da6a-4716-84f6-e3af656943ff”OK(2)c#执行RedisClientclient=newRedisClient(“127.0.0.1”,6379);UserInfouserInfo=newUserInfo();userInfo.id=1;

    2022年10月11日
  • 《JavaScript 模式》读书笔记(2)— 基本技巧3

    这是基本技巧的最后一篇内容,这篇内容示例代码并不多。主要是概念比较多一点。编码约定确定并一致遵循约定比这个具体约定是什么更为重要。一、缩进无论是使用tab还是空格,只要是一致遵循的,是什么并不

  • java序列化和反序列化以及序列化ID的作用分析

    java序列化和反序列化以及序列化ID的作用分析java序列化和反序列化以及序列化ID的作用分析

发表回复

您的电子邮箱地址不会被公开。

关注全栈程序员社区公众号