0
点赞
收藏
分享

微信扫一扫

Apollo学习笔记(18)UKF

雨鸣静声 2022-04-01 阅读 64

背景

上文讨论的EKF方式,通过对系统方程或者观测方程进行泰勒展开后,仅保留其一阶近似项,这样的做法不可避免的会造成一些误差,如果系统的非线性程度不是很强的话,那还好说,误差可以忽略,强非线性系统的话,这种误差就必须要考虑了;另外,每次都要进行一次雅克比矩阵计算,很多计算平台做偏导计算是不太容易实现的。

基于上面所述的两个缺点,UKF则完全没有这两个问题,不过本质的思想都是一样的,UKF使用无迹变换(Unscented Transform,UT)来处理非线性方程的均值和协方差的传递问题。

UKF的核心思想是对非线性函数的概率密度分布进行近似,用一系列的确定样本来逼近状态的后验概率密度。UKF没有把高阶项忽略,因此UKF有较高的精度。

无迹变换

无迹变换就是在估计点附近进行采样,使用这些样本点表示高斯分布近似状态的概率密度函数。其步骤为:

  1. 在原状态分布中,按照规则选取一定的采样点;
  2. 将这些采样点带入非线性函数中,得到非线性函数值点集,通过这些点集求取变换后的均值和协方差。

设一个非线性变换 y = f ( x ) y=f(x) y=f(x),其中状态向量 x x x n n n维随机变量,并且已知其均值为 x ‾ \overline{x} x,协方差为 P P P
在这里插入图片描述
上图所示,即为UT变换,下面说一下,如何通过UT变换得到 2 n + 1 2n+1 2n+1 S i g m a Sigma Sigma χ \chi χ和相应的权重 ω \omega ω来计算 y y y的统计特征(上标表示为采样点的序列数,下标 m m m为均值,下标 c c c为协方差)。

