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

1 篇文章 2 订阅

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

理论讲解

前两篇博客的卡尔曼滤波(参见我的另一篇文章:卡尔曼滤波理论讲解与应用(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对缺点改进
KF只适用于线性系统适用于非线性系统
EKF线性化忽略了高阶项导致强非线性系统误差大;线性化处理需要计算Jacobian矩阵对非线性的概率分布近似,没有线性化忽略高阶项; 不需要计算Jacobian矩阵

UT变换

不敏变换(一种计算非线性随机变量各阶矩的近似方法)可以较好的解决非线性问题,通过一定规律的采样和权重,可以近似获得均值和方差。而且由于不敏变换对统计矩的近似精度较高,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+1Sigma 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的算法步骤如下

  1. 初始化系统状态 x k , P k x_k,P_k xk,Pk
  2. 根据状态 x k , P k x_k,P_k xk,Pk生成Sigma点 X k X_k Xk
  3. 根据预测模型预测未来的Sigma点 X k + 1 ∣ k X_{k+1|k} Xk+1k
  4. 根据预测的Sigma点 X k + 1 ∣ k X_{k+1|k} Xk+1k生成状态预测的Sigma点 x k + 1 ∣ k , P k + 1 ∣ k x_{k+1|k},P_{k+1|k} xk+1k,Pk+1k
  5. 当测量值到来时,将预测的Sigma点 X k + 1 ∣ k X_{k+1|k} Xk+1k转换成预测测量值 Z k + 1 ∣ k Z_{k+1|k} Zk+1k
  6. 根据预测测量值 Z k + 1 ∣ k Z_{k+1|k} Zk+1k与真实测量值 z k + 1 z_{k+1} zk+1的差值更新得到系统状态 x k + 1 ∣ k + 1 , P k + 1 ∣ k + 1 x_{k+1|k+1},P_{k+1|k+1} xk+1k+1,Pk+1k+1(两个高斯分布相乘得到新的系统状态 x k + 1 ∣ k + 1 , P k + 1 ∣ k + 1 x_{k+1|k+1},P_{k+1|k+1} xk+1k+1,Pk+1k+1

预测部分

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

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

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

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

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

现在求出来了这么多的点来描述原来的状态分布(即第 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+1k,i x k + 1 ∣ k , i x_{k+1|k,i} xk+1k,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 xkkw[i]=λ+nλ,i=12nsigmaw[i]=2(λ+n)1,i=2,...,2n+1λ=3n
这样就可以求出预测后 y y y的均值和方差了。下面我们需要求出测量值 z ⃗ \vec z z 的均值和方差,然后这两个分布相乘就可以求出新的状态分布了

更新部分

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

应用实例

CTRV模型

本文将使用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,kcos(ψk)Δt221νa,ksin(ψk)Δt2νa,kΔt21νψ¨,kΔt2νψ¨,kΔt

预测处理

产生点云

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

在这里插入图片描述

Sigma点之前

在这里插入图片描述

生成Sigma点
生成增广矩阵

什么叫增广矩阵?(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]
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0ql1RgjW-1589522887266)(images/unscented kalman filter/image-20200515103049178.png)]

考虑预测噪声为:
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,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个噪声状态。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-vKTgHZVQ-1589522887268)(images/unscented kalman filter/image-20200515103317714.png)]

原状态

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-m2NF2fsH-1589522887270)(images/unscented kalman filter/image-20200515103530611.png)]

扩展状态

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-tcjqgmS1-1589522887272)(images/unscented kalman filter/image-20200515103623171.png)]

扩展状态和协方差
生成预测点

现在我们生成了增广的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]的。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0LY89vP8-1589522887273)(images/unscented kalman filter/image-20200515104241253.png)]

计算预测的均值和方差

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

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-GQBJWF81-1589522887275)(images/unscented kalman filter/image-20200515111340105.png)]

更新处理

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

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

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

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

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=[1001000000]

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

  1. 毫米波雷达的预测映射到测量空间是非线性的,其表达式为:

( ρ ψ ρ ˙ ) = ( 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+1k(矩阵是[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+1k带入 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只是我们计算出来的,并不是来自传感器的数据。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-G01FJH3H-1589522887276)(images/unscented kalman filter/image-20200515120716123.png)]

状态转移矩阵h(x)

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-l4bY40P5-1589522887277)(images/unscented kalman filter/image-20200515121527085.png)]

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

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-b4irieP5-1589522887279)(images/unscented kalman filter/image-20200515121910243.png)]

更新状态

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

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-emE6hA9m-1589522887280)(images/unscented kalman filter/image-20200515122711923.png)]

这是UKF的最后阶段。这里我们最终根据 x x x预测值,和 z z z预测值求出kalman gain K k + 1 ∣ k K_{k+1|k} Kk+1kcross-correlation function T k + 1 ∣ k T_{k+1|k} Tk+1k然后最终更新状态和协方差,这些都是单纯的计算。而且state update和covariance matrix update都是跟标准卡尔曼滤波器一样的。 UKF独有的计算有kalman gaincross-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 就是一种权重)

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-VUZns5hu-1589522887282)(images/unscented kalman filter/image-20200515123129490.png)]

