无损卡尔曼滤波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,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 的大小
• λ 代表关键点的散开情况，一般采用经验值 λ=3−nx

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

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

• Xk+1|k 代表Sigma点集合， Xk+1|k,i 代表Sigma点集合中的第 i 个点
• na 代表 xk+1|k 增广后的大小
• nσ 代表Sigma点集合的大小，一般 nσ=1+2na
• 权重 wi 在 i==0 时 w0=λλ+na ，在 i!=0 时 w0=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中因为噪声是线性关系的所以不需要进行数据增广。

增广后 x，P 变化如下，

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