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

原创 2017年11月16日 17:01:22

这里写图片描述

非线性系统状态估计是一大难点。KF(Kalman Filter)只适用于线性系统。EKF(Extended Kalman Filter)利用泰勒展开将非线性系统线性化。可是,EKF在强非线性系统下的误差很大。本文将介绍一种新型的滤波算法UKF(Unscented Kalman Filter),其计算精度相比EKF更高并省略了Jacobian矩阵的计算。

Why UKF

这里写图片描述

本博客在之前两篇介绍了KFEKF。那么,为什么还需要UKF呢,原因见下表:

模型 缺点 UKF对缺点改进
KF 只适用于线性系统 适用于非线性系统
EKF 线性化忽略了高阶项导致强非线性系统误差大;线性化处理需要计算Jacobian矩阵 对非线性的概率分布近似,没有线性化忽略高阶项; 不需要计算Jacobian矩阵

UKF简述

原理概述

首先,回顾下UKF需要解决的问题,已知系统的状态及其方差xk,Pk。如果经过非线性函数xk+1=f(xk)后,新的状态和方差如何求解。

EKF提供的方法是将非线性函数f(x)作泰勒一阶展开,利用Jacobian矩阵Hj近似将f(x)线性化为Hjx。这种方法一方面在强非线性系统下误差大,另一方面Jacobian矩阵的计算着实令人头疼。

UKF认为每一个状态xk,Pk都可以用几个Sigma点(关键点)Xsig表示。当作用于非线性函数f(x)时,只需要将Sigma点Xsig作用于非线性函数f(x)得到f(Xsig)即可。通过得到的f(Xsig)可以计算新的状态xk+1,Pk+1

这里写图片描述

通过上面的介绍,我们知道UKF只是将非线性函数映射通过关键点映射来实现,那么出现几个问题:

  1. 关键点怎么找
  2. 找到关键点后如何求出新的状态xk+1,Pk+1

关键点怎么找

关键点的意义在于能够充分刻画原状态的分布情况,其经验公式如下图所示,需要注意的是:

  • nx代表xk|k的大小
  • λ代表关键点的散开情况,一般采用经验值λ=3nx

这里写图片描述

找到关键点后如何求出新的状态

新状态的求解公式如下图所示,需要注意的是:

  • Xk+1|k代表Sigma点集合,Xk+1|k,i代表Sigma点集合中的第i个点
  • na代表xk+1|k增广后的大小
  • nσ代表Sigma点集合的大小,一般nσ=1+2na
  • 权重wii==0w0=λλ+na,在i!=0w0=12(λ+na)

这里写图片描述

多传感器融合

下面,将通过lidar、radar跟踪小车的例子,讲解UKF如何应用于小车状态跟踪。相关传感器信息及大体步骤可见扩展卡尔曼滤波EKF与多传感器融合

CTRV模型

EKF文章中使用了CV(constant velocity)模型,本文将使用CTRV(constant turn rate and velocity magnitude)模型。其状态变量如下图所示。

这里写图片描述

因假定turn rate、velocity不变,其Process noise包含加速度与角加速度为:

νk=[νa,kνψ¨,k]

利用x˙及其对时间的积分xk+1=x˙dt可得Process模型为:

xk+1=xk+vkψk˙(sin(ψk+ψk˙Δt)sin(ψk))vkψk˙(cos(ψk+ψk˙Δt)+cos(ψk))0ψk˙Δt0

考虑Process noise为:

xk+1=xk+vkψk˙(sin(ψk+ψk˙Δt)sin(ψk))vkψk˙(cos(ψk+ψk˙Δt)+cos(ψk))0ψk˙Δt0+12νa,kcos(ψk)Δt212νa,ksin(ψk)Δt2νa,kΔt12νψ¨,kΔt2νψ¨,kΔt

RoadMap

这里写图片描述

UKF的RoadMap如上图所示,核心思想在前部分已介绍过,其算法是:

  1. 初始化系统状态xk,Pk
  2. 根据状态xk,Pk生成Sigma点Xk
  3. 根据process model预测未来的Sigma点Xk+1|k
  4. 根据预测的Sigma点Xk+1|k生成状态预测xk+1|k,Pk+1|k
  5. 当测量值到来时,将预测的Sigma点Xk+1|k转换成预测测量值Zk+1|k
  6. 根据预测测量值Zk+1|k与真实测量值zk+1的差值更新得到系统状态xk+1|k+1,Pk+1|k+1

同时,有几个部分需要强调下。

  1. 数据增广
  2. Update State
  3. Noise Level与NIS

数据增广

这里写图片描述

