无迹（损）卡尔曼滤波（UKF）理论讲解与实例

文章目录

无迹（损）卡尔曼滤波（UKF）理论讲解与实例
理论讲解
- 模型对比
- UT变换
- UKF算法步骤
- 预测部分
- 更新部分
应用实例
- CTRV模型
- 预测处理
- - 产生点云
  - - 生成增广矩阵
  - 生成预测点
  - 计算预测的均值和方差
- 更新处理
- - 预测量测值
  - 计算预测量测值的均值和方差
  - 更新状态
  - 完整代码
参考链接

理论讲解

前两篇博客的卡尔曼滤波（参见我的另一篇文章：卡尔曼滤波理论讲解与应用（matlab和python））和扩展卡尔曼滤波（参见我的另一篇文章：扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码））都是都将问题转化为线性高斯模型，所以可以直接解出贝叶斯递推公式中的解析形式，方便运算。但对于非线性问题，扩展卡尔曼滤波除了计算量大，还有线性误差的影响，有没有别的方法？EKF利用高斯假设，通过泰勒分解将模型线性化，进而求出预测模型的概率分布（均值和方差）。而无迹（损）卡尔曼滤波了（Unscented Kalman Filter ，UKF）则通过不敏变换（Unscented Transform，UT）来求出预测模型的均值和方差。如下图所示：

UKF生成了一些点，来近似非线性。由这些点来决定实际xxx和PPP的取值范围。感觉有点像粒子滤波器的概念，但还有些不同，因为UKF里的Sigma点的生成并没有概率的问题。UKF的Sigma点就是把不能解决的非线性单个变量的不确定性，用多个Sigma点的不确定性近似了。

EKF通过泰勒分解将模型线性化求出预测模型的概率均值和方差\color{darkorange}{\textbf{ EKF通过泰勒分解将模型线性化求出预测模型的概率均值和方差}} EKF通过泰勒分解将模型线性化求出预测模型的概率均值和方差

UKF了则通过不敏变换来求出预测模型的均值和方差\color{darkorange}{\textbf{ UKF了则通过不敏变换来求出预测模型的均值和方差}} UKF了则通过不敏变换来求出预测模型的均值和方差

模型对比

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

UT变换

不敏变换（一种计算非线性随机变量各阶矩的近似方法）可以较好的解决非线性问题，通过一定规律的采样和权重，可以近似获得均值和方差。而且由于不敏变换对统计矩的近似精度较高，UKF的效果可以达到二阶EKF的效果。

先来说一下什么叫UT变换：

假设一个非线性系统y=f(x)y=f(x)y=f(x)，其中xxx为nnn维状态向量，并已知其平均值为x‾\overline xx，方差为PxP_xPx，则可以经过UT变换构造2n+12n+12n+1个Sigma点xix_ixi，同时构造xix_ixi相应的权值WiW_iWi，进而得到yyy的统计特性（均值和方差）。有点类似于概率论里已知xxx的均值和方差，求y=f(x)y=f(x)y=f(x)的yyy的均值和方差。

UKF算法步骤

UKF的算法步骤如下

初始化系统状态xk,Pkx_k,P_kxk,Pk

根据状态xk,Pkx_k,P_kxk,Pk生成Sigma点XkX_kXk

根据预测模型预测未来的Sigma点Xk+1∣kX_{k+1|k}Xk+1∣k

根据预测的Sigma点Xk+1∣kX_{k+1|k}Xk+1∣k生成状态预测的Sigma点xk+1∣k,Pk+1∣kx_{k+1|k},P_{k+1|k}xk+1∣k,Pk+1∣k

当测量值到来时，将预测的Sigma点Xk+1∣kX_{k+1|k}Xk+1∣k转换成预测测量值Zk+1∣kZ_{k+1|k}Zk+1∣k

