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

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

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

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

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

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

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

UKF的算法步骤如下

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

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

在该公式中左边的 X k ∣ k X_{k|k} Xk∣k​代表最后的 2 n + 1 2n+1 2n+1个Sigma点，右边的 x k ∣ k x_{k|k} xk∣k​代表初始状态的均值， P k ∣ k P_{k|k} Pk∣k​代表初始状态的方差，剩下的两个式子 x k ∣ k + ( ( n + λ ) P k ∣ k ) x k ∣ k − ( ( n + λ ) P k ∣ 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​ ​)是关于 x k ∣ k x_{k|k} xk∣k​这个点对称的， λ \lambda λ可以决定周围 2 n 2n 2n个sigma点离中心 x k ∣ k x_{k|k} xk∣k​的距离,通常取 λ = 3 − n \lambda=3-n λ=3−n， λ \lambda λ是个经验公式 。

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

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

那 w i w_i wi​（ w i w_i wi​代表权重）怎么求呢？ 第 一 个 x k ∣ k 点 的 权 重 计 算 如 下 ： w [ i ] = λ λ + n , i = 1 剩 下 对 称 的 2 n 个 s i g m a 的 点 权 重 计 算 如 下 w [ i ] = 1 2 ( λ + n ) , i = 2 , . . . , 2 n + 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 这样就可以求出预测后 y y y的均值和方差了。下面我们需要求出测量值 z ⃗ \vec z z 的均值和方差，然后这两个分布相乘就可以求出新的状态分布了。

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

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

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

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

什么叫增广矩阵？（augmented matrix）。 因为我们的状态方程里面是有噪声 v k vk vk的。当这个 v k vk vk对我们的状态转移矩阵有影响的话，我们需要讲这个噪声 v k vk vk考虑到我们的状态转移矩阵里面的。所以，我们同时也把 v k vk vk当作一种状态（噪声状态）放进我们的状态变量空间里。 x k + 1 = F ( x k , v k ) z k = H ( x k , n k ) 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​​]

考虑预测噪声为： 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 ] 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,k​cos(ψk​)Δt221​νa,k​sin(ψ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点插入过程模型 x k + 1 = f ( x k , ν k ) x_{k+1} = f(x_k,\nu_k) xk+1​=f(xk​,νk​)即可。我们对物理现象的建模过程在CTRV模型那一节已经说过，这里就不多阐述。 需要说明的是上式的矩阵是[7x15]的，经过过程模型计算，下式的矩阵是[5x15]的。

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

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

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

H L = [ 1 0 0 0 0 0 1 0 0 0 ] H_L = \left[ \begin{array}{c} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \end{array} \right] HL​=[10​01​00​00​00​]

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

( ρ ψ ρ ˙ ) = ( x 2 + y 2 a t a n 2 ⁡ ( y , x ) v x + v y x 2 + y 2 ) \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+y2 ​atan2​(y,x)x2+y2 ​vx+vy​​⎠⎟⎞​

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

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

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

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

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