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

非线性系统状态估计是一大难点。KF（Kalman Filter）只适用于线性系统。EKF（Extended Kalman Filter）利用泰勒展开将非线性系统线性化。可是，EKF在强非线性系统下的误差很大。本文将介绍一种新型的滤波算法UKF（Unscented Kalman Filter），其计算精度相比EKF更高并省略了Jacobian矩阵的计算。

Young_Gy

15696人浏览 · 2017-11-16 17:01:22

Young_Gy · 2017-11-16 17:01:22 发布

这里写图片描述

Why UKF

这里写图片描述

本博客在之前两篇介绍了KF和EKF。那么，为什么还需要UKF呢，原因见下表：

模型	缺点	UKF对缺点改进
KF	只适用于线性系统	适用于非线性系统
EKF	线性化忽略了高阶项导致强非线性系统误差大；线性化处理需要计算Jacobian矩阵	对非线性的概率分布近似，没有线性化忽略高阶项；不需要计算Jacobian矩阵

UKF简述

原理概述

首先，回顾下UKF需要解决的问题，已知系统的状态及其方差 xk,Pk <script type="math/tex" id="MathJax-Element-1331">x_k,P_k</script>。如果经过非线性函数 xk+1=f(xk) <script type="math/tex" id="MathJax-Element-1332">x_{k+1} = f(x_k)</script>后，新的状态和方差如何求解。

EKF提供的方法是将非线性函数 f(x) <script type="math/tex" id="MathJax-Element-1333">f(x)</script>作泰勒一阶展开，利用Jacobian矩阵 Hj <script type="math/tex" id="MathJax-Element-1334">H_j</script>近似将 f(x) <script type="math/tex" id="MathJax-Element-1335">f(x)</script>线性化为 Hjx <script type="math/tex" id="MathJax-Element-1336">H_j x</script>。这种方法一方面在强非线性系统下误差大，另一方面Jacobian矩阵的计算着实令人头疼。

UKF认为每一个状态 xk,Pk <script type="math/tex" id="MathJax-Element-1337">x_k,P_k</script>都可以用几个Sigma点（关键点） Xsig <script type="math/tex" id="MathJax-Element-1338">X_{sig}</script>表示。当作用于非线性函数 f(x) <script type="math/tex" id="MathJax-Element-1339">f(x)</script>时，只需要将Sigma点 Xsig <script type="math/tex" id="MathJax-Element-1340">X_{sig}</script>作用于非线性函数 f(x) <script type="math/tex" id="MathJax-Element-1341">f(x)</script>得到 f(Xsig) <script type="math/tex" id="MathJax-Element-1342">f(X_{sig})</script>即可。通过得到的 f(Xsig) <script type="math/tex" id="MathJax-Element-1343">f(X_{sig})</script>可以计算新的状态 xk+1,Pk+1 <script type="math/tex" id="MathJax-Element-1344">x_{k+1},P_{k+1}</script>。

这里写图片描述

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

关键点怎么找
找到关键点后如何求出新的状态 xk+1,Pk+1 <script type="math/tex" id="MathJax-Element-1345">x_{k+1},P_{k+1}</script>

关键点怎么找

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

nx <script type="math/tex" id="MathJax-Element-2897">n_x</script>代表 xk|k <script type="math/tex" id="MathJax-Element-2898">x_{k|k}</script>的大小
λ <script type="math/tex" id="MathJax-Element-2899">\lambda</script>代表关键点的散开情况，一般采用经验值 λ=3−nx <script type="math/tex" id="MathJax-Element-2900">\lambda=3-n_x</script>

这里写图片描述

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

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

Xk+1|k <script type="math/tex" id="MathJax-Element-1948">X_{k+1|k}</script>代表Sigma点集合， Xk+1|k,i <script type="math/tex" id="MathJax-Element-1949">X_{k+1|k,i}</script>代表Sigma点集合中的第 i <script type="math/tex" id="MathJax-Element-1950">i</script>个点
na<script type="math/tex" id="MathJax-Element-1951">n_a</script>代表 xk+1|k <script type="math/tex" id="MathJax-Element-1952">x_{k+1|k}</script>增广后的大小
nσ <script type="math/tex" id="MathJax-Element-1953">n_{\sigma}</script>代表Sigma点集合的大小，一般 nσ=1+2na <script type="math/tex" id="MathJax-Element-1954">n_{\sigma} = 1+2n_a</script>
权重 wi <script type="math/tex" id="MathJax-Element-1955">w_i</script>在 i==0 <script type="math/tex" id="MathJax-Element-1956">i==0</script>时 w0=λλ+na <script type="math/tex" id="MathJax-Element-1957">w_0=\frac{\lambda}{\lambda+n_a}</script>，在 i!=0 <script type="math/tex" id="MathJax-Element-1958">i!=0</script>时 w0=12(λ+na) <script type="math/tex" id="MathJax-Element-1959">w_0=\frac{1}{2 (\lambda+n_a)}</script>

