无损卡尔曼滤波UKF与多传感器融合
非线性系统状态估计是一大难点。KF(Kalman Filter)只适用于线性系统。EKF(Extended Kalman Filter)利用泰勒展开将非线性系统线性化。可是,EKF在强非线性系统下的误差很大。本文将介绍一种新型的滤波算法UKF(Unscented Kalman Filter),其计算精度相比EKF更高并省略了Jacobian矩阵的计算。
Why UKF
本博客在之前两篇介绍了KF和EKF。那么,为什么还需要UKF呢,原因见下表:
模型 | 缺点 | UKF对缺点改进 |
---|---|---|
KF | 只适用于线性系统 | 适用于非线性系统 |
EKF | 线性化忽略了高阶项导致强非线性系统误差大;线性化处理需要计算Jacobian矩阵 | 对非线性的概率分布近似,没有线性化忽略高阶项; 不需要计算Jacobian矩阵 |
UKF简述
原理概述
首先,回顾下UKF需要解决的问题,已知系统的状态及其方差xk,Pkx_k,P_k。如果经过非线性函数xk+1=f(xk)x_{k+1} = f(x_k)后,新的状态和方差如何求解。
EKF提供的方法是将非线性函数f(x)f(x)作泰勒一阶展开,利用Jacobian矩阵HjH_j近似将f(x)f(x)线性化为HjxH_j x。这种方法一方面在强非线性系统下误差大,另一方面Jacobian矩阵的计算着实令人头疼。
UKF认为每一个状态xk,Pkx_k,P_k都可以用几个Sigma点(关键点)XsigX_{sig}表示。当作用于非线性函数f(x)f(x)时,只需要将Sigma点XsigX_{sig}作用于非线性函数f(x)f(x)得到f(Xsig)f(X_{sig})即可。通过得到的f(Xsig)f(X_{sig})可以计算新的状态xk+1,Pk+1x_{k+1},P_{k+1}。
通过上面的介绍,我们知道UKF只是将非线性函数映射通过关键点映射来实现,那么出现几个问题:
- 关键点怎么找
- 找到关键点后如何求出新的状态xk+1,Pk+1x_{k+1},P_{k+1}
关键点怎么找
关键点的意义在于能够充分刻画原状态的分布情况,其经验公式如下图所示,需要注意的是:
- nxn_x代表xk|kx_{k|k}的大小
- λ\lambda代表关键点的散开情况,一般采用经验值λ=3−nx\lambda=3-n_x
找到关键点后如何求出新的状态
新状态的求解公式如下图所示,需要注意的是:
- Xk+1|kX_{k+1|k}代表Sigma点集合,Xk+1|k,iX_{k+1|k,i}代表Sigma点集合中的第ii个点
- nan_a代表xk+1|kx_{k+1|k}增广后的大小
- nσn_{\sigma}代表Sigma点集合的大小,一般nσ=1+2nan_{\sigma} = 1+2n_a
- 权重wiw_i在i==0i==0时w0=λλ+naw_0=\frac{\lambda}{\lambda+n_a},在i!=0i!=0时w0=12(λ+na)w_0=\frac{1}{2 (\lambda+n_a)}
多传感器融合
下面,将通过lidar、radar跟踪小车的例子,讲解UKF如何应用于小车状态跟踪。相关传感器信息及大体步骤可见扩展卡尔曼滤波EKF与多传感器融合。
CTRV模型
EKF文章中使用了CV(constant velocity)模型,本文将使用CTRV(constant turn rate and velocity magnitude)模型。其状态变量如下图所示。
因假定turn rate、velocity不变,其Process noise包含加速度与角加速度为:
\nu_k = \begin{bmatrix} \nu_{a,k} \\ \nu_{\ddot{\psi},k} \end{bmatrix}
利用x˙\dot{x}及其对时间的积分xk+1=∫x˙dtx_{k+1}=\int \dot{x} dt可得Process模型为:
x_{k+1}=x_k+\begin{bmatrix} \frac{v_k}{\dot{\psi_k}} (sin(\psi_k+\dot{\psi_k} \Delta t)-sin(\psi_k)) \\ \frac{v_k}{\dot{\psi_k}} (-cos(\psi_k+\dot{\psi_k} \Delta t)+cos(\psi_k)) \\ 0 \\ \dot{\psi_k} \Delta t \\ 0 \end{bmatrix}
考虑Process noise为:
x_{k+1}=x_k+\begin{bmatrix} \frac{v_k}{\dot{\psi_k}} (sin(\psi_k+\dot{\psi_k} \Delta t)-sin(\psi_k)) \\ \frac{v_k}{\dot{\psi_k}} (-cos(\psi_k+\dot{\psi_k} \Delta t)+cos(\psi_k)) \\ 0 \\ \dot{\psi_k} \Delta t \\ 0 \end{bmatrix} + \begin{bmatrix} \frac{1}{2} \nu_{a,k} \cos(\psi_k) \Delta t^2 \\ \frac{1}{2} \nu_{a,k} \sin(\psi_k) \Delta t^2 \\ \nu_{a,k} \Delta t \\ \frac{1}{2} \nu_{\ddot{\psi},k}\Delta t^2 \\ \nu_{\ddot{\psi},k} \Delta t \end{bmatrix}
RoadMap
UKF的RoadMap如上图所示,核心思想在前部分已介绍过,其算法是:
- 初始化系统状态xk,Pkx_k, P_k
- 根据状态xk,Pkx_k, P_k生成Sigma点XkX_k
- 根据process model预测未来的Sigma点Xk+1|kX_{k+1|k}
- 根据预测的Sigma点Xk+1|kX_{k+1|k}生成状态预测xk+1|k,Pk+1|kx_{k+1|k}, P_{k+1|k}
- 当测量值到来时,将预测的Sigma点Xk+1|kX_{k+1|k}转换成预测测量值Zk+1|kZ_{k+1|k}
- 根据预测测量值Zk+1|kZ_{k+1|k}与真实测量值zk+1z_{k+1}的差值更新得到系统状态xk+1|k+1,Pk+1|k+1x_{k+1|k+1}, P_{k+1|k+1}
同时,有几个部分需要强调下。
- 数据增广
- Update State
- Noise Level与NIS
数据增广
如上图,在process预测时需要对xkx_k进行增广,原因是process模型中包含了噪声的非线性关系f(xk,νk)f(x_k,\nu_k)。反之,在measurement model中因为噪声是线性关系的所以不需要进行数据增广。
增广后x,Px,P变化如下,
x = \begin{bmatrix} p_x \\ p_y \\ v \\ \psi \\ \dot{\psi} \end{bmatrix} \Rightarrow x_a= \begin{bmatrix} p_x \\ p_y \\ v \\ \psi \\ \dot{\psi} \\ \nu_a \\ \nu_{\ddot\psi} \end{bmatrix}
P_a=\begin{bmatrix} P & 0\\ 0 & Q \end{bmatrix} Q=\begin{bmatrix} \nu_a^2 & 0\\ 0 & \nu_{\ddot{\psi}}^2 \end{bmatrix}
Update State
State的更新公式如下图所示:
Noise Level与NIS
UKF中牵涉的噪音有两类:
- Process Noise:νa,νψ¨\nu_a,\nu_{\ddot{\psi}},需要自己设定
- 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
无损卡尔曼滤波UKF与多传感器融合相关推荐
- 扩展卡尔曼滤波EKF与多传感器融合
Extended Kalman Filter(扩展卡尔曼滤波)是卡尔曼滤波的非线性版本.在状态转移方程确定的情况下,EKF已经成为了非线性系统状态估计的事实标准.本文将简要介绍EKF,并介绍其在无人驾 ...
- 无人驾驶技术——无损卡尔曼滤波(UKF)
文章目录 无损卡尔曼滤波(UKF) UKF Predict 1.选择预测点sigma点 sigma点计算代码实现 添加过程噪声处理 构建噪声扩充矩阵 c++ 2.预测sigma点 预测sigma点方法 ...
- 解读基于多传感器融合的卡尔曼滤波算法
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 本文转自|3D视觉工坊 卡尔曼滤波器是传感器融合工程师用于自动驾驶 ...
- 组合导航+多传感器融合算法
1.INS/GPS的组合导航系统可以输出高频率的导航参数信息(位置.速度.姿态),并且在长.短期的导航过程中均能具备较高精度.(输出数据的意义什么,PDR是步长+航向) 2.误差反馈系统 (1)在开环 ...
- 无迹卡尔曼滤波UKF—目标跟踪中的应用(算法部分)
无迹卡尔曼滤波UKF-目标跟踪中的应用(算法部分) 原创不易,路过的各位大佬请点个赞 机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313 仿真部分见博客: [无迹卡 ...
- 无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)
无迹卡尔曼滤波UKF-目标跟踪中的应用(仿真部分) 原创不易,路过的各位大佬请点个赞 机动目标跟踪/非线性滤波/传感器融合/导航等探讨联系WX: ZB823618313 算法部分见博客: [无迹卡尔曼 ...
- 领域最全!多传感器融合方法综述!(Camera/Lidar/Radar等多源异构数据)
点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取 点击进入→自动驾驶之心技术交流群 后台回复[ECCV2022]获取ECCV2022所有自动驾驶方向论文! 自动 ...
- 卡尔曼滤波系列——(四)无损卡尔曼滤波
1 简介 无损卡尔曼滤波又称无迹卡尔曼滤波(Unscented Kalman Filter,UKF),是无损变换(Unscented Transform,UT)与标准卡尔曼滤波体系的结合,通过UT变换 ...
- 无人驾驶4: 无损卡尔曼滤波
在上一节的扩展卡尔曼滤波跟踪系统中,有两个缺陷: 系统采用恒速模型:假定行人沿直线运动:实际应用中,出现曲线运动时,预估不够准确. 每次测量都需要计算雅可比矩阵,很耗资源. 当问题一旦变得复杂,预测和 ...
最新文章
- BIO 三位标注 (B-begin,I-inside,O-outside)
- Java高并发编程(二):Java并发机制的底层实现机制
- JPA Hibernate应用实例
- Spark常规性能调优三:并行度调节
- Java集合框架:Arrays工具类
- android的rgb转bitmap,Android: 格式为RGB_565的bitmap问题
- 潜力的监控mysql_Grafana 数据库监控平台
- 数据结构一【树状数组】普通、二维、离线树状数组的(单点修改,单点查询,区间修改,区间查询)模板及应用例题总结
- POJ1207-The 3n + 1 problem
- java listutils_Java的list自定义工具类ListUtils
- 官宣!“一流大学”,异地落户!
- sklearn 常用模块及类与方法
- python的符号函数得到的数字类型_python-1:Number数字类型 之二 相关函数 int.from_bytes,int.to_bytes()...
- python类为什么要初始化_python3类对象需要在init中初始化吗?
- ios开发之 -- 强制横屏
- android office转pdf插件,office另存为pdf插件下载
- Unity3d 局域网小游戏DEMO学习
- 浙大PAT 1051
- 「excel小技巧」一秒快速求和多行数值
- 对ADMEMS架构设计方法论的一点随笔
热门文章
- 彻底搞懂基于LOAM框架的3D激光SLAM全套学习资料汇总!
- CurcveLane-NAS:华为中大提出一种结合NAS的曲线车道检测算法
- mysql 存储过程out,in,inout分别表示什么
- php传递数据给javascript
- 在机械狗上利用AstraPro3D深度摄像头简单实现目标跟踪和人体姿态识别
- 基于生成式深度学习方法设计潜在2019-nCoV蛋白酶抑制剂
- FEMS综述: 如何从微生物网络中的“毛线球”理出头绪(3万字长文带你系统学习网络)...
- 16S+功能预测发Sciences:尸体降解过程中的微生物组
- Fertility of Soils:根系C/P计量比影响水稻残根周际酶活的时空动态分布特征
- 哈佛牙学院博士后:教你口腔保健基本功之牙线篇