扩展卡尔曼滤波python_扩展卡尔曼滤波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。
扩展卡尔曼滤波python_扩展卡尔曼滤波EKF与多传感器融合相关推荐
- 扩展卡尔曼滤波EKF与多传感器融合
Extended Kalman Filter(扩展卡尔曼滤波)是卡尔曼滤波的非线性版本.在状态转移方程确定的情况下,EKF已经成为了非线性系统状态估计的事实标准.本文将简要介绍EKF,并介绍其在无人驾 ...
- 扩展卡尔曼滤波python_扩展卡尔曼滤波（MRPT）
#include #include#include#include#include using namespacemrpt;using namespacemrpt::bayes;using names ...
- 无损卡尔曼滤波UKF与多传感器融合
非线性系统状态估计是一大难点.KF(Kalman Filter)只适用于线性系统.EKF(Extended Kalman Filter)利用泰勒展开将非线性系统线性化.可是,EKF在强非线性系统下的误 ...
- 行驶车辆状态估计，无迹卡尔曼滤波，扩展卡尔曼滤波（EKF UKF）
行驶车辆状态估计,无迹卡尔曼滤波,扩展卡尔曼滤波(EKF UKF) 软件使用:Matlab Simulink 适用场景:采用扩展卡尔曼滤波和无迹卡尔曼滤波EKF UKF进行行驶车辆的"车速, ...
- 卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理
所有滤波问题其实都是求感兴趣的状态的后验概率分布,只是由于针对特定条件的不同,可通过求解递推贝叶斯公式获得后验概率的解析解(KF.EKF.UKF),也可通过大数统计平均求期望的方法来获得后验概率(PF ...
- 使用卡尔曼滤波和扩展卡尔曼滤波进行毫米波雷达和激光雷达数据融合示例
前言 在<无人驾驶技术入门(十三)| 手把手教你写卡尔曼滤波器>的分享中,我以激光雷达的数据为例介绍了卡尔曼滤波器(KF)的七个公式,并用C++代码实现了激光雷达障碍物的跟踪问题:在< ...
- 【概率机器人】3.1 卡尔曼滤波、扩展卡尔曼滤波和无迹卡尔曼滤波
这一章将介绍卡尔曼滤波.扩展卡尔曼滤波以及无迹卡尔曼滤波,并从贝叶斯滤波的角度来进行分析并完成数学推导.如果您对贝叶斯滤波不了解,可以查阅相关书籍或阅读 [概率机器人 2 递归状态估计]. 这三种滤波 ...
- 【camera-lidar-radar】基于卡尔曼滤波和扩展卡尔曼滤波的相机、激光雷达、毫米波雷达多传感器后融合
[camera-lidar-radar]基于卡尔曼滤波和扩展卡尔曼滤波的相机.激光雷达.毫米波雷达多传感器后融合 代码下载地址(C++ and Python):下载地址 红点和蓝点分别表示radar和 ...
- 《最优状态估计-卡尔曼，H∞及非线性滤波》：第7章 卡尔曼滤波的扩展
<最优状态估计-卡尔曼,H∞及非线性滤波>:第7章 卡尔曼滤波的扩展 前言 1. MATLAB仿真:示例7.1 2. MATLAB仿真:示例7.2 3. MATLAB仿真:示例7.12 4 ...
- 卡尔曼滤波算法原理(KF,EKF,AKF,UKF)
卡尔曼滤波算法原理(KF,EKF,AKF,UKF) 主要是KF.EKF.UKF算法公式推导,直接看公式会比较枯燥,建议推导一下. 新增文章卡尔曼运动模型公式推导CTRV+CTRA,主要是EKF的CTR ...
最新文章
- Java 8 一行代码解决了空指针问题，太厉害了...
- jvm六:主动使用(1.new一个对象, 2.反射)
- java ef 引用问题_java调用shell(ps -ef | grep )问题
- 重命名数据库解决“无法用排他锁锁定该数据库”
- [HNOI2002]营业额统计 Splay tree入门题
- UVA1200 LA2972 POJ2295 ZOJ2492 A DP Problem【输入】
- 准确率(Precision)、召回率(Recall)、F值(F-Measure)、ROC、AUC
- 基于javaweb的旅游管理系统旅行平台(springboot+ssm)
- Selenium+Java自动化测试系列教程❤
- 基于FPGA的中值滤波算法的实现
- Apache ECharts教程
- 类对象模型实验：类数据成员大小
- SVG之线条动画相关
- 性能测试CPU占用率太高如果解决？
- 肥猫学习日记---数据结构与算法（三）-----链表
- python——spilt和strip用法
- 每日阅读：你如何过一天，你就如何过一生
- 生物特征识别门禁系统分类和基本概念
- B/S结构和C/S结构详细介绍
- 优酷视频怎么转二维码_优酷视频转二维码