根据预测测量值Zk+1∣kZ_{k+1|k}Zk+1∣k与真实测量值zk+1z_{k+1}zk+1的差值更新得到系统状态xk+1∣k+1,Pk+1∣k+1x_{k+1|k+1},P_{k+1|k+1}xk+1∣k+1,Pk+1∣k+1（两个高斯分布相乘得到新的系统状态xk+1∣k+1,Pk+1∣k+1x_{k+1|k+1},P_{k+1|k+1}xk+1∣k+1,Pk+1∣k+1）

预测部分

那UKF如何运用UT变化来求出预测模型的均值和方差？

首先如何由初始化的系统状态xk∣kx_{k|k}xk∣k这个点求出2n+12n+12n+1个Sigma点。

利用下图公式可以求出2n+12n+12n+1个Sigma点，nnn代表xk∣kx_{k|k}xk∣k这个状态向量的维度，假如xk∣kx_{k|k}xk∣k只有位置信息，即xk∣k=[px,py]x_{k|k}=[p_x,p_y]xk∣k=[px,py]，那么n=2n=2n=2，Sigma点就有5个。同理，如果n=5n=5n=5，Sigma点就有11个。

在该公式中左边的Xk∣kX_{k|k}Xk∣k代表最后的2n+12n+12n+1个Sigma点，右边的xk∣kx_{k|k}xk∣k代表初始状态的均值，Pk∣kP_{k|k}Pk∣k代表初始状态的方差，剩下的两个式子xk∣k+((n+λ)Pk∣k)xk∣k−((n+λ)Pk∣k)x_{k|k} + \left( \sqrt {(n + \lambda)P_{k|k}}\right) \quad x_{k|k} - \left( \sqrt {(n + \lambda)P_{k|k}}\right)xk∣k+((n+λ)Pk∣k)xk∣k−((n+λ)Pk∣k)是关于xk∣kx_{k|k}xk∣k这个点对称的， λ\lambdaλ可以决定周围2n2n2n个sigma点离中心xk∣kx_{k|k}xk∣k的距离,通常取λ=3−n\lambda=3-nλ=3−n， λ\lambdaλ是个经验公式。

计算转换后yyy的均值和方差

现在求出来了这么多的点来描述原来的状态分布（即第kkk步的分布），那么经过非线性函数y=f(xk,vk)y=f(x_k,v_k)y=f(xk,vk)变换后，yyy的均值和方差怎么求呢（即第k+1k+1k+1步的分布）？计算过程如下图所示：

图片中xk+1∣k,ix_{k+1|k,i}xk+1∣k,i（ xk+1∣k,ix_{k+1|k,i}xk+1∣k,i代表Sigma点集合中的第iii个点）可以根据每个Sigma点带入非线性函数y=f(xk,vk)y=f(x_k,v_k)y=f(xk,vk)求出来，如下图所示

那wiw_iwi（wiw_iwi代表权重）怎么求呢？
第一个xk∣k点的权重计算如下：w[i]=λλ+n,i=1剩下对称的2n个sigma的点权重计算如下w[i]=12(λ+n),i=2,...,2n+1其中λ=3−n第一个x_{k|k}点的权重计算如下：\\ w^{[i]} = \frac{\lambda}{\lambda + n}, \quad i = 1\\ 剩下对称的2n个sigma的点权重计算如下\\ w^{[i]} = \frac{1}{2(\lambda + n)}, \quad i = 2, ..., 2n+1\\ 其中\lambda=3-n 第一个xk∣k点的权重计算如下：w[i]=λ+nλ,i=1剩下对称的2n个sigma的点权重计算如下w[i]=2(λ+n)1,i=2,...,2n+1其中λ=3−n
这样就可以求出预测后yyy的均值和方差了。下面我们需要求出测量值z⃗\vec zz的均值和方差，然后这两个分布相乘就可以求出新的状态分布了。

更新部分

针对不同的传感器，测量值z⃗\vec zz求均值和方差的方式也不同，本文以CTRV模型为基础，通过激光雷达（Lidar）和毫米波雷达（Radar）跟踪车辆位置为例讲解。求出测量值z⃗\vec zz的均值和方差，然后这两个分布相乘就可以求出新的状态分布了，这个和传统的卡尔曼滤波基本是一样的。具体的参见下面例子解释。

应用实例