第一步,计算当前时刻的 S i g m a Sigma Sigma点,
{ χ 0 = μ ,   i = 0 χ i = μ + ( ( n + λ ) P ) i ,   i = 1 , … , n χ i = μ − ( ( n + λ ) P ) i ,   i = n + 1 , … , 2 n \begin{cases} \chi^{0}=\mu ,\space i=0 \\ \chi^{i}=\mu+(\sqrt{(n+\lambda)P})_{i}, \space i=1 ,\dots , n \\ \chi^{i}=\mu-(\sqrt{(n+\lambda)P})_{i}, \space i=n+1 ,\dots , 2n \\ \end{cases} χ0=μ, i=0χi=μ+((n+λ)P )i, i=1,,nχi=μ((n+λ)P )i, i=n+1,,2n

上式中, n n n为系统状态的维度,
λ \lambda λ为缩放因子,
( ( n + λ ) P ) i (\sqrt{(n+\lambda)P})_{i} ((n+λ)P )i表示矩阵 ( n + λ ) P (n+\lambda)P (n+λ)P平方根的第 i i i列(当 P = A T A P=A^{T}A P=ATA时, ( P i (\sqrt{P}_{i} (P i A A A的第 i i i行;)。

第二步,计算下一时刻经非线性变换产生的预测采样点,
Y i = f ( χ i ) Y^{i}=f(\chi^{i}) Yi=f(χi)

第三步,计算对应的权重为
{ w m 0 = λ n + λ w m 0 = λ n + λ + ( 1 − α 2 + β ) w m i = w c i = 1 2 ( n + λ ) ,   i = 1 , … , 2 n \begin{cases} w^{0}_{m}=\frac{\lambda}{n+\lambda} \\ w^{0}_{m}=\frac{\lambda}{n+\lambda}+(1-\alpha^{2}+\beta) \\ w^{i}_{m}=w^{i}_{c}=\frac{1}{2(n+\lambda)}, \space i=1 ,\dots , 2n \end{cases} wm0=n+λλwm0=n+λλ+(1α2+β)wmi=wci=2(n+λ)1, i=1,,2n

其中,缩放因子 λ = α ( n + k ) − n \lambda=\alpha(n+k)-n λ=α(n+k)n用来降低总的预测误差, α \alpha α决定了采样点的分布状态,一般取值范围为 1 0 − 4 ≤ α ≤ 1 10^{-4} \le \alpha\le 1 104α1;至于 k k k的选取则没有太多的限制,但是起码要保证 ( n + λ ) P (n+\lambda)P (n+λ)P为半正定矩阵,一般都是选取 n + k = 3 n+k=3 n+k=3;参数 β ≥ 0 \beta\ge0 β0,可以通过调节此项提高方差的精度,把高阶项的影响考虑进来,对于正态分布,选取 β = 2 \beta=2 β=2

第四步,计算非线性变换后的均值和协方差,
{ Y ˉ = ∑ i = 0 2 n w m i Y i P Y Y = ∑ i = 0 2 n w c i [ Y i − Y ˉ ] [ Y i − Y ˉ ] T \begin{cases} \bar{Y}=\displaystyle\sum_{i=0}^{2n}w^{i}_{m}Y^{i} \\ P_{YY}=\displaystyle\sum_{i=0}^{2n}w^{i}_{c}[Y^{i}-\bar{Y}][Y^{i}-\bar{Y}]^{T} \end{cases} Yˉ=i=02nwmiYiPYY=i=02nwci[YiYˉ][YiYˉ]T

UKF的实现

由具有高斯白噪声 W ( k ) W(k) W(k)的随机变量 X X X和具有高斯白噪声 V ( k ) V(k) V(k)的观测变量 Z Z Z构成的非线性系统,如下所示,
{ X ( k + 1 ) = f ( X ( k ) , W ( k ) ) Z ( k ) = h ( X ( k ) , V ( k ) ) \begin{cases} X(k+1)=f(X(k),W(k)) \\ Z(k)=h(X(k),V(k)) \end{cases} {X(k+1)=f(X(k),W(k))Z(k)=h(X(k),V(k))
式中, f f f为非线性状态方程函数, h h h为观测方程函数, W ( k ) W(k) W(k)的协方差矩阵为 Q Q Q V ( k ) V(k) V(k)的协方差矩阵为 R R R

具体的UKF计算步骤如下:

  1. 计算当前时刻的 S i g m a Sigma Sigma
    X i ( k ∣ k ) = [ X ^ ( k ∣ k ) X ^ ( k ∣ k ) + ( n + λ ) P ( k ∣ k ) X ^ ( k ∣ k ) − ( n + λ ) P ( k ∣ k ) ] (1) X^{i}(k|k)=[\hat{X}(k|k) \hspace{3mm} \hat{X}(k|k)+\sqrt{(n+\lambda)P(k|k)} \hspace{3mm} \hat{X}(k|k)-\sqrt{(n+\lambda)P(k|k)}] \tag{1} Xi(kk)=[X^(kk)X^(kk)+(n+λ)P(kk) X^(kk)(n+λ)P(kk) ](1)
  2. 预测当前时刻的 S i g m a Sigma Sigma点集的一步预测
    X i ( k + 1 ∣ k ) = f [ k , X i ( k ∣ k ) ] (2) X^{i}(k+1|k)=f[k,X^{i}(k|k)] \tag{2} Xi(k+1k)=f[k,Xi(kk)](2)
  3. 计算一步预测的均值和协方差矩阵
    { X ^ ( k + 1 ∣ k ) = ∑ i = 0 2 n w i X i ( k + 1 ∣ k ) P ( k + 1 ∣ k ) = ∑ i = 0 2 n w i [ X ^ ( k + 1 ∣ k ) − X i ( k + 1 ∣ k ) ] [ X ^ ( k + 1 ∣ k ) − X i ( k + 1 ∣ k ) ] T + Q (3) \begin{cases} \hat{X}(k+1|k)=\displaystyle\sum_{i=0}^{2n}w^{i}X^{i}(k+1|k) \\ P(k+1|k)=\displaystyle\sum_{i=0}^{2n}w^{i}[\hat{X}(k+1|k)-X^{i}(k+1|k)][\hat{X}(k+1|k)-X^{i}(k+1|k)]^{T}+Q \tag{3} \end{cases} X^(k+1k)=i=02nwiXi(k+1k)P(k+1k)=i=02nwi[X^(k+1k)Xi(k+1k)][X^(k+1k)Xi(k+1k)]T+Q(3)
  4. 根据一步预测值,再次使用UT变换,产生新的 S i g m a Sigma Sigma点集
    X i ( k + 1 ∣ k ) = [ X ^ ( k + 1 ∣ k ) X ^ ( k + 1 ∣ k ) + ( n + λ ) P ( k + 1 ∣ k ) X ^ ( k + 1 ∣ k ) − ( n + λ ) P ( k + 1 ∣ k ) ] (4) X^{i}(k+1|k)=[\hat{X}(k+1|k) \hspace{2mm} \hat{X}(k+1|k)+\sqrt{(n+\lambda)P(k+1|k)} \hspace{2mm} \hat{X}(k+1|k)-\sqrt{(n+\lambda)P(k+1|k)}] \tag{4} Xi(k+1k)=[X^(k+1k)X^(k+1k)+(n+λ)P(k+1k) X^(k+1k)(n+λ)P(k+1k) ](4)
  5. k + 1 k+1 k+1时刻的 S i g m a Sigma Sigma点集带入观测方程,得到预测的观测量,
    Z i ( k + 1 ∣ k ) = ∑ i = 0 2 n w i Z i ( k + 1 ∣ k ) (5) Z^{i}(k+1|k)=\displaystyle\sum_{i=0}^{2n}w^{i}Z^{i}(k+1|k) \tag{5} Zi(k+1k)=i=02nwiZi(k+1k)(5)
  6. 由第五步得到 S i g m a Sigma Sigma点集的观测预测值,通过加权求和得到系统预测的均值以及协方差
    { Z ˉ ( k + 1 ∣ k ) = ∑ i = 0 2 n w i Z i ( k + 1 ∣ k ) P z k z k = ∑ i = 0 2 n w i [ Z i ( k + 1 ∣ k ) − Z ˉ ( k + 1 ∣ k ) ] [ Z i ( k + 1 ∣ k ) − Z ˉ ( k + 1 ∣ k ) ] T + R P x k z k = ∑ i = 0 2 n w i [ X i ( k + 1 ∣ k ) − Z ˉ ( k + 1 ∣ k ) ] [ X i ( k + 1 ∣ k ) − Z ˉ ( k + 1 ∣ k ) ] T (6) \begin{cases} \bar{Z}(k+1|k)=\displaystyle\sum_{i=0}^{2n}w^{i}Z^{i}(k+1|k) \\ P_{z_{k}z_{k}}=\displaystyle\sum_{i=0}^{2n}w^{i}[Z^{i}(k+1|k)-\bar{Z}(k+1|k)][Z^{i}(k+1|k)-\bar{Z}(k+1|k)]^{T}+R \\ P_{x_{k}z_{k}}=\displaystyle\sum_{i=0}^{2n}w^{i}[X^{i}(k+1|k)-\bar{Z}(k+1|k)][X^{i}(k+1|k)-\bar{Z}(k+1|k)]^{T} \tag{6} \end{cases} Zˉ(k+1k)=i=02nwiZi(k+1k)Pzkzk=i=02nwi[Zi(k+1k)Zˉ(k+1k)][Zi(k+1k)Zˉ(k+1k)]T+RPxkzk=i=02nwi[Xi(k+1k)Zˉ(k+1k)][Xi(k+1k)Zˉ(k+1k)]T(6)
  7. 计算卡尔曼增益矩阵
    K ( k + 1 ) = P x k z k P z k z k − 1 (7) K(k+1)=P_{x_{k}z_{k}}P_{z_{k}z_{k}}^{-1} \tag{7} K(k+1)=PxkzkPzkzk1(7)
  8. 最后,计算系统的状态更新和协方差更新

{ X ^ ( k + 1 ∣ k + 1 ) = X ^ ( k + 1 ∣ k ) + K ( k + 1 ) [ Z ( k + 1 ) − Z ^ ( k + 1 ∣ k ) ] P ( k + 1 ∣ k + 1 ) = P ( k + 1 ∣ k ) − K ( k + 1 ) P z k z k K ( k + 1 ) T (8) \begin{cases} \hat{X}(k+1|k+1)=\hat{X}(k+1|k)+K(k+1)[Z(k+1)-\hat{Z}(k+1|k)] \\ P(k+1|k+1)=P(k+1|k)-K(k+1)P_{z_{k}z_{k}}K(k+1)^{T} \tag{8} \end{cases} {X^(k+1k+1)=X^(k+1k)+K(k+1)[Z(k+1)Z^(k+1k)]P(k+1k+1)=P(k+1k)K(k+1)PzkzkK(k+1)T(8)

总结

至此,卡尔曼的三种滤波方式都学完了,线性系统就卡尔曼,非线性不强的系统就使用扩展卡尔曼,强非线性系统无迹卡尔曼。

卡尔曼的原理就不说了,第一篇有很详细的解释。

扩展卡尔曼利用泰勒展开对非线性系统进行拟合,在对展开的系统舍去高阶项,所以不可避免的有一些精度损失,所以不适用于强非线性系统。

无迹卡尔曼的思路则与扩展卡尔曼不同,在估计点附近进行UT变换,使用获得的 S i g m a Sigma Sigma点集的均值和协方差与原统计特征进行匹配,再把 S i g m a Sigma Sigma点集进行非线性映射,以近似得到状态概率密度函数,这种思路本质其实是一种统计近似。

举报

相关推荐

0 条评论