这里写图片描述

多传感器融合

下面，将通过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˙ <script type="math/tex" id="MathJax-Element-2128">\dot{x}</script>及其对时间的积分 xk+1=∫x˙dt <script type="math/tex" id="MathJax-Element-2129">x_{k+1}=\int \dot{x} dt</script>可得Process模型为：

x k + 1 = x k + ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ v k ψ k ˙ (s i n (ψ k + ψ k ˙ Δ t) - s i n (ψ k)) v k ψ k ˙ (- c o s (ψ k + ψ k ˙ Δ t) + c o s (ψ k)) 0 ψ k ˙ Δ t 0 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

考虑Process noise为：

x k + 1 = x k + ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ v k ψ k ˙ (s i n (ψ k + ψ k ˙ Δ t) - s i n (ψ k)) v k ψ k ˙ (- c o s (ψ k + ψ k ˙ Δ t) + c o s (ψ k)) 0 ψ k ˙ Δ t 0 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ + ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 1 2 ν a, k cos (ψ k) Δ t 2 1 2 ν a, k sin (ψ k) Δ t 2 ν a, k Δ t 1 2 ν ψ ¨, k Δ t 2 ν ψ ¨, k Δ t ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

RoadMap

这里写图片描述

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

初始化系统状态 xk,Pk <script type="math/tex" id="MathJax-Element-2912">x_k, P_k</script>
根据状态 xk,Pk <script type="math/tex" id="MathJax-Element-2913">x_k, P_k</script>生成Sigma点 Xk <script type="math/tex" id="MathJax-Element-2914">X_k</script>
根据process model预测未来的Sigma点 Xk+1|k <script type="math/tex" id="MathJax-Element-2915">X_{k+1|k}</script>
根据预测的Sigma点 Xk+1|k <script type="math/tex" id="MathJax-Element-2916">X_{k+1|k}</script>生成状态预测 xk+1|k,Pk+1|k <script type="math/tex" id="MathJax-Element-2917">x_{k+1|k}, P_{k+1|k}</script>
当测量值到来时，将预测的Sigma点 Xk+1|k <script type="math/tex" id="MathJax-Element-2918">X_{k+1|k}</script>转换成预测测量值 Zk+1|k <script type="math/tex" id="MathJax-Element-2919">Z_{k+1|k}</script>
根据预测测量值 Zk+1|k <script type="math/tex" id="MathJax-Element-2920">Z_{k+1|k}</script>与真实测量值 zk+1 <script type="math/tex" id="MathJax-Element-2921">z_{k+1}</script>的差值更新得到系统状态 xk+1|k+1,Pk+1|k+1 <script type="math/tex" id="MathJax-Element-2922">x_{k+1|k+1}, P_{k+1|k+1}</script>

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

数据增广
Update State
Noise Level与NIS

数据增广

这里写图片描述

如上图，在process预测时需要对 xk <script type="math/tex" id="MathJax-Element-2799">x_k</script>进行增广，原因是process模型中包含了噪声的非线性关系 f(xk,νk) <script type="math/tex" id="MathJax-Element-2800">f(x_k,\nu_k)</script>。反之，在measurement model中因为噪声是线性关系的所以不需要进行数据增广。

增广后 x，P <script type="math/tex" id="MathJax-Element-2801">x，P</script>变化如下，

x = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ p x p y v ψ ψ ˙ ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ \Rightarrow x a = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ p x p y v ψ ψ ˙ ν a ν ψ ¨ ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

P a = [P 0 0 Q] Q = [ν 2 a 0 0 ν 2 ψ ¨]

Update State

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

这里写图片描述

Noise Level与NIS

UKF中牵涉的噪音有两类：

Process Noise： νa,νψ¨ <script type="math/tex" id="MathJax-Element-2923">\nu_a,\nu_{\ddot{\psi}}</script>，需要自己设定
Measurement Noise：lidar、radar的噪音水平，由厂家提供