CTRV模型

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

因假定turn rate（ψ\psiψ）、velocity（vvv）不变，其预测噪声包含加速度与角加速度为：
νk=[νa,kνψ¨,k]\nu_k = \begin{bmatrix} \nu_{a,k} \\\nu_{\ddot{\psi},k} \end{bmatrix} νk=[νa,kνψ¨,k]
利用x˙\dot xx˙及其对时间的积分xk+1=∫x˙dtx_{k+1}=\int \dot{x}dtxk+1=∫x˙dt可得预测模型为：
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} xk+1=xk+⎣⎢⎢⎢⎢⎢⎡ψk˙vk(sin(ψk+ψk˙Δt)−sin(ψk))ψk˙vk(−cos(ψk+ψk˙Δt)+cos(ψk))0ψk˙Δt0⎦⎥⎥⎥⎥⎥⎤
考虑预测噪声为：
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} xk+1=xk+⎣⎢⎢⎢⎢⎢⎡ψk˙vk(sin(ψk+ψk˙Δt)−sin(ψk))ψk˙vk(−cos(ψk+ψk˙Δt)+cos(ψk))0ψk˙Δt0⎦⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎡21νa,kcos(ψk)Δt221νa,ksin(ψk)Δt2νa,kΔt21νψ¨,kΔt2νψ¨,kΔt⎦⎥⎥⎥⎥⎤

预测处理

产生点云

下图的公式为生成Sigma点的公式。第一列就是初始化的系统状态xk∣kx_{k|k}xk∣k现在的值，也就是从上一个状态接手的xxx值。第二列和第三列的公式中 λ\lambdaλ 是一个数字。具体算法是λ=3−n\lambda=3-nλ=3−n，是个经验公式。这里nnn就是状态变量的个数。如果我们有5个状态需要测量，那么nnn就等于5。那么根据下面公式就可以得到[5x11]的矩阵了。生成的矩阵代表的含义就是，按照一定规律生成了环绕在x周边的10个点。由这10个点的平均值定义xxx的实际值（见下两图）。事实上，λ\lambdaλ 表现的是Sigma点离xxx的距离。

Sigma点之前

生成Sigma点

生成增广矩阵

什么叫增广矩阵？（augmented matrix）。因为我们的状态方程里面是有噪声vkvkvk的。当这个vkvkvk对我们的状态转移矩阵有影响的话，我们需要讲这个噪声vkvkvk考虑到我们的状态转移矩阵里面的。所以，我们同时也把vkvkvk当作一种状态（噪声状态）放进我们的状态变量空间里。
xk+1=F(xk,vk)zk=H(xk,nk)x_{k+1}=F(x_k,v_k)\\z_k=H(x_k,n_k) xk+1=F(xk,vk)zk=H(xk,nk)
其预测噪声包含加速度与角加速度为：
νk=[νa,kνψ¨,k]\nu_k = \begin{bmatrix} \nu_{a,k} \\\nu_{\ddot{\psi},k} \end{bmatrix} νk=[νa,kνψ¨,k]

考虑预测噪声为：
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} xk+1=xk+⎣⎢⎢⎢⎢⎢⎡ψk˙vk(sin(ψk+ψk˙Δt)−sin(ψk))ψk˙vk(−cos(ψk+ψk˙Δt)+cos(ψk))0ψk˙Δt0⎦⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎡21νa,kcos(ψk)Δt221νa,ksin(ψk)Δt2νa,kΔt21νψ¨,kΔt2νψ¨,kΔt⎦⎥⎥⎥⎥⎤
也就是说，按照上面的介绍中说道，假设原来的状态变量个数是5个。那么由于还要顾及νa,k\nu_{a,k}νa,k和νψ¨,k\nu_{\ddot{\psi},k}νψ¨,k的影响，要把这两个噪声也放进状态变量里。

5个状态–>7个状态。 5个原来的状态+2个噪声状态。

原状态

扩展状态

扩展状态和协方差

生成预测点

