1、白噪声条件下的卡尔曼滤波器的一般形式
前面的文章介绍了卡尔曼滤波的基本方程和一些简单的应用实例。其实,只用基本方程就已经能够适用于很多场合了,工程上用的最多的也就是这个基本方程而已。那为什么还会有一般形式呢。这个问题的提出是系统状态方程是具有一般化形式的。
我们已经知道,之前介绍的卡尔曼滤波所用到的状态方程只有状态转移和噪声驱动。而很多系统除了这些,它其实还有一项,就是控制项,忘了的同学自己去翻控制理论。
状态方程的一般形式应该是下面这个样子:
X
k
=
Φ
k
,
k
−
1
X
k
−
1
+
B
k
−
1
u
k
−
1
+
Γ
k
−
1
W
k
−
1
X_k=\Phi_{k,k-1}X_{k-1}+B_{k-1}u_{k-1}+\Gamma_{k-1}W_{k-1}
Xk=Φk,k−1Xk−1+Bk−1uk−1+Γk−1Wk−1
那个
u
k
−
1
u_{k-1}
uk−1就是控制项,是已知的确定的输入。
量测方程除了白噪声误差外,还可以有一个确定的已知的系统性偏差(仪器零偏那样的),于是量测方程变成
Z
k
=
H
k
X
k
+
Y
k
+
V
k
Z_k=H_kX_k+Y_k+V_k
Zk=HkXk+Yk+Vk
那个
Y
k
Y_k
Yk就是量测的系统性偏差。而且,为了让系统更具有一般性,我们让系统噪声和量测噪声在同时刻相关。
E
[
W
k
V
k
T
]
=
S
k
E[W_kV_k^T]=S_k
E[WkVkT]=Sk
R
k
R_k
Rk和
Q
k
Q_k
Qk的定义与之前相同。
说实话,这事儿变得相当麻烦,因为状态噪声与量测噪声相关了。再建立卡尔曼滤波的预测方程、估计方程、增益方程和方差方程需要一些处理了。先想办法将相关的噪声归置归置,变成两个不相关的噪声才可以。处理过程有些麻烦,有兴趣的同学自己去查资料吧,编公式很烦的,我就直接出结论了。
先定义
J
k
=
Γ
k
S
k
R
k
−
1
J_k=\Gamma_kS_kR_k^{-1}
Jk=ΓkSkRk−1 于是
X
^
k
/
k
−
1
=
Φ
k
,
k
−
1
X
^
k
−
1
+
B
k
−
1
u
k
−
1
+
J
k
−
1
(
Z
k
−
1
−
Y
k
−
1
−
H
k
−
1
X
^
k
−
1
)
\hat X_{k/k-1}=\Phi_{k,k-1}\hat X_{k-1}+B_{k-1}u_{k-1}+J_{k-1}(Z_{k-1}-Y_{k-1}-H_{k-1}\hat X_{k-1})
X^k/k−1=Φk,k−1X^k−1+Bk−1uk−1+Jk−1(Zk−1−Yk−1−Hk−1X^k−1)
X
^
k
=
X
^
k
/
k
−
1
+
K
k
(
Z
k
−
Y
k
−
H
k
X
^
k
/
k
−
1
)
\hat X_k=\hat X_{k/k-1}+K_k(Z_k-Y_k-H_k\hat X_{k/k-1})
X^k=X^k/k−1+Kk(Zk−Yk−HkX^k/k−1)
K
k
=
P
k
/
k
−
1
H
k
T
(
H
k
P
k
/
k
−
1
H
k
T
+
R
k
)
−
1
K_k=P_{k/k-1}H_k^T(H_kP_{k/k-1}H_k^T+R_k)^{-1}
Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1
P
k
/
k
−
1
=
(
Φ
k
,
k
−
1
−
J
k
−
1
H
k
−
1
)
P
k
−
1
(
Φ
k
,
k
−
1
−
J
k
−
1
H
k
−
1
)
T
+
Γ
k
−
1
Q
k
−
1
Γ
k
−
1
T
−
J
k
−
1
S
k
−
1
T
Γ
k
−
1
T
P_{k/k-1}=(\Phi_{k,k-1}-J_{k-1}H_{k-1})P_{k-1}(\Phi_{k,k-1}-J_{k-1}H_{k-1})^T+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T-J_{k-1}S_{k-1}^T\Gamma_{k-1}^T
Pk/k−1=(Φk,k−1−Jk−1Hk−1)Pk−1(Φk,k−1−Jk−1Hk−1)T+Γk−1Qk−1Γk−1T−Jk−1Sk−1TΓk−1T
P
k
=
(
I
−
K
k
H
k
)
P
k
/
k
−
1
P_k=(I-K_kH_k)P_{k/k-1}
Pk=(I−KkHk)Pk/k−1
这套方程就是卡尔曼滤波器的一般形式,它适合于系统具有可以确定的控制输入和状态噪声与量测噪声相关的情况。也是五个方程,跟估计相关的方程基本没变,跟预测的方程都变复杂了。
顺便说一下,我觉得我在后面的文章中,能用到卡尔曼滤波一般形式的情况较少,所以在举例子和说明情况是,还是以基本方程为主。
2、有色噪声下的卡尔曼滤波
系统噪声为有色噪声时的情况
还是那个系统方程和量测方程。
X
k
+
1
=
Φ
k
+
1
,
k
X
k
+
Γ
k
W
k
X_{k+1}=\Phi_{k+1,k}X_k+\Gamma _kW_k
Xk+1=Φk+1,kXk+ΓkWk
Z
k
=
H
k
X
k
+
V
k
Z_k=H_kX_k+V_k
Zk=HkXk+Vk
前面讨论的卡尔曼滤波时要求系统噪声
W
k
W_k
Wk是白噪声。而如果系统噪声不满足白噪声,那又该如何?
不是白噪声就是有色噪声了。而对于有色噪声我们知道它一般都可以写成下面这个样子:
W
k
+
1
=
Π
k
+
1
,
k
W
k
+
ζ
k
W_{k+1}=\Pi_{k+1,k}W_k+\zeta_k
Wk+1=Πk+1,kWk+ζk
而
ζ
k
\zeta_k
ζk是零均值的白噪声。
这就简单了,我们可以把
W
k
W_k
Wk也当成状态量。于是重新定义系统状态量:
X
k
a
=
(
X
k
W
k
)
X_k^a=\begin{pmatrix}X_k\\W_k\end{pmatrix}
Xka=(XkWk)
这样扩展后的系统状态方程就会变成下面这个样子:
(
X
k
+
1
W
k
+
1
)
=
(
Φ
k
+
1
,
k
Γ
k
0
Π
k
+
1
,
k
)
(
X
k
W
k
)
+
(
0
I
)
ζ
k
\begin{pmatrix}X_{k+1}\\W_{k+1}\end{pmatrix}=\begin{pmatrix}\Phi_{k+1,k}&\Gamma_k\\0&\Pi_{k+1,k}\end{pmatrix}\begin{pmatrix}X_k\\W_k\end{pmatrix}+\begin{pmatrix}0\\I\end{pmatrix}\zeta_k
(Xk+1Wk+1)=(Φk+1,k0ΓkΠk+1,k)(XkWk)+(0I)ζk
量测方程也要扩展了:
Z
k
=
(
H
k
0
)
(
X
k
W
k
)
+
V
k
Z_k=\begin{pmatrix}H_k&0\end{pmatrix}\begin{pmatrix}X_k\\W_k\end{pmatrix}+V_k
Zk=(Hk0)(XkWk)+Vk
这不就是:
X
k
+
1
a
=
Φ
k
+
1
,
k
a
X
k
a
+
Γ
k
a
W
k
a
X_{k+1}^a=\Phi_{k+1,k}^aX_k^a+\Gamma _k^aW_k^a
Xk+1a=Φk+1,kaXka+ΓkaWka
Z
k
=
H
k
a
X
k
a
+
V
k
Z_k=H_k^aX_k^a+V_k
Zk=HkaXka+Vk
后面就不用我多说了吧。把这个当成新的状态方程和量测方程,用卡尔曼滤波的一般方程就好了。
量测噪声为有色噪声的情形
上面讨论的是系统噪声是有色的,那如果量测噪声是有色的呢?就是
Z
k
=
H
k
X
k
+
V
k
Z_k=H_kX_k+V_k
Zk=HkXk+Vk 里面
V
k
+
1
=
Ψ
k
+
1
,
k
V
k
+
ξ
k
V_{k+1}=\Psi_{k+1,k}V_k+\xi_k
Vk+1=Ψk+1,kVk+ξk
这里面那个
ξ
k
\xi_k
ξk是零均值的白噪声。
跟上面的系统噪声处理方法一样的吧。我们还是把这个有色噪声放到状态量里面去试试:
(
X
k
+
1
V
k
+
1
)
=
(
Φ
k
+
1
,
k
0
0
Ψ
k
+
1
,
k
)
(
X
k
V
k
)
+
(
Γ
k
0
0
I
)
(
W
k
ξ
k
)
\begin{pmatrix}X_{k+1}\\V_{k+1}\end{pmatrix}=\begin{pmatrix}\Phi_{k+1,k}&0\\0&\Psi_{k+1,k}\end{pmatrix}\begin{pmatrix}X_k\\V_k\end{pmatrix}+\begin{pmatrix}\Gamma_k&0\\0&I\end{pmatrix}\begin{pmatrix}W_k\\\xi_k\end{pmatrix}
(Xk+1Vk+1)=(Φk+1,k00Ψk+1,k)(XkVk)+(Γk00I)(Wkξk)
好的,还挺顺利,下一步是量测方程的扩展:
Z
k
=
(
H
k
I
)
(
X
k
V
k
)
Z_k=\begin{pmatrix}H_k&I\end{pmatrix}\begin{pmatrix}X_k\\V_k\end{pmatrix}
Zk=(HkI)(XkVk)
等一下,扩展完了以后,量测方程没噪声了。也就是R阵等于0。而在卡尔曼滤波方程的增益矩阵K的求解和推导过程是必须要求R阵不能为0的。所以,这么扩展后的量测方程是不能套用卡尔曼滤波的。那怎么办?这时候,我们可能需要重新定义量测量了,想办法让量测量具有量测噪声是白噪声的特性。
定义:
Z
k
∗
=
Z
k
+
1
−
Ψ
k
+
1
,
k
Z
k
Z_k^*=Z_{k+1}-\Psi_{k+1,k}Z_k
Zk∗=Zk+1−Ψk+1,kZk
然后根据前面的状态方程和量测方程进行代入和推导,会发现:
Z
k
+
1
−
Ψ
k
+
1
,
k
Z
k
=
(
H
k
+
1
Φ
k
+
1
,
k
−
Ψ
k
+
1
,
k
H
k
)
X
k
+
H
k
+
1
Γ
k
W
k
+
ξ
k
Z_{k+1}-\Psi_{k+1,k}Z_k=(H_{k+1}\Phi_{k+1,k}-\Psi_{k+1,k}H_k)X_k+H_{k+1}\Gamma_kW_k+\xi_k
Zk+1−Ψk+1,kZk=(Hk+1Φk+1,k−Ψk+1,kHk)Xk+Hk+1ΓkWk+ξk
我们把
(
H
k
+
1
Φ
k
+
1
,
k
−
Ψ
k
+
1
,
k
H
k
)
(H_{k+1}\Phi_{k+1,k}-\Psi_{k+1,k}H_k)
(Hk+1Φk+1,k−Ψk+1,kHk)定义成
H
k
∗
H_k^*
Hk∗,
(
H
k
+
1
Γ
k
W
k
+
ξ
k
)
(H_{k+1}\Gamma_kW_k+\xi_k)
(Hk+1ΓkWk+ξk)定义成
V
k
∗
V_k^*
Vk∗,这样,就会重新建立一个量测方程:
Z
k
∗
=
H
k
∗
X
k
+
V
k
∗
Z_k^*=H_k^*X_k+V_k^*
Zk∗=Hk∗Xk+Vk∗
而且,可以证明
V
k
∗
V_k^*
Vk∗是零均值,而且是白噪声。它的方差为:
R
k
∗
=
H
k
+
1
Γ
k
Q
k
Γ
k
T
H
k
+
1
T
+
R
k
R_k^*=H_{k+1}\Gamma_kQ_k\Gamma_k^TH_{k+1}^T+R_k
Rk∗=Hk+1ΓkQkΓkTHk+1T+Rk
这样不就很好了吗,系统的状态方程没变,量测方程重新写了,而且量测噪声满足白噪声的要求,可以用卡尔曼滤波了吧。
呵呵,还不行,因为我们这么处理得到的量测噪声
V
k
∗
=
H
k
+
1
Γ
k
W
k
+
ξ
k
V_k^*=H_{k+1}\Gamma_kW_k+\xi_k
Vk∗=Hk+1ΓkWk+ξk,它里面包含了了系统噪声
W
k
W_k
Wk。稍微想一下就可以猜到,这时的量测噪声和系统噪声是一定相关的。
事实也的确如此,这两个噪声的相关矩阵长这个样子:
S
k
=
Q
k
Γ
k
T
H
k
+
1
T
S_k=Q_k\Gamma_k^TH_{k+1}^T
Sk=QkΓkTHk+1T 而卡尔曼滤波的基本方程要求是系统噪声和量测噪声不相关。既然他俩相关,就要用到卡尔曼滤波的一般形式了。在进行化简。过程这里就不罗嗦了,直接给出结论:
H
k
∗
=
H
k
+
1
Φ
k
+
1
,
k
−
Ψ
k
+
1
,
k
H
k
H_k^*=H_{k+1}\Phi_{k+1,k}-\Psi_{k+1,k}H_k
Hk∗=Hk+1Φk+1,k−Ψk+1,kHk
R
k
∗
=
H
k
+
1
Γ
k
Q
k
Γ
k
T
H
k
+
1
T
+
R
k
R_k^*=H_{k+1}\Gamma_kQ_k\Gamma_k^TH_{k+1}^T+R_k
Rk∗=Hk+1ΓkQkΓkTHk+1T+Rk
S
k
=
Q
k
Γ
k
T
H
k
+
1
T
S_k=Q_k\Gamma_k^TH_{k+1}^T
Sk=QkΓkTHk+1T
X
^
k
+
1
=
Φ
k
+
1
,
k
X
^
k
+
K
‾
k
+
1
(
Z
k
+
1
−
Ψ
k
+
1
,
k
Z
k
−
H
k
∗
X
^
k
)
\hat X_{k+1}=\Phi_{k+1,k}\hat X_k+\overline K_{k+1}(Z_{k+1}-\Psi_{k+1,k}Z_k-H_k^*\hat X_k)
X^k+1=Φk+1,kX^k+Kk+1(Zk+1−Ψk+1,kZk−Hk∗X^k)
K
‾
k
+
1
=
(
Φ
k
+
1
,
k
P
k
H
k
∗
T
+
Γ
k
S
k
)
(
H
k
∗
P
k
H
k
∗
T
+
R
k
∗
)
−
1
\overline K_{k+1} = (\Phi_{k+1,k}P_kH_k^{*T}+\Gamma_kS_k)(H_k^*P_kH_k^{*T}+R_k^*)^{-1}
Kk+1=(Φk+1,kPkHk∗T+ΓkSk)(Hk∗PkHk∗T+Rk∗)−1
P
k
+
1
=
Φ
k
+
1
,
k
P
k
Φ
k
+
1
,
k
T
+
Γ
k
Q
k
Γ
k
T
−
K
‾
k
+
1
(
H
k
∗
P
k
Φ
k
+
1
,
k
T
+
S
k
T
Γ
k
T
)
P_{k+1}=\Phi_{k+1,k}P_k\Phi_{k+1,k}^T+\Gamma_kQ_k\Gamma_k^T-\overline K_{k+1}(H_k^*P_k\Phi_{k+1,k}^T+S_k^T\Gamma_k^T)
Pk+1=Φk+1,kPkΦk+1,kT+ΓkQkΓkT−Kk+1(Hk∗PkΦk+1,kT+SkTΓkT)
上面这六个就是量测噪声为有色噪声时的全套卡尔曼滤波方程。虽然状态量和观测量的个数并没有增加,但是计算量确实增加了不少。
剩下的就是初值问题了,也就是如何确定
X
^
0
\hat X_0
X^0和
P
0
P_0
P0。最保险的办法是利用初始的量测
Z
0
Z_0
Z0对它进行线性最小方差估计。还记得估值公式吗,不记得的话翻前面的文章。
3、小结
我们已经把卡尔曼滤波器的基本方程,初值选取,离散化处理,一般形式,以及有色噪声的处理都介绍了一遍了。看没看懂都给点个赞哈。后面,我希望从算法优化的角度,进一步鼓捣卡尔曼滤波的各个方程,因为,对于这套算法来讲,太耗资源了,比如矩阵求逆这一部分。后面的文章将讨论有没有可以避免大号矩阵求逆的方法等一些工程上经常遇到的问题。