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

转自：https://blog.csdn.net/Young_Gy/article/details/78542754

 

非线性系统状态估计是一大难点。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需要解决的问题，已知系统的状态及其方差

。如果经过非线性函数

后，新的状态和方差如何求解。

EKF提供的方法是将非线性函数

作泰勒一阶展开，利用Jacobian矩阵近似将线性化为

。这种方法一方面在强非线性系统下误差大，另一方面Jacobian矩阵的计算着实令人头疼。

UKF认为每一个状态

都可以用几个Sigma点（关键点）表示。当作用于非线性函数时，只需要将Sigma点作用于非线性函数得到即可。通过得到的可以计算新的状态

。

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

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

1.  

关键点怎么找

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

•  

代表

的大小代表关键点的散开情况，一般采用经验值

•  

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

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

•  

代表Sigma点集合，代表Sigma点集合中的第个点代表增广后的大小代表Sigma点集合的大小，一般
权重在时，在时

•  

多传感器融合

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

CTRV模型

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

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

利用

及其对时间的积分

可得Process模型为：

考虑Process noise为：

RoadMap

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

1. 初始化系统状态

根据状态生成Sigma点
根据process model预测未来的Sigma点
根据预测的Sigma点生成状态预测
当测量值到来时，将预测的Sigma点转换成预测测量值
根据预测测量值与真实测量值的差值更新得到系统状态

1.  

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

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

数据增广

如上图，在process预测时需要对

进行增广，原因是process模型中包含了噪声的非线性关系

。反之，在measurement model中因为噪声是线性关系的所以不需要进行数据增广。

增广后

变化如下，

Update State

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

Noise Level与NIS

UKF中牵涉的噪音有两类：

• Process Noise：

• ，需要自己设定
• 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