现在我们生成了增广的Sigma点，那么因为物体会按一定规律移动，所以我们需要预测物体的下一个状态。这里就是根据状态转移矩阵来计算的，我们只需要把每个Sigma点插入过程模型xk+1=f(xk,νk)x_{k+1} = f(x_k,\nu_k)xk+1=f(xk,νk)即可。我们对物理现象的建模过程在CTRV模型那一节已经说过，这里就不多阐述。需要说明的是上式的矩阵是[7x15]的，经过过程模型计算，下式的矩阵是[5x15]的。

计算预测的均值和方差

我们现在有很多预测后的Sigma点。那么我们需要计算预测的均值和方差了。注意weight的第一个值的计算方法和其他不一样哦。参数的解释在理论讲解里面说过了。

更新处理

假设我们有激光雷达（Lidar）和毫米波雷达（Radar）两个传感器，它们分别以一定的频率来测量如下数据：

激光雷达：测量目标车辆的坐标 (x,y)(x,y)(x,y) 。这里的x,yx,yx,y是相对于我们的车辆的坐标系的，即我们的车辆为坐标系的原点，我们的车头为xxx轴，车的左侧为yyy轴。
毫米波雷达：测量目标车辆在我们车辆坐标系下与本车的距离ρρρ，目标车辆与x轴的夹角 ψψψ，以及目标车辆与我们自己的相对距离变化率 ρ˙\dot ρρ˙（本质上就是目标车辆的实际速度在我们和目标车辆的连线上的分量）

前面的卡尔曼滤波器中，我们使用一个测量矩阵 HHH 将预测的结果映射到测量空间，那是因为这个映射本身就是线性的，现在，我们使用毫米波雷达和激光雷达来测量目标车辆(我们把这个过程称为传感器融合)，这个时候会有两种情况，即：

激光雷达的测量模型仍然是线性的，其测量矩阵为：

HL=[1000001000]H_L = \left[ \begin{array}{c} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \end{array} \right] HL=[1001000000]

将预测映射到激光雷达测量空间：
HLx→=(x,y)TH_L\overrightarrow{x} = (x, y)^T HLx=(x,y)T

毫米波雷达的预测映射到测量空间是非线性的，其表达式为：

(ρψρ˙)=(x2+y2atan2⁡(y,x)vx+vyx2+y2)\left(\begin{matrix} \rho \\ \psi \\ \dot\rho \end{matrix}\right) = \left(\begin{matrix}\sqrt{x^{2} + y^{2}}\\\operatorname{atan_{2}}{\left (y,x \right )}\\\frac{v x + v y}{\sqrt{x^{2} + y^{2}}}\end{matrix}\right) ⎝⎛ρψρ˙⎠⎞=⎝⎜⎛x2+y2atan2(y,x)x2+y2vx+vy⎠⎟⎞

此时我们使用 h(x)h(x)h(x)来表示这样一个非线性映射

预测量测值

测量更新分为两个部分，Lidar测量和Radar测量，其中Lidar测量模型本身就是线性的，所以我们重点还是放在Radar测量模型的处理上面。

为了计算我们预测的zzz和实际的传感器数据zzz的误差，我们需要通过测量转移矩阵h(x)h(x)h(x)把状态空间向量和传感器可得到的数据关联起来。因为毫米波雷达的测量转移矩阵h(x)h(x)h(x)是一个非线性的函数，这里和我们预测步骤碰到的问题非常类似，我们也需要求出一些Sigma点，然后通过非线性函数h(x)h(x)h(x)把预测值Xk+1∣kX_ {k+1|k}Xk+1∣k（矩阵是[5x15]）转换到量测空间z⃗k+1\vec z_{k+1}zk+1（矩阵是[3x15]，zk+1z_{k+1}zk+1就是求的z预测值z_{预测值}z预测值。），这里我们就可以偷一个懒，只需重用我们在预测步骤已经有的Sigma点即可，我们这次可以跳过生成sigma点，我们直接用生成的预测点xk+1∣kx_ {k+1|k}xk+1∣k带入zk+1=h(xk+1+wk+1)z_{k+1}=h(x_{k+1}+w_{k+1})zk+1=h(xk+1+wk+1)求得量测空间的值。注意，这里zk+1z_{k+1}zk+1只是我们计算出来的，并不是来自传感器的数据。

