无损卡尔曼滤波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需要解决的问题,已知系统的状态及其方差$x_k,P_k$。如果经过非线性函数$x_{k+1} = f(x_k)$后,新的状态和方差如何求解。

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

UKF认为每一个状态$x_k,P_k$都可以用几个Sigma点(关键点)$X_{sig}$表示。当作用于非线性函数$f(x)$时,只需要将Sigma点$X_{sig}$作用于非线性函数$f(x)$得到$f(X_{sig})$即可。通过得到的$f(X_{sig})$可以计算新的状态$x_{k+1},P_{k+1}$。

这里写图片描述

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

  1. 关键点怎么找
  2. 找到关键点后如何求出新的状态$x_{k+1},P_{k+1}$

关键点怎么找

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

  • $n_x$代表$x_{k|k}$的大小
  • $\lambda$代表关键点的散开情况,一般采用经验值$\lambda=3-n_x$

这里写图片描述

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

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

  • $X_{k+1|k}$代表Sigma点集合,$X_{k+1|k,i}$代表Sigma点集合中的第$i$个点
  • $n_a$代表$x_{k+1|k}$增广后的大小
  • $n_{\sigma}$代表Sigma点集合的大小,一般$n_{\sigma} = 1+2n_a$
  • 权重$w_i$在$i==0$时$w_0=\frac{\lambda}{\lambda+n_a}$,在$i!=0$时$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} $$

利用$\dot{x}$及其对时间的积分$x_{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如上图所示,核心思想在前部分已介绍过,其算法是:

  1. 初始化系统状态$x_k, P_k$
  2. 根据状态$x_k, P_k$生成Sigma点$X_k$
  3. 根据process model预测未来的Sigma点$X_{k+1|k}$
  4. 根据预测的Sigma点$X_{k+1|k}$生成状态预测$x_{k+1|k}, P_{k+1|k}$
  5. 当测量值到来时,将预测的Sigma点$X_{k+1|k}$转换成预测测量值$Z_{k+1|k}$
  6. 根据预测测量值$Z_{k+1|k}$与真实测量值$z_{k+1}$的差值更新得到系统状态$x_{k+1|k+1}, P_{k+1|k+1}$

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

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

数据增广

这里写图片描述

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

增广后$x,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:$\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