如上图,在process预测时需要对xk进行增广,原因是process模型中包含了噪声的非线性关系f(xk,νk)。反之,在measurement model中因为噪声是线性关系的所以不需要进行数据增广。

增广后xP变化如下,

x=pxpyvψψ˙xa=pxpyvψψ˙νaνψ¨

Pa=[P00Q]Q=[ν2a00ν2ψ¨]

Update State

State的更新公式如下图所示:

这里写图片描述
这里写图片描述

Noise Level与NIS

UKF中牵涉的噪音有两类:

  • Process Noise:νa,νψ¨,需要自己设定
  • Measurement Noise:lidar、radar的噪音水平,由厂家提供

自己设定调整的方法有NIS,NIS的分布服从chi-square分布,调整合适的噪音水平使其符合规定的chi-square分布即可。

这里写图片描述
这里写图片描述
这里写图片描述

示例

本文采用与扩展卡尔曼滤波EKF与多传感器融合相同的数据集,结果如下。

NIS验证结果如下:

这里写图片描述

总体跟踪情况如下:

这里写图片描述

UKF与EKF的RMSE对比如下,UKF明显占优:

方法 X Y VX VY
EKF 0.0973 0.0855 0.4513 0.4399
UKF 0.0661 0.0827 0.3323 0.2146

相关代码可参考:YoungGer的Github

Unscented kalman Filtering 无损卡尔曼滤波的matlab程序

本来想写下原理的,CSDN的博客貌似不可以编辑公式,那就算了,直接放程序好了。 这个程序大部分是从前人那里看来的,向他们表示感谢,它帮助我完成了一个课程论文的仿真。我很喜欢原作者简洁有效的编程风格。...
  • qq514218063
  • qq514218063
  • 2014年02月07日 22:52
  • 3402

卡尔曼滤波_1

wiki原文卡尔曼滤波(2016.8.4)卡尔曼滤波,也常被称作线性二次估计(LQE),是一种使用一段时间内的观测数据,其中观测数据中包含统计噪声和其他不确定性,来估计未知变量的值的方法。它比一般的基...
  • u010480899
  • u010480899
  • 2017年02月16日 20:59
  • 1711

ukf(无迹卡尔曼滤波)算法的matlab程序.

 ukf(无迹卡尔曼滤波)算法的matlab程序. function [x,P]=ukf(fstate,x,P,hmeas,z,Q,R) % UKF   Unscented Kalman F...
  • ss19890125
  • ss19890125
  • 2014年06月18日 14:34
  • 6542

《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.3UKF

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 程序说明:对比UKF与EKF在非线性系统中应用...
  • sinat_20265495
  • sinat_20265495
  • 2016年11月23日 16:29
  • 2157

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

Extended Kalman Filter(扩展卡尔曼滤波)是卡尔曼滤波的非线性版本。在状态转移方程确定的情况下,EKF已经成为了非线性系统状态估计的事实标准。本文将简要介绍EKF,并介绍其在无人驾...
  • Young_Gy
  • Young_Gy
  • 2017年11月07日 17:49
  • 486

SLAM笔记六——Unscented Kalman Filter

卡尔曼滤波都需要线性模型,EKF用的是泰勒公式进行局部线性的方法,而UKF提供了另一种线性化的方法。Unscented Transform步骤: 首先选择一组点,称为sigma点 然后通过...
  • qq_30159351
  • qq_30159351
  • 2016年12月01日 10:51
  • 1030

Google开源SLAM软件cartographer中使用的UKF滤波器解析

转自:http://www.cnblogs.com/location-sensing/p/6208557.html 在Google开源SLAM软件cartographer中,相对《SLAM ...
  • hjwang1
  • hjwang1
  • 2017年03月19日 17:27
  • 758

无人驾驶汽车系统入门(三)——无损卡尔曼滤波,目标追踪,C++

前面两篇文章我们了解了卡尔曼滤波以及扩展卡尔曼滤波在目标追踪的应用,我们在上一篇文章中还具体用Python实现了EKF,但是细心的同学会发现,EKF的效率确实很低,计算雅可比矩阵确实是一个很费时的操作...
  • AdamShan
  • AdamShan
  • 2017年10月26日 21:37
  • 1195

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

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

无损卡尔曼滤波UKF 和 多传感器数据融合

非线性系统状态估计是一大难点。KF(Kalman Filter)只适用于线性系统。EKF(Extended Kalman Filter)利用泰勒展开将非...
  • datase
  • datase
  • 2018年02月23日 17:15
  • 6
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:无损卡尔曼滤波UKF与多传感器融合
举报原因:
原因补充:

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