状态转移矩阵h(x)

矩阵大小的变化

计算预测量测值的均值和方差

先利用上一步Sigma点预测值（也就是矩阵是[5x15]的Sigma点）挨个求出相对应的测量值预测值。这样我们可以得到15列相应的测量值（矩阵是[3x15]）。这个值就是下图中的zk+1∣k,iz_{k+1|k,i}zk+1∣k,i。
这里又一次出现了wiw_iwi 。 wiw_iwi就是上一步中用来求xxx平均值的权重。这里我们可以不需要额外的计算，依然用预测过程中求得的 wiw_iwi值。
把每一个 wiw_iwi（[1x15]）和对应的每一个zk+1∣k,iz_{k+1|k,i}zk+1∣k,i（[3x15]）相乘并相加，最终得到预测值zzz的均值。
这里wk+1w_{k+1}wk+1 是量测模型里面的噪声。他对系统没有非线性影响，所以也不用被扩展测量向量空间，只需要简单的相加就可以了。
然后计算预测量测值的协方差。按照一下公式计算就可以。同样，因为RRR也是因为没有非线性影响，所以也可以直接相加。

更新状态

现在我们有预测的状态均值xk+1∣kx_{k+1|k}xk+1∣k和协方差Pk+1∣kP_{k+1|k}Pk+1∣k，以及预测的测量均值zk+1∣kz_{k+1|k}zk+1∣k和协方差Sk+1∣kS_{k+1|k}Sk+1∣k，但我们还需要一个东西就是我们从时间步 k+1k+1k+1 收到的实际测量值 zk+1z_{k+1}zk+1

这是UKF的最后阶段。这里我们最终根据xxx预测值，和zzz预测值求出kalman gainKk+1∣kK_{k+1|k}Kk+1∣k和cross-correlation functionTk+1∣kT_{k+1|k}Tk+1∣k然后最终更新状态和协方差，这些都是单纯的计算。而且state update和covariance matrix update都是跟标准卡尔曼滤波器一样的。 UKF独有的计算有kalman gain和cross-correlation function。不过也都是单纯的计算。这里我想说一下，我理解的cross-correlation function的作用。通过式子我们可以看出求cross-correlation function的内部结构。他需要每个预测的Sigma点和xxx预测值的差和每个Sigma点预测的测量值和z预测值的差。也就是说，cross-correlation 这个方程会根据Sigma点和预测值之间的差来平衡kalman gain。而kalman gain里面不仅用到cross-correlation function，还会用到测量值协方差预测值。这样kalman gain就可以利用每个预测的Sigma点和x预测值的差和每个Sigma点预测的测量值和zzz预测值的差，来平衡模型的预测准确度和传感器的预测准确度。（kalman gain 就是一种权重）

完整代码

事实上，这个project 是融合Lidar 和Radar的算法。说是融合，但说白了就是，不同时间段处理不同传感器input而已。因为这个input的不一样决定了，里面运行的代码是ukf还是ekf。像在这个project 里面，因为lidar 是线性的，所以就不需要用到UKF ，而radar因为传感器获取的数据种类就要求了它要用UKF来实现（当然EKF也可以，但是精度低而已）。

完整的C++代码：C++_UKF_CTRV 代码

另外这里还有一个代码例程把EKF和UKF做了一个对比。
C++_EKF_UKF_实验对比
仿真场景是跟踪预测机器人的位置，实验图片如下：

最后，插播一条广告，这里有完整的优达学城（udacity）无人驾驶车完整的教程，带中英文字幕（每一学期的中英文字幕都有哟！），需要的私信我，嘻嘻~~。

参考链接

UKF的理念
无损滤波器 UKF
无迹Kalman滤波算法(matlab)
无损卡尔曼滤波器-UKF
无损卡尔曼滤波UKF与多传感器融合
无损卡尔曼滤波
无迹卡尔曼滤波器完整公式推导