非线性系统状态估计是一大难点。KF(Kalman Filter)只适用于线性系统。EKF(Extended Kalman Filter)利用泰勒展开将非线性系统线性化。可是,EKF在强非线性系统下的误差很大。本文将介绍一种新型的滤波算法UKF(Unscented Kalman Filter),其计算精度相比EKF更高并省略了Jacobian矩阵的计算。
本博客在之前两篇介绍了KF和EKF。那么,为什么还需要UKF呢,原因见下表:
模型 | 缺点 | UKF对缺点改进 |
---|---|---|
KF | 只适用于线性系统 | 适用于非线性系统 |
EKF | 线性化忽略了高阶项导致强非线性系统误差大;线性化处理需要计算Jacobian矩阵 | 对非线性的概率分布近似,没有线性化忽略高阶项; 不需要计算Jacobian矩阵 |
首先,回顾下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只是将非线性函数映射通过关键点映射来实现,那么出现几个问题:
关键点的意义在于能够充分刻画原状态的分布情况,其经验公式如下图所示,需要注意的是:
新状态的求解公式如下图所示,需要注意的是:
下面,将通过lidar、radar跟踪小车的例子,讲解UKF如何应用于小车状态跟踪。相关传感器信息及大体步骤可见扩展卡尔曼滤波EKF与多传感器融合。
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}
UKF的RoadMap如上图所示,核心思想在前部分已介绍过,其算法是:
同时,有几个部分需要强调下。
如上图,在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=P00QQ=ν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}
State的更新公式如下图所示:
UKF中牵涉的噪音有两类:
自己设定调整的方法有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