背景
上文讨论的EKF方式,通过对系统方程或者观测方程进行泰勒展开后,仅保留其一阶近似项,这样的做法不可避免的会造成一些误差,如果系统的非线性程度不是很强的话,那还好说,误差可以忽略,强非线性系统的话,这种误差就必须要考虑了;另外,每次都要进行一次雅克比矩阵计算,很多计算平台做偏导计算是不太容易实现的。
基于上面所述的两个缺点,UKF则完全没有这两个问题,不过本质的思想都是一样的,UKF使用无迹变换(Unscented Transform,UT)来处理非线性方程的均值和协方差的传递问题。
UKF的核心思想是对非线性函数的概率密度分布进行近似,用一系列的确定样本来逼近状态的后验概率密度。UKF没有把高阶项忽略,因此UKF有较高的精度。
无迹变换
无迹变换就是在估计点附近进行采样,使用这些样本点表示高斯分布近似状态的概率密度函数。其步骤为:
- 在原状态分布中,按照规则选取一定的采样点;
- 将这些采样点带入非线性函数中,得到非线性函数值点集,通过这些点集求取变换后的均值和协方差。
设一个非线性变换
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}
(Pi取
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 10−4≤α≤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=0∑2nwmiYiPYY=i=0∑2nwci[Yi−Yˉ][Yi−Yˉ]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计算步骤如下:
- 计算当前时刻的
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(k∣k)=[X^(k∣k)X^(k∣k)+(n+λ)P(k∣k)X^(k∣k)−(n+λ)P(k∣k)](1) - 预测当前时刻的
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+1∣k)=f[k,Xi(k∣k)](2) - 计算一步预测的均值和协方差矩阵
{ 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+1∣k)=i=0∑2nwiXi(k+1∣k)P(k+1∣k)=i=0∑2nwi[X^(k+1∣k)−Xi(k+1∣k)][X^(k+1∣k)−Xi(k+1∣k)]T+Q(3) - 根据一步预测值,再次使用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+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) - 将
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+1∣k)=i=0∑2nwiZi(k+1∣k)(5) - 由第五步得到
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+1∣k)=i=0∑2nwiZi(k+1∣k)Pzkzk=i=0∑2nwi[Zi(k+1∣k)−Zˉ(k+1∣k)][Zi(k+1∣k)−Zˉ(k+1∣k)]T+RPxkzk=i=0∑2nwi[Xi(k+1∣k)−Zˉ(k+1∣k)][Xi(k+1∣k)−Zˉ(k+1∣k)]T(6) - 计算卡尔曼增益矩阵
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)=PxkzkPzkzk−1(7) - 最后,计算系统的状态更新和协方差更新
{ 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+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)PzkzkK(k+1)T(8)
总结
至此,卡尔曼的三种滤波方式都学完了,线性系统就卡尔曼,非线性不强的系统就使用扩展卡尔曼,强非线性系统无迹卡尔曼。
卡尔曼的原理就不说了,第一篇有很详细的解释。
扩展卡尔曼利用泰勒展开对非线性系统进行拟合,在对展开的系统舍去高阶项,所以不可避免的有一些精度损失,所以不适用于强非线性系统。
无迹卡尔曼的思路则与扩展卡尔曼不同,在估计点附近进行UT变换,使用获得的 S i g m a Sigma Sigma点集的均值和协方差与原统计特征进行匹配,再把 S i g m a Sigma Sigma点集进行非线性映射,以近似得到状态概率密度函数,这种思路本质其实是一种统计近似。