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

非线性系统状态估计是一大难点。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,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只是将非线性函数映射通过关键点映射来实现,那么出现几个问题:

  1. 关键点怎么找
  2. 找到关键点后如何求出新的状态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包含加速度与角加速度为:

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

\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模型为:

xk+1=xk+⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢vkψk˙(sin(ψk+ψk˙Δt)−sin(ψk))vkψk˙(−cos(ψk+ψk˙Δt)+cos(ψk))0ψk˙Δt0⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥

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为:

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⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥

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如上图所示,核心思想在前部分已介绍过,其算法是:

  1. 初始化系统状态xk,Pkx_k, P_k
  2. 根据状态xk,Pkx_k, P_k生成Sigma点XkX_k
  3. 根据process model预测未来的Sigma点Xk+1|kX_{k+1|k}
  4. 根据预测的Sigma点Xk+1|kX_{k+1|k}生成状态预测xk+1|k,Pk+1|kx_{k+1|k}, P_{k+1|k}
  5. 当测量值到来时,将预测的Sigma点Xk+1|kX_{k+1|k}转换成预测测量值Zk+1|kZ_{k+1|k}
  6. 根据预测测量值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}

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

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

数据增广

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

增广后x,Px,P变化如下,

x=⎡⎣⎢⎢⎢⎢⎢⎢pxpyvψψ˙⎤⎦⎥⎥⎥⎥⎥⎥⇒xa=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢pxpyvψψ˙νaνψ¨⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥

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}

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

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

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

发表于

我来说两句

0 条评论
登录 后参与评论

相关文章

来自专栏梦里茶室

Google机器学习教程心得(三) 好的feature

什么造就好的Feature 这里举了一个对两种狗狗做分类的问题介绍好的Feature应有的特性 简化问题 好的feature能有力地说明两个类别的不同 ...

2077
来自专栏SIGAI学习与实践平台

大话AdaBoost算法

AI 39年(公元1995年),扁鹊成立了一家专治某疑难杂症的医院,经过半年的精心筹备,硬件设施已全部到位,只缺经验丰富的医生前来坐诊。找几个猎头打听了一下,乖...

812
来自专栏专知

【重温经典】吴恩达课程学习笔记一:监督学习

【导读】前一段时间,专知内容组推出了春节充电系列:李宏毅2017机器学习课程学习笔记,反响热烈,由此可见,大家对人工智能、机器学习的系列课程非常感兴趣,近期,专...

3467
来自专栏深度学习思考者

算法优化——如何将人脸检测的速度做到极致

零、检测与识别   首先要区分两个概念“人脸检测/face detection”和“人脸识别/face recognition”。“人脸检测”是从图像中确定人脸...

4746
来自专栏人工智能

最小二乘回归的Python实现

写在前面 我们构建了非常强大的私募基金数据库,并基于这个数据库,衍生出了FOF Easy数据可视化终端和FOF Power组合基金管理系统,涉及到非常多复杂的...

2836
来自专栏大数据文摘

用100元的支票骗到100万:看看对抗性攻击是怎么为非作歹的

1473
来自专栏人工智能头条

神经网络太好骗?清华团队如何做到打NIPS攻防赛得3冠军的

今天带来的文章,由同济大学研究生张子豪投稿。介绍了人工智能与信息安全的交叉前沿研究领域:深度学习攻防对抗。

821
来自专栏用户3246163的专栏

4.1 市场风险

daily 5% VaR as $1000: 有5%的概率一天的损失大于¥1000

2743
来自专栏AI科技评论

机器视觉的阿基里斯之踵,秘密都在谷歌Brain论文中

“从一些方面看,机器视觉比人类视觉更好。但是现在研究人员找到了一类能够轻松‘愚弄’机器视觉的‘对抗性图像’。“——来自arXiv的Emerging Techno...

2976
来自专栏数据处理

矩阵奇异分解奇异值分解定理

1923

扫码关注云+社区