完整代码

事实上,这个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. 无迹卡尔曼滤波器完整公式推导
  • 59
    点赞
  • 358
    收藏
    觉得还不错? 一键收藏
  • 9
    评论
### 回答1: 车辆状态估计是指根据车辆传感器数据和先验信息,通过数学方法推测出车辆当前的状态信息,如位置、速度、方向等。扩展卡尔曼滤波(Extended Kalman Filter, EKF)和无迹卡尔曼滤波(Unscented Kalman Filter, UKF)是常用的状态估计算法。 EKF是对卡尔曼滤波算法的扩展,解决了非线性系统的状态估计问题。它通过一系列线性化技术来近似非线性系统,并根据线性化的模型进行滤波。EKF对非线性性能较强,但在高维状态空间或非线性程度较高的系统中计算复杂度较高。 UKF则是对EKF的改进,无需进行系统线性化。它通过一种称为无迹变换(unscented transformation)的方法,通过一组经过特定变换的采样点来近似系统的非线性变换。这种采样方法能够更好地保持状态向量的高斯分布特性,从而提高滤波精度。UKF适用于一些非线性程度较高或状态空间较大的问题,较EKF具有更好的性能和计算效率。 总而言之,扩展卡尔曼滤波(EKF)和无迹卡尔曼滤波UKF)是常用于车辆状态估计的算法。EKF通过线性化非线性系统来进行滤波,适用于中等复杂度的非线性问题。UKF则通过无迹变换来近似非线性变换,适用于非线性程度较高或状态空间较大的问题。根据具体的应用场景和系统特性,选择适当的算法可以提高车辆状态估计的精度和效率。 ### 回答2: 车辆状态估计是指通过利用车辆传感器提供的数据,推测车辆在特定时刻的位置、速度、方向等状态的过程。而扩展卡尔曼滤波(Extended Kalman Filter,EKF)和无迹卡尔曼滤波(Unscented Kalman Filter,UKF)是常用的车辆状态估计算法。 扩展卡尔曼滤波(EKF)是基于卡尔曼滤波的一种改进算法,可以用于非线性系统的状态估计。对于车辆的状态估计,EKF通过对车辆的运动模型和观测模型进行线性化,然后使用卡尔曼滤波的递推公式来进行状态的预测和更新。EKF通过不断迭代预测和更新步骤,逐步优化对车辆状态的估计。 无迹卡尔曼滤波UKF)是对EKF的一种改进算法,主要解决了EKF由于线性化误差引起的估计误差问题。UKF通过使用一组特定的采样点(称为Sigma点)来代替传统的线性化过程,以更准确地近似非线性系统的状态分布。通过对Sigma点进行预测和更新,UKF能够更好地估计车辆的状态。 总结而言,扩展卡尔曼滤波(EKF)和无迹卡尔曼滤波UKF)都是常用的车辆状态估计算法。它们通过对车辆的运动模型和观测模型进行线性化或者非线性化处理,通过迭代预测和更新的方式,对车辆的状态进行估计。其中,UKF通过使用一组特定的采样点来更准确地估计非线性系统的状态分布,相对于EKF具有更高的精度。 ### 回答3: 车辆状态估计是指对车辆的运动状态进行估计和预测的过程。在车辆动态系统中,状态包括位置、速度、加速度等信息,这些信息对于自动驾驶、智能交通等应用非常重要。 扩展卡尔曼滤波(Extended Kalman Filter,EKF)是一种基于卡尔曼滤波(Kalman Filter)的状态估计方法。EKF通过建立非线性运动方程和测量方程的雅可比矩阵,将非线性系统近似为线性系统进行状态估计。其主要思想是通过使用一阶泰勒展开对非线性方程进行线性化,得到近似的线性方程,然后利用卡尔曼滤波算法进行状态估计。由于EKF是对非线性系统的线性化近似,因此在系统非线性程度较高时,其估计精度可能会有所下降。 无迹卡尔曼滤波(Unscented Kalman Filter,UKF)是对EKF的改进和扩展。UKF通过使用一种特定的变换(无迹变换)将高斯分布转化为一组采样点,并在非线性方程中使用这些采样点来近似非线性函数的传播。无迹变换可以更好地保留非线性函数的特性,从而提高了状态估计的精度。相对于EKF而言,UKF在非线性程度高的情况下表现更加稳定和精确。 总之,EKetF和UkF是两种常用的车辆状态估计方法。EKetF是对非线性系统的线性化近似,而UKF通过无迹变换来更好地保留非线性函数的特性。在车辆状态估计应用中,选择合适的方法取决于系统的非线性程度和对估计精度的要求。
评论 9
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值