广义(通用)卡尔曼-布什(Kalman-Bucy)滤波器
本文共1.6万字,所有文本和markdown公式都手动码字完成。分享和码字不易,希望得到您的一个收藏点赞和关注,您的支持就是我更新的最大动力!
1. Kalman-Bucy滤波器及系统背景
Kalman-Bucy滤波器本质上是一种针对线性系统的滤波器,可以在系统中存在高斯白噪声时对系统的内禀信号进行滤波,从而得到相对纯净、相对接近内禀信号的测量信号。
设滤波器的输出信号(亦即测量信号)为 Y ( t ) \mathbf{Y}(t) Y(t),滤波器的输入信号(即系统的内禀信号,或称有用信号)为 X ( t ) \mathbf{X}(t) X(t)。滤波器的作用,就是将系统中的状态量 X ( t ) \mathbf{X}(t) X(t)在噪声影响下,借助测量值 Y ( t ) \mathbf{Y}(t) Y(t),正确估计出来系统中的内禀(有用)信号。
系统中的有用信号可以表示为
X
˙
(
t
)
=
A
(
t
)
X
(
t
)
+
G
(
t
)
N
1
(
t
)
,
A
∈
R
n
×
n
,
G
∈
R
n
×
p
(1)
\mathbf{\dot X} (t) = \mathbf{A}(t) \mathbf{X}(t) + \mathbf{G}(t)\mathbf{N}_1(t), \quad \mathbf{A} \in \mathbb{R}^{ n \times n}, \mathbf{G} \in \mathbb{R}^{n \times p} \tag{1}
X˙(t)=A(t)X(t)+G(t)N1(t),A∈Rn×n,G∈Rn×p(1)其中,初始条件为
X
(
t
0
)
=
X
0
\mathbf{X}(t_0) = \mathbf{X}^0
X(t0)=X0,
N
1
(
t
)
\mathbf{N}_1(t)
N1(t)为高斯白噪声,是系统在运行中所具有的噪声,具有如下期望值和方差:
M
{
N
1
(
t
)
}
=
0
;
R
N
1
N
1
(
t
,
τ
)
=
M
{
N
1
(
t
)
N
1
T
(
t
)
}
=
S
1
(
t
)
δ
(
t
−
τ
)
M\left\{ \mathbf{N}_1(t) \right\} = 0; \quad \mathbf{R}_{\mathbf{N}_1 \mathbf{N}_1} (t, \tau) = M\left\{ \mathbf{N}_1(t) \mathbf{N}_1^{\rm T}(t) \right\} = \mathbf{S}_1 (t) \delta (t - \tau)
M{N1(t)}=0;RN1N1(t,τ)=M{N1(t)N1T(t)}=S1(t)δ(t−τ)其中
S
1
(
t
)
\mathbf{S}_1(t)
S1(t)为表征噪声信号强度的对称正定矩阵,这意味着
S
1
T
(
t
)
=
S
1
(
t
)
\mathbf{S}_1^{\rm T}(t) = \mathbf{S}_1(t)
S1T(t)=S1(t)。
另外,假设有用信号初值的期望已知:
X
ˉ
0
=
M
{
X
0
}
=
0
\bar{ \mathbf{X} }^0 = M\left\{ \mathbf{X}^0 \right\} = 0
Xˉ0=M{X0}=0方差已知:
D
00
=
M
{
(
X
0
−
X
ˉ
0
)
(
X
0
−
X
ˉ
0
)
T
}
=
0
\mathbf{D}_{00} = M\left\{ \left( \mathbf{X}^0 - \bar{\mathbf{X}}^0 \right) \left( \mathbf{X}^0 - \bar{\mathbf{X}}^0 \right)^{\rm T} \right\} = 0
D00=M{(X0−Xˉ0)(X0−Xˉ0)T}=0
再设系统的输出信号
Y
(
t
)
\mathbf{Y}(t)
Y(t)具有如下形式
Y
(
t
)
=
C
(
t
)
X
(
t
)
+
N
2
(
t
)
,
C
∈
R
l
×
n
(2)
\mathbf{Y}(t) = \mathbf{C}(t) \mathbf{X}(t) + \mathbf{N}_2(t), \quad \mathbf{C} \in \mathbb{R}^{ l \times n} \tag{2}
Y(t)=C(t)X(t)+N2(t),C∈Rl×n(2)其中
N
2
(
t
)
\mathbf{N}_2(t)
N2(t)亦为高斯白噪声,是测量时的测量噪声:
M
{
N
2
(
t
)
}
=
0
;
R
N
2
N
2
(
t
,
τ
)
=
M
{
N
2
(
t
)
N
2
T
(
t
)
}
=
S
2
(
t
)
δ
(
t
−
τ
)
M\left\{ \mathbf{N}_2(t) \right\} = 0; \quad \mathbf{R}_{\mathbf{N}_2 \mathbf{N}_2} (t, \tau) = M\left\{ \mathbf{N}_2(t) \mathbf{N}_2^{\rm T}(t) \right\} = \mathbf{S}_2 (t) \delta (t - \tau)
M{N2(t)}=0;RN2N2(t,τ)=M{N2(t)N2T(t)}=S2(t)δ(t−τ)设滤波器的滤波误差为
X
σ
(
t
)
=
X
(
t
)
−
X
^
(
t
)
\mathbf{X}_\sigma (t) = \mathbf{X}(t) - \hat{\mathbf{X} } (t)
Xσ(t)=X(t)−X^(t)滤波器的设计应当满足如下条件,使得滤波器得到的状态估计量为无偏的,且估计误差的均方差
M
{
∥
X
σ
(
t
)
∥
2
}
M\left\{ \Vert \mathbf{X}_\sigma (t) \Vert ^2 \right\}
M{∥Xσ(t)∥2}最小。
2. 系统中相关关系整理
(1) 假设有用信号初值
X
0
\mathbf{X}^0
X0与系统噪声
N
1
(
t
)
\mathbf{N}_1(t)
N1(t)、测量噪声
N
2
(
t
)
\mathbf{N}_2(t)
N2(t)之间无关:
M
{
X
0
N
1
}
=
0
,
M
{
X
0
N
2
}
=
0
M\left\{ \mathbf{X}^0 \mathbf{N}_1 \right\} = 0, \quad M\left\{ \mathbf{X}^0 \mathbf{N}_2 \right\} = 0
M{X0N1}=0,M{X0N2}=0(2) 而假设系统噪声
N
1
(
t
)
\mathbf{N}_1(t)
N1(t)与测量噪声
N
2
(
t
)
\mathbf{N}_2(t)
N2(t)之间具有如下相关关系(注意!通常认为二者不具有相关关系,但此处二者是具有相关关系的!)
M
{
N
1
(
t
)
N
2
T
(
τ
)
}
=
S
3
(
t
)
δ
(
t
−
τ
)
M\left\{ \mathbf{N}_1(t) \mathbf{N}_2^{\rm T}(\tau) \right\} = \mathbf{S}_3 (t) \delta (t - \tau)
M{N1(t)N2T(τ)}=S3(t)δ(t−τ)其中
S
3
(
t
)
\mathbf{S}_3 (t)
S3(t)为对称正定矩阵,因此
S
3
T
(
t
)
=
S
3
(
t
)
\mathbf{S}_3^{\rm T}(t) = \mathbf{S}_3(t)
S3T(t)=S3(t)。
(3) 式(1)给出了系统的微分方程表达式。当然,可以借助状态转移矩阵的思想,将(1)式两边积分,就可以得到系统状态
X
(
t
)
\mathbf{X}(t)
X(t)在任意时刻
∀
t
≥
t
0
\forall t \geq t_0
∀t≥t0的值:
X
(
t
)
=
Φ
(
t
,
t
0
)
X
(
t
0
)
+
∫
t
0
t
Φ
(
t
,
s
)
G
(
s
)
N
1
(
s
)
d
s
\mathbf{X}(t) = \mathbf{\Phi}(t, t_0) \mathbf{X}(t_0) + \int_{t_0}^t \mathbf{\Phi}(t, {\rm s}) \mathbf{G}({\rm s}) \mathbf{N}_1({\rm s}) d{\rm s}
X(t)=Φ(t,t0)X(t0)+∫t0tΦ(t,s)G(s)N1(s)ds其中
Φ
(
t
,
t
0
)
\mathbf{\Phi}(t, t_0)
Φ(t,t0)为状态转移矩阵,利用它和初值可以得到任意时刻的状态值。那么,可以求解状态量
X
(
t
)
\mathbf{X}(t)
X(t)和测量噪声
N
2
(
t
)
\mathbf{N}_2(t)
N2(t)之间的相关关系:
M
{
X
(
t
)
N
2
T
(
τ
)
}
=
M
{
Φ
(
t
,
t
0
)
X
(
t
0
)
N
2
T
(
τ
)
+
∫
t
0
t
Φ
(
t
,
s
)
G
(
s
)
N
1
(
s
)
d
s
N
2
T
(
τ
)
}
=
Φ
(
t
,
t
0
)
M
{
X
(
t
0
)
N
2
T
(
τ
)
}
+
∫
t
0
t
Φ
(
t
,
s
)
G
(
s
)
M
{
N
1
(
s
)
N
2
T
(
τ
)
}
d
s
=
0
+
∫
t
0
t
Φ
(
t
,
s
)
G
(
s
)
⋅
S
3
(
t
)
δ
(
t
−
τ
)
d
s
=
Φ
(
t
,
τ
)
G
(
τ
)
S
3
(
τ
)
,
(
t
0
≤
τ
≤
t
)
\begin{aligned} M\left\{ \mathbf{X}(t) \mathbf{N}_2^{\rm T} (\tau) \right\} &= M\left\{ \mathbf{\Phi}(t, t_0) \mathbf{X}(t_0) \mathbf{N}_2^{\rm T} (\tau) + \int_{t_0}^t \mathbf{\Phi}(t, {\rm s}) \mathbf{G}({\rm s}) \mathbf{N}_1({\rm s}) d{\rm s} \mathbf{N}_2^{\rm T}(\tau) \right\} \\ &= \mathbf{\Phi}(t, t_0) M\left\{ \mathbf{X}(t_0) \mathbf{N}_2^{\rm T} (\tau) \right\} + \int_{t_0}^t \mathbf{\Phi}(t, {\rm s}) \mathbf{G}({\rm s}) M \left\{ \mathbf{N}_1({\rm s}) \mathbf{N}_2^{\rm T}(\tau) \right\} d{\rm s} \\ &= 0 + \int_{t_0}^t \mathbf{\Phi}(t, {\rm s}) \mathbf{G}({\rm s}) \cdot \mathbf{S}_3 (t) \delta (t - \tau) d{\rm s} \\ &= \mathbf{\Phi}(t, \tau) \mathbf{G}(\tau) \mathbf{S}_3(\tau), \quad \left( t_0 \leq \tau \leq t \right) \end{aligned}
M{X(t)N2T(τ)}=M{Φ(t,t0)X(t0)N2T(τ)+∫t0tΦ(t,s)G(s)N1(s)dsN2T(τ)}=Φ(t,t0)M{X(t0)N2T(τ)}+∫t0tΦ(t,s)G(s)M{N1(s)N2T(τ)}ds=0+∫t0tΦ(t,s)G(s)⋅S3(t)δ(t−τ)ds=Φ(t,τ)G(τ)S3(τ),(t0≤τ≤t)
滤波器的任务,就是根据测量值
Y
(
t
)
\mathbf{Y}(t)
Y(t)得到状态量
X
(
t
)
\mathbf{X}(t)
X(t)的估计,且该估计为无偏的和最小误差均方差的。
3. 公式推导
(1) 无偏估计条件
设所求的线性滤波器的估计值
X
^
(
t
)
\hat {\mathbf{X} }(t)
X^(t)可以描述为
X
^
˙
(
t
)
=
F
(
t
)
X
^
(
t
)
+
K
ϕ
(
t
)
Y
(
t
)
(3)
\dot{ \hat{ \mathbf{X} } } (t) = \mathbf{F}(t) \hat {\mathbf{X} }(t) + \mathbf{K}_\phi(t) \mathbf{Y}(t) \tag{3}
X^˙(t)=F(t)X^(t)+Kϕ(t)Y(t)(3)为满足无偏估计的条件,需要有
M
{
X
^
(
t
)
}
=
M
{
X
(
t
)
}
=
X
ˉ
(
t
)
(4)
M\left\{ \hat {\mathbf{X} }(t) \right\} = M\left\{ \mathbf{X}(t) \right\} = \bar {\mathbf{X} }(t) \tag{4}
M{X^(t)}=M{X(t)}=Xˉ(t)(4)对式(3)两边取期望有
M
{
X
^
˙
(
t
)
}
=
F
(
t
)
M
{
X
^
(
t
)
}
+
K
ϕ
(
t
)
M
{
Y
(
t
)
}
(5)
M\left\{ \dot{ \hat{ \mathbf{X} } } (t) \right\} = \mathbf{F}(t) M\left\{ \hat {\mathbf{X} }(t) \right\} + \mathbf{K}_\phi(t) M\left\{ \mathbf{Y}(t) \right\} \tag{5}
M{X^˙(t)}=F(t)M{X^(t)}+Kϕ(t)M{Y(t)}(5)又由式(2)有
Y
(
t
)
=
C
(
t
)
X
(
t
)
+
N
2
(
t
)
\mathbf{Y}(t) = \mathbf{C}(t) \mathbf{X}(t) + \mathbf{N}_2(t)
Y(t)=C(t)X(t)+N2(t),对式(2)两边取期望有
M
{
Y
(
t
)
}
=
M
{
C
(
t
)
X
(
t
)
+
N
2
(
t
)
}
=
M
{
C
(
t
)
X
(
t
)
}
+
M
{
N
2
(
t
)
}
=
C
(
t
)
X
ˉ
(
t
)
+
0
=
C
(
t
)
X
ˉ
(
t
)
\begin{aligned} M\left\{ \mathbf{Y}(t) \right\} &= M\left\{ \mathbf{C}(t) \mathbf{X}(t) + \mathbf{N}_2(t) \right\} \\ &= M\left\{ \mathbf{C}(t) \mathbf{X}(t) \right\} + M\left\{ \mathbf{N}_2(t) \right\}\\ &= \mathbf{C}(t) \bar {\mathbf{X} }(t) + 0 \\ &= \mathbf{C}(t) \bar {\mathbf{X} }(t) \end{aligned}
M{Y(t)}=M{C(t)X(t)+N2(t)}=M{C(t)X(t)}+M{N2(t)}=C(t)Xˉ(t)+0=C(t)Xˉ(t)代入式(5)并根据式(4)有
M
{
X
^
˙
(
t
)
}
=
F
(
t
)
M
{
X
^
(
t
)
}
+
K
ϕ
(
t
)
M
{
Y
(
t
)
}
=
F
(
t
)
M
{
X
^
(
t
)
}
+
K
ϕ
(
t
)
⋅
C
(
t
)
X
ˉ
(
t
)
=
F
(
t
)
M
{
X
^
(
t
)
}
+
K
ϕ
(
t
)
⋅
C
(
t
)
M
{
X
^
(
t
)
}
=
[
F
(
t
)
+
K
ϕ
(
t
)
C
(
t
)
]
⋅
M
{
X
^
(
t
)
}
(6)
\begin{aligned} M\left\{ \dot{ \hat{ \mathbf{X} } } (t) \right\} &= \mathbf{F}(t) M\left\{ \hat {\mathbf{X} }(t) \right\} + \mathbf{K}_\phi(t) M\left\{ \mathbf{Y}(t) \right\} \\ &= \mathbf{F}(t) M\left\{ \hat {\mathbf{X} }(t) \right\} + \mathbf{K}_\phi(t) \cdot \mathbf{C}(t) \bar {\mathbf{X} }(t) \\ &= \mathbf{F}(t) M\left\{ \hat {\mathbf{X} }(t) \right\} + \mathbf{K}_\phi(t) \cdot \mathbf{C}(t) M\left\{ \hat {\mathbf{X} }(t) \right\} \\ &= \left[ \mathbf{F}(t) + \mathbf{K}_\phi(t) \mathbf{C}(t) \right] \cdot M\left\{ \hat {\mathbf{X} }(t) \right\} \end{aligned} \tag{6}
M{X^˙(t)}=F(t)M{X^(t)}+Kϕ(t)M{Y(t)}=F(t)M{X^(t)}+Kϕ(t)⋅C(t)Xˉ(t)=F(t)M{X^(t)}+Kϕ(t)⋅C(t)M{X^(t)}=[F(t)+Kϕ(t)C(t)]⋅M{X^(t)}(6)另一方面,对式(1)两边取数学期望有(考虑到噪声的期望为0)
X
ˉ
˙
(
t
)
=
A
(
t
)
X
ˉ
(
t
)
(7)
\dot{ \bar{ \mathbf{X} } }(t) = \mathbf{A}(t) \bar{ \mathbf{X} }(t) \tag{7}
Xˉ˙(t)=A(t)Xˉ(t)(7)根据式(4)可以看出,式(6)和式(7)的等号右边应该相等,那么
F
(
t
)
+
K
ϕ
(
t
)
C
(
t
)
=
A
(
t
)
\mathbf{F}(t) + \mathbf{K}_\phi(t) \mathbf{C}(t) = \mathbf{A}(t)
F(t)+Kϕ(t)C(t)=A(t)或
F
(
t
)
=
A
(
t
)
−
K
ϕ
(
t
)
C
(
t
)
(8)
\mathbf{F}(t) = \mathbf{A}(t) - \mathbf{K}_\phi(t) \mathbf{C}(t) \tag{8}
F(t)=A(t)−Kϕ(t)C(t)(8)反代回式(3),得到滤波器的结构
X
^
˙
(
t
)
=
[
A
(
t
)
−
K
ϕ
(
t
)
C
(
t
)
]
X
^
(
t
)
+
K
ϕ
(
t
)
Y
(
t
)
(9)
\dot{ \hat{ \mathbf{X} } } (t) = \left[ \mathbf{A}(t) - \mathbf{K}_\phi(t) \mathbf{C}(t) \right] \hat {\mathbf{X} }(t) + \mathbf{K}_\phi(t) \mathbf{Y}(t) \tag{9}
X^˙(t)=[A(t)−Kϕ(t)C(t)]X^(t)+Kϕ(t)Y(t)(9)应当注意的是,估计值
X
^
\hat{ \mathbf{X} }
X^的初值应当尽可能接近真值,即
X
^
(
t
0
)
=
X
ˉ
0
\hat{ \mathbf{X} }(t_0) = \bar{ \mathbf{X} }^0
X^(t0)=Xˉ0,此时的滤波器才有效。
(2) 最小误差均方差条件
最小误差均方差条件表示为
M
{
∥
X
σ
(
t
)
∥
2
}
→
min
K
ϕ
(
t
)
M\left\{ \Vert \mathbf{X}_\sigma (t) \Vert ^2 \right\} \rightarrow \min_{\mathbf{K}_\phi (t) }
M{∥Xσ(t)∥2}→Kϕ(t)min其中
X
σ
(
t
)
=
X
(
t
)
−
X
^
(
t
)
\mathbf{X}_\sigma (t) = \mathbf{X}(t) - \hat{ \mathbf{X} }(t)
Xσ(t)=X(t)−X^(t)。
不难看出:
M
{
∥
X
σ
(
t
)
∥
2
}
=
t
r
[
D
σ
σ
(
t
)
]
(10)
M\left\{ \Vert \mathbf{X}_\sigma (t) \Vert ^2 \right\} = {\rm tr} \left[ \mathbf{D}_{\sigma \sigma}(t) \right] \tag{10}
M{∥Xσ(t)∥2}=tr[Dσσ(t)](10)其中
D
X
X
(
t
)
\mathbf{D}_{XX} (t)
DXX(t)表示
X
(
t
)
\mathbf{X}(t)
X(t)方差,即
D
X
X
(
t
)
=
M
{
(
X
−
X
ˉ
)
(
X
−
X
ˉ
)
T
}
\mathbf{D}_{XX} (t) = M \left\{ \left( \mathbf{X} - \bar{ \mathbf{X} } \right) \left( \mathbf{X} - \bar{ \mathbf{X} } \right)^{\rm T} \right\}
DXX(t)=M{(X−Xˉ)(X−Xˉ)T}。
对误差求导有(用到式(2)(9))
X
˙
σ
(
t
)
=
X
˙
(
t
)
−
X
^
˙
(
t
)
=
[
A
(
t
)
X
(
t
)
+
G
(
t
)
N
1
(
t
)
]
−
{
[
A
(
t
)
−
K
ϕ
(
t
)
C
(
t
)
]
X
^
(
t
)
+
K
ϕ
(
t
)
Y
(
t
)
}
=
A
(
t
)
X
(
t
)
+
G
(
t
)
N
1
(
t
)
−
A
(
t
)
X
^
(
t
)
+
K
ϕ
(
t
)
C
(
t
)
X
^
(
t
)
−
K
ϕ
(
t
)
Y
(
t
)
=
A
(
t
)
[
X
(
t
)
−
X
^
(
t
)
]
+
G
(
t
)
N
1
(
t
)
+
K
ϕ
(
t
)
C
(
t
)
X
^
(
t
)
−
K
ϕ
(
t
)
[
C
(
t
)
X
(
t
)
+
N
2
(
t
)
]
=
A
(
t
)
X
σ
(
t
)
+
G
(
t
)
N
1
(
t
)
−
K
ϕ
(
t
)
N
2
(
t
)
−
K
ϕ
(
t
)
C
(
t
)
X
σ
(
t
)
=
[
A
(
t
)
−
K
ϕ
(
t
)
C
(
t
)
]
X
σ
(
t
)
+
G
(
t
)
N
1
(
t
)
−
K
ϕ
(
t
)
N
2
(
t
)
=
[
A
(
t
)
−
K
ϕ
(
t
)
C
(
t
)
]
X
σ
(
t
)
+
u
=
A
~
(
t
)
X
σ
(
t
)
+
u
(11)
\begin{aligned} \dot{ \mathbf{X} }_\sigma (t) &= \dot{ \mathbf{X} }(t) - \dot{ \hat{ \mathbf{X} } }(t) \\ &= \left[ \mathbf{A}(t) \mathbf{X}(t) + \mathbf{G}(t) \mathbf{N}_1(t) \right] - \left\{ \left[ \mathbf{A}(t) - \mathbf{K}_\phi(t) \mathbf{C}(t) \right] \hat {\mathbf{X} }(t) + \mathbf{K}_\phi(t) \mathbf{Y}(t) \right\} \\ &= \mathbf{A}(t) \mathbf{X}(t) + \mathbf{G}(t) \mathbf{N}_1(t) - \mathbf{A}(t) \hat {\mathbf{X} }(t) + \mathbf{K}_\phi(t) \mathbf{C}(t) \hat {\mathbf{X} }(t) - \mathbf{K}_\phi(t) \mathbf{Y}(t) \\ &= \mathbf{A}(t) \left[ \mathbf{X}(t) - \hat {\mathbf{X} }(t) \right] + \mathbf{G}(t) \mathbf{N}_1(t) + \mathbf{K}_\phi(t) \mathbf{C}(t) \hat {\mathbf{X} }(t) - \mathbf{K}_\phi(t) \left[ \mathbf{C}(t) \mathbf{X}(t) + \mathbf{N}_2(t) \right] \\ &= \mathbf{A}(t) \mathbf{X}_\sigma(t) + \mathbf{G}(t) \mathbf{N}_1(t) - \mathbf{K}_\phi(t) \mathbf{N}_2(t) - \mathbf{K}_\phi(t) \mathbf{C}(t) \mathbf{X}_\sigma(t) \\ &= \left[ \mathbf{A}(t) - \mathbf{K}_\phi(t) \mathbf{C}(t) \right] \mathbf{X}_\sigma(t) + \mathbf{G}(t) \mathbf{N}_1(t) - \mathbf{K}_\phi(t) \mathbf{N}_2(t) \\ &= \left[ \mathbf{A}(t) - \mathbf{K}_\phi(t) \mathbf{C}(t) \right] \mathbf{X}_\sigma(t) + \mathbf{u} \\ &= \tilde{ \mathbf{A} }(t) \mathbf{X}_\sigma(t) + \mathbf{u} \end{aligned} \tag{11}
X˙σ(t)=X˙(t)−X^˙(t)=[A(t)X(t)+G(t)N1(t)]−{[A(t)−Kϕ(t)C(t)]X^(t)+Kϕ(t)Y(t)}=A(t)X(t)+G(t)N1(t)−A(t)X^(t)+Kϕ(t)C(t)X^(t)−Kϕ(t)Y(t)=A(t)[X(t)−X^(t)]+G(t)N1(t)+Kϕ(t)C(t)X^(t)−Kϕ(t)[C(t)X(t)+N2(t)]=A(t)Xσ(t)+G(t)N1(t)−Kϕ(t)N2(t)−Kϕ(t)C(t)Xσ(t)=[A(t)−Kϕ(t)C(t)]Xσ(t)+G(t)N1(t)−Kϕ(t)N2(t)=[A(t)−Kϕ(t)C(t)]Xσ(t)+u=A~(t)Xσ(t)+u(11)其中
u
(
t
)
=
G
(
t
)
N
1
(
t
)
−
K
ϕ
(
t
)
N
2
(
t
)
,
M
{
u
(
t
)
}
=
0
\mathbf{u}(t) = \mathbf{G}(t) \mathbf{N}_1(t) - \mathbf{K}_\phi(t) \mathbf{N}_2(t),\quad M\left\{ \mathbf{u}(t) \right\} = 0
u(t)=G(t)N1(t)−Kϕ(t)N2(t),M{u(t)}=0且初始条件
X
σ
(
t
)
=
X
(
t
0
)
−
X
ˉ
0
\mathbf{X}_\sigma(t) = \mathbf{X}(t_0) - \bar{ \mathbf{X} }^0
Xσ(t)=X(t0)−Xˉ0。
对于
u
(
t
)
\mathbf{u}(t)
u(t)来说,其方差(式中省略
(
t
)
(t)
(t)以便书写)
M
{
u
(
t
)
u
T
(
τ
)
}
=
M
{
[
G
N
1
−
K
ϕ
N
2
]
⋅
[
G
N
1
−
K
ϕ
N
2
]
T
}
=
M
{
[
G
N
1
−
K
ϕ
N
2
]
⋅
[
N
1
T
G
T
−
N
2
T
K
ϕ
T
]
}
=
M
{
G
N
1
N
1
T
G
T
−
G
N
1
N
2
T
K
ϕ
T
−
K
ϕ
N
2
N
1
T
G
T
+
K
ϕ
N
2
N
2
T
K
ϕ
T
}
=
M
{
G
N
1
N
1
T
G
T
}
−
M
{
G
N
1
N
2
T
K
ϕ
T
}
−
M
{
K
ϕ
N
2
N
1
T
G
T
}
+
M
{
K
ϕ
N
2
N
2
T
K
ϕ
T
}
\begin{aligned} M\left\{ \mathbf{u}(t) \mathbf{u}^{\rm T}(\tau) \right\} &= M\left\{ \left[ \mathbf{G} \mathbf{N}_1 - \mathbf{K}_\phi \mathbf{N}_2 \right] \cdot \left[ \mathbf{G} \mathbf{N}_1 - \mathbf{K}_\phi \mathbf{N}_2 \right]^{\rm T} \right\} \\ &= M\left\{ \left[ \mathbf{G} \mathbf{N}_1 - \mathbf{K}_\phi \mathbf{N}_2 \right] \cdot \left[ \mathbf{N}_1^{\rm T} \mathbf{G}^{\rm T} - \mathbf{N}_2^{\rm T} \mathbf{K}_\phi^{\rm T} \right] \right\} \\ &= M\left\{ \mathbf{G} \mathbf{N}_1 \mathbf{N}_1^{\rm T} \mathbf{G}^{\rm T} - \mathbf{G} \mathbf{N}_1 \mathbf{N}_2^{\rm T} \mathbf{K}_\phi^{\rm T} - \mathbf{K}_\phi \mathbf{N}_2 \mathbf{N}_1^{\rm T} \mathbf{G}^{\rm T} + \mathbf{K}_\phi \mathbf{N}_2 \mathbf{N}_2^{\rm T} \mathbf{K}_\phi^{\rm T} \right\} \\ &= M\left\{ \mathbf{G} \mathbf{N}_1 \mathbf{N}_1^{\rm T} \mathbf{G}^{\rm T} \right\} - M\left\{ \mathbf{G} \mathbf{N}_1 \mathbf{N}_2^{\rm T} \mathbf{K}_\phi^{\rm T} \right\} - M\left\{ \mathbf{K}_\phi \mathbf{N}_2 \mathbf{N}_1^{\rm T} \mathbf{G}^{\rm T} \right\} + M\left\{ \mathbf{K}_\phi \mathbf{N}_2 \mathbf{N}_2^{\rm T} \mathbf{K}_\phi^{\rm T} \right\} \end{aligned}
M{u(t)uT(τ)}=M{[GN1−KϕN2]⋅[GN1−KϕN2]T}=M{[GN1−KϕN2]⋅[N1TGT−N2TKϕT]}=M{GN1N1TGT−GN1N2TKϕT−KϕN2N1TGT+KϕN2N2TKϕT}=M{GN1N1TGT}−M{GN1N2TKϕT}−M{KϕN2N1TGT}+M{KϕN2N2TKϕT}由于
M
{
N
1
(
t
)
N
1
T
(
t
)
}
=
S
1
(
t
)
δ
(
t
−
τ
)
,
M
{
N
2
(
t
)
N
2
T
(
t
)
}
=
S
2
(
t
)
δ
(
t
−
τ
)
,
M
{
N
1
(
t
)
N
2
T
(
τ
)
}
=
S
3
(
t
)
δ
(
t
−
τ
)
M\left\{ \mathbf{N}_1(t) \mathbf{N}_1^{\rm T}(t) \right\} = \mathbf{S}_1 (t) \delta (t - \tau),\quad M\left\{ \mathbf{N}_2(t) \mathbf{N}_2^{\rm T}(t) \right\} = \mathbf{S}_2 (t) \delta (t - \tau), \quad M\left\{ \mathbf{N}_1(t) \mathbf{N}_2^{\rm T}(\tau) \right\} = \mathbf{S}_3 (t) \delta (t - \tau)
M{N1(t)N1T(t)}=S1(t)δ(t−τ),M{N2(t)N2T(t)}=S2(t)δ(t−τ),M{N1(t)N2T(τ)}=S3(t)δ(t−τ),于是上式变为
M
{
u
(
t
)
u
T
(
τ
)
}
=
M
{
G
N
1
N
1
T
G
T
}
−
M
{
G
N
1
N
2
T
K
ϕ
T
}
−
M
{
K
ϕ
N
2
N
1
T
G
T
}
+
M
{
K
ϕ
N
2
N
2
T
K
ϕ
T
}
=
[
G
S
1
G
T
−
G
(
t
)
S
3
K
ϕ
T
−
K
ϕ
S
3
G
T
+
K
ϕ
S
2
K
ϕ
T
]
δ
(
t
−
τ
)
(12)
\begin{aligned} M\left\{ \mathbf{u}(t) \mathbf{u}^{\rm T}(\tau) \right\} &= M\left\{ \mathbf{G} \mathbf{N}_1 \mathbf{N}_1^{\rm T} \mathbf{G}^{\rm T} \right\} - M\left\{ \mathbf{G} \mathbf{N}_1 \mathbf{N}_2^{\rm T} \mathbf{K}_\phi^{\rm T} \right\} - M\left\{ \mathbf{K}_\phi \mathbf{N}_2 \mathbf{N}_1^{\rm T} \mathbf{G}^{\rm T} \right\} + M\left\{ \mathbf{K}_\phi \mathbf{N}_2 \mathbf{N}_2^{\rm T} \mathbf{K}_\phi^{\rm T} \right\} \\ &= \left[ \mathbf{G} \mathbf{S}_1 \mathbf{G}^{\rm T} - \mathbf{G}(t) \mathbf{S}_3 \mathbf{K}_\phi^{\rm T} - \mathbf{K}_\phi \mathbf{S}_3 \mathbf{G}^{\rm T} + \mathbf{K}_\phi \mathbf{S}_2 \mathbf{K}_\phi^{\rm T} \right] \delta(t - \tau) \tag{12} \end{aligned}
M{u(t)uT(τ)}=M{GN1N1TGT}−M{GN1N2TKϕT}−M{KϕN2N1TGT}+M{KϕN2N2TKϕT}=[GS1GT−G(t)S3KϕT−KϕS3GT+KϕS2KϕT]δ(t−τ)(12)
(3) 误差的方差条件
这里不加证明地给出如下结论:
当
X
˙
(
t
)
=
A
(
t
)
X
(
t
)
+
G
(
t
)
N
1
(
t
)
\dot{ \mathbf{X} }(t) = \mathbf{A}(t) \mathbf{X}(t) + \mathbf{G}(t) \mathbf{N}_1(t)
X˙(t)=A(t)X(t)+G(t)N1(t)时,其方差满足如下微分方程
D
˙
X
X
(
t
)
=
A
(
t
)
D
X
X
(
t
)
+
D
X
X
(
t
)
A
T
(
t
)
+
G
(
t
)
S
1
(
t
)
G
T
(
t
)
(13)
\dot{ \mathbf{D} }_{XX}(t) = \mathbf{A}(t) \mathbf{D}_{XX}(t) + \mathbf{D}_{XX}(t) \mathbf{A}^{\rm T}(t) + \mathbf{G}(t) \mathbf{S}_1(t) \mathbf{G}^{\rm T}(t) \tag{13}
D˙XX(t)=A(t)DXX(t)+DXX(t)AT(t)+G(t)S1(t)GT(t)(13)那么,当
X
˙
σ
(
t
)
\dot{ \mathbf{X} }_\sigma(t)
X˙σ(t)具有式(11)的形式时,代入式(13)可以得到估计误差
X
σ
(
t
)
\mathbf{X}_\sigma(t)
Xσ(t)的方差
D
σ
σ
(
t
)
\mathbf{D}_{\sigma \sigma}(t)
Dσσ(t)满足的微分方程:
D
˙
σ
σ
(
t
)
=
A
~
(
t
)
D
X
X
(
t
)
+
D
X
X
(
t
)
A
~
T
(
t
)
+
M
{
u
(
t
)
u
T
(
τ
)
}
/
δ
(
t
−
τ
)
=
[
A
−
K
ϕ
C
]
D
σ
σ
+
D
σ
σ
[
A
−
K
ϕ
C
]
T
+
[
G
S
1
G
T
−
G
S
3
K
ϕ
T
−
K
ϕ
S
3
G
T
+
K
ϕ
S
2
K
ϕ
T
]
=
A
~
(
t
)
D
σ
σ
+
D
σ
σ
A
~
T
(
t
)
+
G
S
1
G
T
−
G
S
3
K
ϕ
T
−
K
ϕ
S
3
G
T
+
K
ϕ
S
2
K
ϕ
T
(14)
\begin{aligned} \dot{ \mathbf{D} }_{\sigma \sigma}(t) &= \tilde{ \mathbf{A} }(t) \mathbf{D}_{XX}(t) + \mathbf{D}_{XX}(t) \tilde{ \mathbf{A} }^{\rm T}(t) + M\left\{ \mathbf{u}(t) \mathbf{u}^{\rm T}(\tau) \right\} / \delta(t - \tau) \\ &= \left[ \mathbf{A} - \mathbf{K}_\phi \mathbf{C} \right] \mathbf{D}_{\sigma \sigma} + \mathbf{D}_{\sigma \sigma} \left[ \mathbf{A} - \mathbf{K}_\phi \mathbf{C} \right]^{\rm T} + \left[ \mathbf{G} \mathbf{S}_1 \mathbf{G}^{\rm T} - \mathbf{G} \mathbf{S}_3 \mathbf{K}_\phi^{\rm T} - \mathbf{K}_\phi \mathbf{S}_3 \mathbf{G}^{\rm T} + \mathbf{K}_\phi \mathbf{S}_2 \mathbf{K}_\phi^{\rm T} \right] \\ &= \tilde{ \mathbf{A} }(t) \mathbf{D}_{\sigma \sigma} + \mathbf{D}_{\sigma \sigma} \tilde{ \mathbf{A} }^{\rm T}(t) + \mathbf{G} \mathbf{S}_1 \mathbf{G}^{\rm T} - \mathbf{G} \mathbf{S}_3 \mathbf{K}_\phi^{\rm T} - \mathbf{K}_\phi \mathbf{S}_3 \mathbf{G}^{\rm T} + \mathbf{K}_\phi \mathbf{S}_2 \mathbf{K}_\phi^{\rm T} \tag{14} \end{aligned}
D˙σσ(t)=A~(t)DXX(t)+DXX(t)A~T(t)+M{u(t)uT(τ)}/δ(t−τ)=[A−KϕC]Dσσ+Dσσ[A−KϕC]T+[GS1GT−GS3KϕT−KϕS3GT+KϕS2KϕT]=A~(t)Dσσ+DσσA~T(t)+GS1GT−GS3KϕT−KϕS3GT+KϕS2KϕT(14)
(4) K ϕ ( t ) \mathbf{K}_\phi(t) Kϕ(t)的推导
现在滤波器的任务转化为求解
K
ϕ
(
t
)
\mathbf{K}_\phi(t)
Kϕ(t)使得
D
σ
σ
(
t
)
\mathbf{D}_{\sigma \sigma}(t)
Dσσ(t)最小。可以想到,当方差
D
σ
σ
(
t
)
\mathbf{D}_{\sigma \sigma}(t)
Dσσ(t)最小时,同样我们也期望方差稳定在该最小值附近不要变化,即其导数
D
˙
σ
σ
(
t
)
\dot{ \mathbf{D} }_{\sigma \sigma}(t)
D˙σσ(t)同样方差最小。表示为
min
K
ϕ
(
t
)
{
t
r
[
D
σ
σ
(
t
)
]
}
→
K
~
ϕ
∗
(
t
)
max
K
ϕ
(
t
)
{
−
t
r
[
D
˙
σ
σ
(
t
)
]
}
→
K
~
~
ϕ
∗
(
t
)
\min_{ \mathbf{K}_\phi(t) } \left\{ {\rm tr} \left[ \mathbf{D}_{\sigma \sigma} (t) \right] \right\} \rightarrow \tilde{ \mathbf{K} }_\phi^* (t) \\ \max_{ \mathbf{K}_\phi(t) } \left\{ {\rm -tr} \left[ \dot{ \mathbf{D} }_{\sigma \sigma} (t) \right] \right\} \rightarrow \tilde{ \tilde{ \mathbf{K} } }_\phi^* (t)
Kϕ(t)min{tr[Dσσ(t)]}→K~ϕ∗(t)Kϕ(t)max{−tr[D˙σσ(t)]}→K~~ϕ∗(t)由于
K
ϕ
(
t
)
\mathbf{K}_\phi(t)
Kϕ(t)唯一,故
K
~
ϕ
∗
(
t
)
=
K
~
~
ϕ
∗
(
t
)
\tilde{ \mathbf{K} }_\phi^* (t) = \tilde{ \tilde{ \mathbf{K} } }_\phi^* (t)
K~ϕ∗(t)=K~~ϕ∗(t)借助变分法知识并参照欧拉方程(这里我也不知道怎么得来的),
K
ϕ
(
t
)
\mathbf{K}_\phi(t)
Kϕ(t)满足
∂
∂
K
ϕ
(
t
)
t
r
[
D
˙
σ
σ
(
t
)
]
=
0
(15)
\frac{\partial}{\partial \mathbf{K}_\phi(t)} {\rm tr} \left[ \dot{ \mathbf{D} }_{\sigma \sigma} (t) \right] = 0 \tag{15}
∂Kϕ(t)∂tr[D˙σσ(t)]=0(15)
(5) 矩阵迹的求导
为了求解式(15),需要先介绍一些涉及到矩阵迹的求导公式:
∂
t
r
(
P
Q
P
T
)
∂
P
=
P
(
Q
+
Q
T
)
∂
t
r
(
P
Q
)
∂
Q
=
P
T
∂
t
r
(
Q
P
)
∂
Q
=
P
T
∂
t
r
(
P
Q
T
)
∂
Q
=
P
∂
t
r
(
Q
T
P
)
∂
Q
=
P
\frac{\partial {\rm tr} \left( \mathbf{P} \mathbf{Q} \mathbf{P}^{\rm T} \right)}{\partial \mathbf{P}} = \mathbf{P} \left( \mathbf{Q} + \mathbf{Q}^{\rm T} \right) \\ \frac{\partial {\rm tr} \left( \mathbf{P} \mathbf{Q} \right)}{\partial \mathbf{Q}} = \mathbf{P}^{\rm T} \\ \frac{\partial {\rm tr} \left( \mathbf{Q} \mathbf{P} \right)}{\partial \mathbf{Q}} = \mathbf{P}^{\rm T} \\ \frac{\partial {\rm tr} \left( \mathbf{P} \mathbf{Q}^{\rm T} \right)}{\partial \mathbf{Q}} = \mathbf{P} \\ \frac{\partial {\rm tr} \left( \mathbf{Q}^{\rm T} \mathbf{P} \right)}{\partial \mathbf{Q}} = \mathbf{P}
∂P∂tr(PQPT)=P(Q+QT)∂Q∂tr(PQ)=PT∂Q∂tr(QP)=PT∂Q∂tr(PQT)=P∂Q∂tr(QTP)=P参照式(14)右边的各项,先得出如下项的迹的导数:
∂
t
r
(
A
−
K
ϕ
C
)
∂
K
ϕ
=
∂
t
r
(
A
)
∂
K
ϕ
−
∂
t
r
(
K
ϕ
C
)
∂
K
ϕ
=
0
−
C
T
=
−
C
T
∂
t
r
(
K
ϕ
S
2
K
ϕ
T
)
∂
K
ϕ
=
K
ϕ
(
S
2
+
S
2
T
)
=
2
K
ϕ
S
2
∂
t
r
(
G
(
t
)
S
3
K
ϕ
T
)
∂
K
ϕ
=
G
(
t
)
S
3
∂
t
r
(
K
ϕ
S
3
G
T
)
∂
K
ϕ
=
[
S
3
G
T
]
T
=
G
(
t
)
S
3
\frac{\partial {\rm tr} \left( \mathbf{A} - \mathbf{K}_\phi \mathbf{C} \right)}{\partial \mathbf{K}_\phi} = \frac{\partial {\rm tr} \left( \mathbf{A} \right)}{\partial \mathbf{K}_\phi} - \frac{\partial {\rm tr} \left( \mathbf{K}_\phi \mathbf{C} \right)}{\partial \mathbf{K}_\phi} = 0 - \mathbf{C}^{\rm T} = - \mathbf{C}^{\rm T} \\ \frac{\partial {\rm tr} \left( \mathbf{K}_\phi \mathbf{S}_2 \mathbf{K}_\phi^{\rm T} \right)}{\partial \mathbf{K}_\phi} = \mathbf{K}_\phi \left( \mathbf{S}_2 + \mathbf{S}_2^{\rm T} \right) = 2 \mathbf{K}_\phi \mathbf{S}_2 \\ \frac{\partial {\rm tr} \left( \mathbf{G}(t) \mathbf{S}_3 \mathbf{K}_\phi^{\rm T} \right)}{\partial \mathbf{K}_\phi} = \mathbf{G}(t) \mathbf{S}_3 \\ \frac{\partial {\rm tr} \left( \mathbf{K}_\phi \mathbf{S}_3 \mathbf{G}^{\rm T} \right)}{\partial \mathbf{K}_\phi} = \left[ \mathbf{S}_3 \mathbf{G}^{\rm T} \right]^{\rm T} = \mathbf{G}(t) \mathbf{S}_3
∂Kϕ∂tr(A−KϕC)=∂Kϕ∂tr(A)−∂Kϕ∂tr(KϕC)=0−CT=−CT∂Kϕ∂tr(KϕS2KϕT)=Kϕ(S2+S2T)=2KϕS2∂Kϕ∂tr(G(t)S3KϕT)=G(t)S3∂Kϕ∂tr(KϕS3GT)=[S3GT]T=G(t)S3把如上式子全部代入微分方程(15),可以求解得
K
ϕ
∗
(
t
)
=
[
D
σ
σ
(
t
)
C
T
(
t
)
+
G
(
t
)
S
3
(
t
)
]
S
2
−
1
(
t
)
(16 – 1)
\mathbf{K}_\phi^* (t) = \left[ \mathbf{D}_{\sigma \sigma}(t) \mathbf{C}^{\rm T}(t) + \mathbf{G}(t) \mathbf{S}_3(t) \right] \mathbf{S}_2^{-1}(t) \tag{16 -- 1}
Kϕ∗(t)=[Dσσ(t)CT(t)+G(t)S3(t)]S2−1(t)(16 – 1)
(6) 两个噪声之间的特殊情况
以上所有推导都是建立在第2节第(2)点上的: N 1 ( t ) \mathbf{N}_1(t) N1(t)与 N 2 ( t ) \mathbf{N}_2(t) N2(t)相关,即 M { N 1 ( t ) N 2 T ( τ ) } = S 3 ( t ) δ ( t − τ ) M\left\{ \mathbf{N}_1(t) \mathbf{N}_2^{\rm T}(\tau) \right\} = \mathbf{S}_3 (t) \delta (t - \tau) M{N1(t)N2T(τ)}=S3(t)δ(t−τ)。然而,这两个噪声一个是系统噪声,一个是测量噪声,在实际中往往是不相关的。
当二者不相关时,
M
{
N
1
(
t
)
N
2
T
(
τ
)
}
=
0
M\left\{ \mathbf{N}_1(t) \mathbf{N}_2^{\rm T}(\tau) \right\} = 0
M{N1(t)N2T(τ)}=0,即
S
3
(
t
)
=
0
\mathbf{S}_3 (t) = 0
S3(t)=0。在这种特殊情况下,式(16 – 1)具有较为理想的特殊形式
K
ϕ
∗
(
t
)
=
D
σ
σ
(
t
)
C
T
(
t
)
S
2
−
1
(
t
)
(16 – 2)
\mathbf{K}_\phi^* (t) = \mathbf{D}_{\sigma \sigma}(t) \mathbf{C}^{\rm T}(t) \mathbf{S}_2^{-1}(t) \tag{16 -- 2}
Kϕ∗(t)=Dσσ(t)CT(t)S2−1(t)(16 – 2)式(16 – 1)和(16 – 2)即为所求得的最优Kalman-Bucy滤波器表达式。
最后,将式(16 – 1)反代回式(14),以求得不显含
K
ϕ
∗
(
t
)
\mathbf{K}_\phi^*(t)
Kϕ∗(t)的误差方差
D
σ
σ
(
t
)
\mathbf{D}_{\sigma \sigma}(t)
Dσσ(t)满足的微分方程:
D
˙
σ
σ
(
t
)
=
[
A
−
K
ϕ
∗
C
]
D
σ
σ
+
D
σ
σ
[
A
−
K
ϕ
∗
C
]
T
+
[
G
S
1
G
T
−
G
S
3
K
ϕ
∗
T
−
K
ϕ
∗
S
3
G
T
+
K
ϕ
∗
S
2
K
ϕ
∗
T
]
=
{
A
−
[
D
σ
σ
C
T
+
G
S
3
]
S
2
−
1
⋅
C
}
D
σ
σ
+
D
σ
σ
{
A
−
[
D
σ
σ
C
T
+
G
S
3
]
S
2
−
1
⋅
C
}
T
+
+
G
S
1
G
T
−
G
S
3
{
[
D
σ
σ
C
T
+
G
S
3
]
S
2
−
1
}
T
−
[
D
σ
σ
C
T
+
G
S
3
]
S
2
−
1
⋅
S
3
G
T
+
[
D
σ
σ
C
T
+
G
S
3
]
S
2
−
1
⋅
S
2
⋅
K
ϕ
∗
T
{
[
D
σ
σ
C
T
+
G
S
3
]
S
2
−
1
}
T
=
(
A
−
G
S
3
S
2
−
1
C
)
D
σ
σ
+
D
σ
σ
(
A
−
G
S
3
S
2
−
1
C
)
T
−
D
σ
σ
C
T
S
2
−
1
C
D
σ
σ
+
G
(
S
1
−
S
3
S
2
−
1
S
3
)
G
T
(17 – 1)
\begin{aligned} \dot{ \mathbf{D} }_{\sigma \sigma}(t) &= \left[ \mathbf{A} - \mathbf{K}_\phi^* \mathbf{C} \right] \mathbf{D}_{\sigma \sigma} + \mathbf{D}_{\sigma \sigma} \left[ \mathbf{A} - \mathbf{K}_\phi^* \mathbf{C} \right]^{\rm T} + \left[ \mathbf{G} \mathbf{S}_1 \mathbf{G}^{\rm T} - \mathbf{G} \mathbf{S}_3 \mathbf{K}_\phi^{* \rm T} - \mathbf{K}_\phi^* \mathbf{S}_3 \mathbf{G}^{\rm T} + \mathbf{K}_\phi^* \mathbf{S}_2 \mathbf{K}_\phi^{* \rm T} \right] \\ &= \left\{ \mathbf{A} - \left[ \mathbf{D}_{\sigma \sigma} \mathbf{C}^{\rm T} + \mathbf{G} \mathbf{S}_3 \right] \mathbf{S}_2^{-1} \cdot \mathbf{C} \right\} \mathbf{D}_{\sigma \sigma} + \mathbf{D}_{\sigma \sigma} \left\{ \mathbf{A} - \left[ \mathbf{D}_{\sigma \sigma} \mathbf{C}^{\rm T} + \mathbf{G} \mathbf{S}_3 \right] \mathbf{S}_2^{-1} \cdot \mathbf{C} \right\}^{\rm T} + \\ &+ \mathbf{G} \mathbf{S}_1 \mathbf{G}^{\rm T} - \mathbf{G} \mathbf{S}_3 \left\{ \left[ \mathbf{D}_{\sigma \sigma} \mathbf{C}^{\rm T} + \mathbf{G} \mathbf{S}_3 \right] \mathbf{S}_2^{-1} \right\}^{\rm T} - \left[ \mathbf{D}_{\sigma \sigma} \mathbf{C}^{\rm T} + \mathbf{G} \mathbf{S}_3 \right] \mathbf{S}_2^{-1} \cdot \mathbf{S}_3 \mathbf{G}^{\rm T} \\ &+ \left[ \mathbf{D}_{\sigma \sigma} \mathbf{C}^{\rm T} + \mathbf{G} \mathbf{S}_3 \right] \mathbf{S}_2^{-1} \cdot \mathbf{S}_2 \cdot \mathbf{K}_\phi^{* \rm T} \left\{ \left[ \mathbf{D}_{\sigma \sigma} \mathbf{C}^{\rm T} + \mathbf{G} \mathbf{S}_3 \right] \mathbf{S}_2^{-1} \right\}^{\rm T} \\ &= \left( \mathbf{A} - \mathbf{G} \mathbf{S}_3 \mathbf{S}_2^{-1} \mathbf{C} \right) \mathbf{D}_{\sigma \sigma} + \mathbf{D}_{\sigma \sigma} \left( \mathbf{A} - \mathbf{G} \mathbf{S}_3 \mathbf{S}_2^{-1} \mathbf{C} \right)^{\rm T} - \mathbf{D}_{\sigma \sigma} \mathbf{C}^{\rm T} \mathbf{S}_2^{-1} \mathbf{C} \mathbf{D}_{\sigma \sigma} + \mathbf{G} \left( \mathbf{S}_1 - \mathbf{S}_3 \mathbf{S}_2^{-1} \mathbf{S}_3 \right) \mathbf{G}^{\rm T} \end{aligned} \tag{17 -- 1}
D˙σσ(t)=[A−Kϕ∗C]Dσσ+Dσσ[A−Kϕ∗C]T+[GS1GT−GS3Kϕ∗T−Kϕ∗S3GT+Kϕ∗S2Kϕ∗T]={A−[DσσCT+GS3]S2−1⋅C}Dσσ+Dσσ{A−[DσσCT+GS3]S2−1⋅C}T++GS1GT−GS3{[DσσCT+GS3]S2−1}T−[DσσCT+GS3]S2−1⋅S3GT+[DσσCT+GS3]S2−1⋅S2⋅Kϕ∗T{[DσσCT+GS3]S2−1}T=(A−GS3S2−1C)Dσσ+Dσσ(A−GS3S2−1C)T−DσσCTS2−1CDσσ+G(S1−S3S2−1S3)GT(17 – 1)当
N
1
(
t
)
\mathbf{N}_1(t)
N1(t)与
N
2
(
t
)
\mathbf{N}_2(t)
N2(t)不相关时
S
3
(
t
)
=
0
\mathbf{S}_3(t)=0
S3(t)=0,上式变为
D
˙
σ
σ
(
t
)
=
A
D
σ
σ
+
D
σ
σ
A
T
−
D
σ
σ
C
T
S
2
−
1
C
D
σ
σ
+
G
S
1
G
T
(17 – 2)
\dot{ \mathbf{D} }_{\sigma \sigma}(t) = \mathbf{A} \mathbf{D}_{\sigma \sigma} + \mathbf{D}_{\sigma \sigma} \mathbf{A}^{\rm T} - \mathbf{D}_{\sigma \sigma} \mathbf{C}^{\rm T} \mathbf{S}_2^{-1} \mathbf{C} \mathbf{D}_{\sigma \sigma} + \mathbf{G} \mathbf{S}_1\mathbf{G}^{\rm T} \tag{17 -- 2}
D˙σσ(t)=ADσσ+DσσAT−DσσCTS2−1CDσσ+GS1GT(17 – 2)至此,式(17 – 1)和(17 – 2)给出了不含
K
ϕ
(
t
)
\mathbf{K}_\phi(t)
Kϕ(t)的估计误差方差
D
σ
σ
(
t
)
\mathbf{D}_{\sigma \sigma}(t)
Dσσ(t)表达式。
(7) 滤波器的结论
式(16)给出了最优Kalman-Bucy滤波器的表达式 K ϕ ∗ ( t ) \mathbf{K}_\phi^* (t) Kϕ∗(t)。
a) Kalman-Bucy滤波器具有式(9)的形式,它利用测量值
Y
(
t
)
\mathbf{Y}(t)
Y(t)可以求出状态量
X
(
t
)
\mathbf{X}(t)
X(t)的真值。
b) 由式(9)可以看出,在这个过程中需要用到状态矩阵
A
(
t
)
\mathbf{A}(t)
A(t)和测量矩阵
C
(
t
)
\mathbf{C}(t)
C(t),二者可以在参数估计或参数识别的前提下获得。
c) 利用式(17),可以直接基于状态矩阵
A
(
t
)
\mathbf{A}(t)
A(t)、测量矩阵
C
(
t
)
\mathbf{C}(t)
C(t)与两个噪声的方差分布矩阵
S
i
(
t
)
\mathbf{S}_i(t)
Si(t)计算出估计误差
D
σ
σ
\mathbf{D}_{\sigma \sigma}
Dσσ,而绕开
K
ϕ
(
t
)
\mathbf{K}_\phi(t)
Kϕ(t)的计算。
d) 在计算出
D
σ
σ
\mathbf{D}_{\sigma \sigma}
Dσσ之后,代入式(16),就能计算出
K
ϕ
(
t
)
\mathbf{K}_\phi(t)
Kϕ(t),进而得出Kalman-Bucy滤波器的结构如式(9)。
e) 式(9)计算得出的状态估计值
X
^
(
t
)
\hat{ \mathbf{X} }(t)
X^(t)可以满足无偏估计,并使得估计误差的均方差最小。
f) 不可忽视的条件:以上所有都建立在2大基础上:一是对系统的状态矩阵
A
(
t
)
\mathbf{A}(t)
A(t)和测量矩阵
C
(
t
)
\mathbf{C}(t)
C(t)的估计足够准确,这对参数辨识提出了较高要求;二是估计值
X
^
(
t
)
\hat{ \mathbf{X} }(t)
X^(t)的初值需要估计准确,即结果受初值影响较大。
g) 利用Kalman-Bucy滤波器对状态进行滤波的步骤:参数辨识得出系统模型
A
(
t
)
,
C
(
t
)
\mathbf{A}(t),\mathbf{C}(t)
A(t),C(t) —— 根据式(17)计算得到误差方差
D
σ
σ
\mathbf{D}_{\sigma \sigma}
Dσσ —— 将
D
σ
σ
\mathbf{D}_{\sigma \sigma}
Dσσ代入式(16)计算得到滤波器的最优系数矩阵
K
ϕ
∗
(
t
)
\mathbf{K}_\phi^*(t)
Kϕ∗(t),该矩阵可以满足状态估计值的无偏和误差均方差最小 —— 利用式(9)构建Kalman-Bucy滤波器,并求解得出状态估计值
X
^
(
t
)
\hat{ \mathbf{X} }(t)
X^(t)。