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

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

理论讲解

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

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

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

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

模型对比

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

UT变换

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

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

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

UKF算法步骤

UKF的算法步骤如下

1. 初始化系统状态$x_k,P_k$
2. 根据状态$x_k,P_k$生成Sigma点$X_k$
3. 根据预测模型预测未来的Sigma点$X_{k+1|k}$
4. 根据预测的Sigma点$X_{k+1|k}$生成状态预测的Sigma点$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}$（两个高斯分布相乘得到新的系统状态$x_{k+1|k+1},P_{k+1|k+1}$）

预测部分

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

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

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

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

2. 计算转换后$y$的均值和方差

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

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

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

更新部分

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

应用实例

CTRV模型

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

因假定turn rate（$\psi$）、velocity（$v$）不变，其预测噪声包含加速度与角加速度为：
$${\nu }_k=\left[\begin{array}{c}{\nu }_{a,k}\\ {\nu }_{\ddot{\psi },k}\end{array}\right]$$
利用$\dot{x}$及其对时间的积分$x_{k+1}=\int \dot{x}dt$可得预测模型为：
$$x_{k+1}=x_k+\left[\begin{array}{c}\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{array}\right]$$
考虑预测噪声为：
$$x_{k+1}=x_k+\left[\begin{array}{c}\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{array}\right]+\left[\begin{array}{c}\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{array}\right]$$

预测处理

产生点云

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

Sigma点之前

生成Sigma点

生成增广矩阵

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

考虑预测噪声为：
$$x_{k+1}=x_k+\left[\begin{array}{c}\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{array}\right]+\left[\begin{array}{c}\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{array}\right]$$
也就是说，按照上面的介绍中说道，假设原来的状态变量个数是5个。那么由于还要顾及${\nu }_{a,k}$和${\nu }_{\ddot{\psi },k}$的影响，要把这两个噪声也放进状态变量里。

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

原状态

扩展状态

扩展状态和协方差

生成预测点

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

计算预测的均值和方差

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

更新处理

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

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

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

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

$$H_L=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\end{array}\right]$$

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

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

$$\left(\begin{array}{c}\rho \\ \psi \\ \dot{\rho }\end{array}\right)=\left(\begin{array}{c}\sqrt{x^2+y^2}\\ \mathrm{a}\mathrm{t}\mathrm{a}{\mathrm{n}}_2\left(y,x\right)\\ \frac{vx+vy}{\sqrt{x^2+y^2}}\end{array}\right)$$

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

预测量测值

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

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

状态转移矩阵h(x)

矩阵大小的变化

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

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

更新状态

现在我们有预测的状态均值$x_{k+1|k}$和协方差$P_{k+1|k}$，以及预测的测量均值$z_{k+1|k}$和协方差$S_{k+1|k}$， 但我们还需要一个东西 就是我们从时间步 $k+1$ 收到的实际测量值 $z_{k+1}$

这是UKF的最后阶段。这里我们最终根据$x$预测值，和$z$预测值求出kalman gain$K_{k+1|k}$和cross-correlation function$T_{k+1|k}$然后最终更新状态和协方差，这些都是单纯的计算。而且state update和covariance matrix update都是跟标准卡尔曼滤波器一样的。 UKF独有的计算有kalman gain和cross-correlation function。不过也都是单纯的计算。这里我想说一下，我理解的cross-correlation function的作用。 通过式子我们可以看出求cross-correlation function的内部结构。他需要每个预测的Sigma点和$x$预测值的差和每个Sigma点预测的测量值和z预测值的差 。也就是说，cross-correlation 这个方程会根据Sigma点和预测值之间的差来平衡kalman gain。 而kalman gain里面不仅用到cross-correlation function，还会用到测量值协方差预测值。这样kalman gain就可以利用每个预测的Sigma点和x预测值的差和每个Sigma点预测的测量值和$z$预测值的差，来平衡模型的预测准确度和传感器的预测准确度。（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）无人驾驶车完整的教程，带中英文字幕（每一学期的中英文字幕都有哟！），需要的私信我， 嘻嘻~~。

参考链接

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