在信号与系统里面,我们研究一个系统的特性,通常是通过系统的输入输出来求解系统函数,这样的研究思路相当于将系统当成了一个黑箱,并未对系统本身特性进行建模处理,因此也未将关于系统模型的一些先验知识应用上去。现在我们尝试用另外一种思路来对一个系统进行研究,在此需要引入状态空间的概念。在这种研究思路下,系统的特性能通过系统的状态变化完全体现出来。通常,系统的状态具有如下两个特点:一是系统的状态能够体现系统本身的特性,它的变化遵循一定的物理规律,可对其进行建模,进而将与之相关的一些先验知识应用起来;二是系统的状态通常难以直接测量得到,因此需要依赖特定的观测向量来了解系统的状态。根据上面的描述我们建立下面的状态方程和观测方程
S
t
a
t
e
E
q
u
a
t
i
o
n
:
x
n
+
1
=
f
(
x
n
,
v
n
)
O
b
s
e
r
v
a
t
i
o
n
E
q
u
a
t
i
o
n
:
y
n
=
g
(
x
n
,
w
n
)
(1)
\begin{equation} \begin{aligned} {\rm State \quad Equation:}\quad &\boldsymbol{x}_{n+1}=f(\boldsymbol{x}_n,\boldsymbol{v}_n)\\ {\rm Observation \quad Equation:}\quad &\boldsymbol{y}_n=g(\boldsymbol{x}_n,\boldsymbol{w}_n) \end{aligned} \end{equation}\tag{1}
StateEquation:ObservationEquation:xn+1=f(xn,vn)yn=g(xn,wn)(1)
在上面的表达式中状态方程表示不同时刻系统状态之间的变化规律,它反映了系统最本质的特性,由系统模型决定。观测方程表示了观测向量和系统状态之间的转化关系。其中
v
n
\boldsymbol{v}_n
vn,
w
n
\boldsymbol{w}_n
wn分别表示状态噪声和观测噪声,通常我们假设两者为零均值白噪声。(1)中两个方程是一般化的情况,在实际应用中通常可用线性模型进行转化,以方便计算,为此,将(1)改写为
S
t
a
t
e
E
q
u
a
t
i
o
n
:
x
n
+
1
=
F
n
x
n
+
v
n
O
b
s
e
r
v
a
t
i
o
n
E
q
u
a
t
i
o
n
:
y
n
=
H
n
x
n
+
w
n
(2)
\begin{equation} \begin{aligned} {\rm State \quad Equation:}\quad &\boldsymbol{x}_{n+1}=\boldsymbol{F}_n \boldsymbol{x}_n+\boldsymbol{v}_n\\ {\rm Observation \quad Equation:}\quad &\boldsymbol{y}_n=\boldsymbol{H}_n \boldsymbol{x}_n+\boldsymbol{w}_n \end{aligned} \end{equation}\tag{2}
StateEquation:ObservationEquation:xn+1=Fnxn+vnyn=Hnxn+wn(2)
在上面的表达式中
F
n
\boldsymbol{F}_n
Fn,
H
n
\boldsymbol{H}_n
Hn分别表示状态转移矩阵和观测矩阵,需要注意的是这两者都带下标
n
n
n,说明这两者是时变的,这也就意味着,此时允许系统是非平稳的。
基于上面的状态方程和观测方程,我们的目的是更好地估计系统的状态。为此,我们先定义如下一些符号,方便后续说明:
x
^
n
∣
n
=
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
x
n
x
^
n
+
1
∣
n
=
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
x
n
+
1
x
^
n
+
1
∣
n
+
1
=
P
r
o
j
(
y
1
,
.
.
.
,
y
n
,
y
n
+
1
)
x
n
+
1
(3)
\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n|n}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_n\\ \hat{\boldsymbol{x}}_{n+1|n}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}\\ \hat{\boldsymbol{x}}_{n+1|n+1}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})}\boldsymbol{x}_{n+1}\\ \end{aligned} \end{equation}\tag{3}
x^n∣nx^n+1∣nx^n+1∣n+1=Proj(y1,...,yn)xn=Proj(y1,...,yn)xn+1=Proj(y1,...,yn,yn+1)xn+1(3)
其中
x
n
,
x
n
+
1
\boldsymbol{x}_n,\boldsymbol{x}_{n+1}
xn,xn+1分别表示
n
n
n时刻和
n
+
1
n+1
n+1时刻的状态真值,
x
n
∣
n
^
\hat{\boldsymbol{x}_{n|n}}
xn∣n^表示用
1
−
n
1-n
1−n时刻的观测向量
(
y
1
,
.
.
.
,
y
n
)
(\boldsymbol{y}_1,...,\boldsymbol{y}_n)
(y1,...,yn)估计得到的
n
n
n时刻的状态向量,
x
n
+
1
∣
n
^
\hat{\boldsymbol{x}_{n+1|n}}
xn+1∣n^表示用
1
−
n
1-n
1−n时刻的观测向量
(
y
1
,
.
.
.
,
y
n
)
(\boldsymbol{y}_1,...,\boldsymbol{y}_n)
(y1,...,yn)估计得到的
n
+
1
n+1
n+1时刻的状态向量,
x
n
+
1
∣
n
+
1
^
\hat{\boldsymbol{x}_{n+1|n+1}}
xn+1∣n+1^表示用
1
−
n
+
1
1-n+1
1−n+1时刻的观测向量
(
y
1
,
.
.
.
,
y
n
,
y
n
+
1
)
(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})
(y1,...,yn,yn+1)估计得到的
n
+
1
n+1
n+1时刻的状态向量。
P
r
o
j
{\rm Proj}
Proj表示投影操作,且有KaTeX parse error: Got function '\boldsymbol' with no arguments as subscript at position 12: {\rm Proj}_\̲b̲o̲l̲d̲s̲y̲m̲b̲o̲l̲{Y}\boldsymbol{…。
我们在 n n n时刻拥有的信息包括:观测向量 y 1 , . . . , y n ) \boldsymbol{y}_1,...,\boldsymbol{y}_n) y1,...,yn), n n n时刻的状态估计向量 x ^ n ∣ n \hat{\boldsymbol{x}}_{n|n} x^n∣n。同时在 n + 1 n+1 n+1时刻,我们将得到一个新的观测向量 y n + 1 \boldsymbol{y}_{n+1} yn+1。我们想要利用上述信息,更好地估计系统在 n + 1 n+1 n+1时刻的状态向量,即 x ^ n + 1 ∣ n + 1 \hat{\boldsymbol{x}}_{n+1|n+1} x^n+1∣n+1。一个很自然的思路就是,首先利用 x ^ n ∣ n \hat{\boldsymbol{x}}_{n|n} x^n∣n结合状态转移矩阵 F n \boldsymbol{F}_n Fn(即系统模型),可以预测预测得到 x ^ n + 1 ∣ n \hat{\boldsymbol{x}}_{n+1|n} x^n+1∣n。此时,由于系统建模不一定完全准确( F n \boldsymbol{F}_n Fn不一定完全符合实际模型)或者状态噪声的影响,使得 x ^ n + 1 ∣ n \hat{\boldsymbol{x}}_{n+1|n} x^n+1∣n并不一定准确。因此,我们需要利用新增加的信息 y n + 1 \boldsymbol{y}_{n+1} yn+1对上述估计结果进行修正,得到 x ^ n + 1 ∣ n + 1 \hat{\boldsymbol{x}}_{n+1|n+1} x^n+1∣n+1。修正的过程我们越简单越好,因此我们可以大致猜测修正的方法为: x ^ n + 1 ∣ n + 1 = x ^ n + 1 ∣ n + K n + 1 ( y n + 1 − y ^ n + 1 ) \hat{\boldsymbol{x}}_{n+1|n+1}=\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{K}_{n+1}(\boldsymbol{y}_{n+1}-\hat{\boldsymbol{y}}_{n+1}) x^n+1∣n+1=x^n+1∣n+Kn+1(yn+1−y^n+1)。该方法用观测向量的估计误差来进行修正, K n + 1 \boldsymbol{K}_{n+1} Kn+1为修正系数,一方面它用于表示我们相信模型预测结果和相信观测值的程度,另一方面可以将其理解为对观测向量和状态向量之间相互转换的矩阵,因为观测向量和状态向量不仅可能量纲不同,甚至物理含义也不一样,因此不能直接用观测向量的估计误差来修正状态向量的预测误差,因此需要在两者之间做一个转化。显然上面的研究思路遵循了预测->修正的思路,即由 x ^ n ∣ n → x ^ n + 1 ∣ n → x ^ n + 1 ∣ n + 1 \hat{\boldsymbol{x}}_{n|n} \rightarrow \hat{\boldsymbol{x}}_{n+1|n} \rightarrow \hat{\boldsymbol{x}}_{n+1|n+1} x^n∣n→x^n+1∣n→x^n+1∣n+1的路线。以下通过严谨的推导来证明上述思路的可行性。
(1)
x
^
n
∣
n
→
x
^
n
+
1
∣
n
\hat{\boldsymbol{x}}_{n|n} \rightarrow \hat{\boldsymbol{x}}_{n+1|n}
x^n∣n→x^n+1∣n的过程推导
x
^
n
+
1
∣
n
=
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
x
n
+
1
=
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
(
F
n
x
n
+
v
n
)
=
F
n
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
x
n
+
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
v
n
=
F
n
x
^
n
∣
n
(4)
\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}(\boldsymbol{F}_n \boldsymbol{x}_n+\boldsymbol{v}_n)\\ &=\boldsymbol{F}_n{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_n+{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{v}_n=\boldsymbol{F}_n \hat{\boldsymbol{x}}_{n|n} \end{aligned} \end{equation}\tag{4}
x^n+1∣n=Proj(y1,...,yn)xn+1=Proj(y1,...,yn)(Fnxn+vn)=FnProj(y1,...,yn)xn+Proj(y1,...,yn)vn=Fnx^n∣n(4)
在(4)中需要理解的是
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
v
n
=
0
{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{v}_n=0
Proj(y1,...,yn)vn=0,即
v
n
\boldsymbol{v}_n
vn与
(
y
1
,
.
.
.
,
y
n
)
(\boldsymbol{y}_1,...,\boldsymbol{y}_n)
(y1,...,yn)是正交的,因为由观测方程可知,
(
y
1
,
.
.
.
,
y
n
)
(\boldsymbol{y}_1,...,\boldsymbol{y}_n)
(y1,...,yn)依赖
(
x
1
,
.
.
.
,
x
n
)
(\boldsymbol{x}_1,...,\boldsymbol{x}_n)
(x1,...,xn),而根据状态方程
(
x
1
,
.
.
.
,
x
n
)
(\boldsymbol{x}_1,...,\boldsymbol{x}_n)
(x1,...,xn)受
n
n
n时刻以前的状态噪声影响,并不受
n
n
n时刻的状态噪声影响,因此
v
n
\boldsymbol{v}_n
vn与
(
y
1
,
.
.
.
,
y
n
)
(\boldsymbol{y}_1,...,\boldsymbol{y}_n)
(y1,...,yn)是正交的。
(2)
x
^
n
+
1
∣
n
→
x
^
n
+
1
∣
n
+
1
\hat{\boldsymbol{x}}_{n+1|n} \rightarrow \hat{\boldsymbol{x}}_{n+1|n+1}
x^n+1∣n→x^n+1∣n+1的过程推导
x
^
n
+
1
∣
n
+
1
=
P
r
o
j
(
y
1
,
.
.
.
,
y
n
,
y
n
+
1
)
x
n
+
1
(5)
\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n+1}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})}\boldsymbol{x}_{n+1} \end{aligned} \end{equation}\tag{5}
x^n+1∣n+1=Proj(y1,...,yn,yn+1)xn+1(5)
由于在(4)中我们得到了
x
^
n
+
1
∣
n
=
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
x
n
+
1
\hat{\boldsymbol{x}}_{n+1|n}={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}
x^n+1∣n=Proj(y1,...,yn)xn+1,因此我们自然希望若(5)式满足
P
r
o
j
(
y
1
,
.
.
.
,
y
n
,
y
n
+
1
)
x
n
+
1
=
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
x
n
+
1
+
P
r
o
j
y
n
+
1
x
n
+
1
{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})}\boldsymbol{x}_{n+1}={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}+{\rm Proj}_{\boldsymbol{y}_{n+1}}\boldsymbol{x}_{n+1}
Proj(y1,...,yn,yn+1)xn+1=Proj(y1,...,yn)xn+1+Projyn+1xn+1,则可以利用到已有的结论。但是,遗憾的是,这个等式并不一定满足,因为
y
n
+
1
\boldsymbol{y}_{n+1}
yn+1并不一定与
(
y
1
,
.
.
.
,
y
n
,
y
n
+
1
)
(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})
(y1,...,yn,yn+1)张成的空间正交。因此一个很自然的思路就是对
y
n
+
1
\boldsymbol{y}_{n+1}
yn+1进行正交化,令
y
‾
n
+
1
=
y
n
+
1
−
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
y
n
+
1
\overline{\boldsymbol{y}}_{n+1}=\boldsymbol{y}_{n+1}-{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{y}_{n+1}
yn+1=yn+1−Proj(y1,...,yn)yn+1,据此,可将(5)改写为
x
^
n
+
1
∣
n
+
1
=
P
r
o
j
(
y
1
,
.
.
.
,
y
n
,
y
n
+
1
)
x
n
+
1
=
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
x
n
+
1
+
P
r
o
j
y
‾
n
+
1
x
n
+
1
=
x
^
n
+
1
∣
n
+
E
(
x
n
+
1
y
‾
n
+
1
T
)
[
E
(
y
‾
n
+
1
y
‾
n
+
1
T
)
]
−
1
y
‾
n
+
1
(6)
\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n+1}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})}\boldsymbol{x}_{n+1}={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}+{\rm Proj}_{\overline{\boldsymbol{y}}_{n+1}}\boldsymbol{x}_{n+1}\\ &=\hat{\boldsymbol{x}}_{n+1|n}+{\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\overline{\boldsymbol{y}}_{n+1} \end{aligned} \end{equation}\tag{6}
x^n+1∣n+1=Proj(y1,...,yn,yn+1)xn+1=Proj(y1,...,yn)xn+1+Projyn+1xn+1=x^n+1∣n+E(xn+1yn+1T)[E(yn+1yn+1T)]−1yn+1(6)
由于
y
‾
n
+
1
=
y
n
+
1
−
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
y
n
+
1
=
y
n
+
1
−
P
r
o
j
(
y
1
,
.
.
.
,
y
n
)
(
H
n
+
1
x
n
+
1
+
w
n
+
1
)
=
y
n
+
1
−
H
n
+
1
x
^
n
+
1
∣
n
\overline{\boldsymbol{y}}_{n+1}=\boldsymbol{y}_{n+1}-{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{y}_{n+1}=\boldsymbol{y}_{n+1}-{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}(\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}+\boldsymbol{w}_{n+1})=\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}
yn+1=yn+1−Proj(y1,...,yn)yn+1=yn+1−Proj(y1,...,yn)(Hn+1xn+1+wn+1)=yn+1−Hn+1x^n+1∣n,将其代入(6)有
x
^
n
+
1
∣
n
+
1
=
x
^
n
+
1
∣
n
+
E
(
x
n
+
1
y
‾
n
+
1
T
)
[
E
(
y
‾
n
+
1
y
‾
n
+
1
T
)
]
−
1
y
‾
n
+
1
=
x
^
n
+
1
∣
n
+
K
n
+
1
(
y
n
+
1
−
H
n
+
1
x
^
n
+
1
∣
n
)
(7)
\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n+1}&=\hat{\boldsymbol{x}}_{n+1|n}+{\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\overline{\boldsymbol{y}}_{n+1}\\ &=\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{K}_{n+1}(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}) \end{aligned} \end{equation}\tag{7}
x^n+1∣n+1=x^n+1∣n+E(xn+1yn+1T)[E(yn+1yn+1T)]−1yn+1=x^n+1∣n+Kn+1(yn+1−Hn+1x^n+1∣n)(7)
从(7)可以看出,该修正形式完全符合上面的猜想,即用观测向量的估计误差来修正状态向量的预测值。其中修正权值
K
n
+
1
=
E
(
x
n
+
1
y
‾
n
+
1
T
)
[
E
(
y
‾
n
+
1
y
‾
n
+
1
T
)
]
−
1
\boldsymbol{K}_{n+1}={\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}
Kn+1=E(xn+1yn+1T)[E(yn+1yn+1T)]−1被称为卡尔曼增益。
(3)
K
n
+
1
=
E
(
x
n
+
1
y
‾
n
+
1
T
)
[
E
(
y
‾
n
+
1
y
‾
n
+
1
T
)
]
−
1
\boldsymbol{K}_{n+1}={\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}
Kn+1=E(xn+1yn+1T)[E(yn+1yn+1T)]−1的计算和化简
E
(
x
n
+
1
y
‾
n
+
1
T
)
=
E
(
x
n
+
1
(
y
n
+
1
−
H
n
+
1
x
^
n
+
1
∣
n
)
T
)
=
E
(
x
n
+
1
(
H
n
+
1
x
n
+
1
−
H
n
+
1
x
^
n
+
1
∣
n
+
w
n
+
1
)
T
)
=
E
(
x
n
+
1
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
T
)
H
n
+
1
T
=
E
(
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
T
)
H
n
+
1
T
=
P
n
+
1
∣
n
H
n
+
1
T
(8)
\begin{equation} \begin{aligned} {\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})&={\rm E}(\boldsymbol{x}_{n+1}(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\\ &={\rm E}(\boldsymbol{x}_{n+1}(\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{w}_{n+1})^{\rm T})\\ &={\rm E}(\boldsymbol{x}_{n+1}(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\boldsymbol{H}_{n+1}^{\rm T}\\ &={\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\boldsymbol{H}_{n+1}^{\rm T}=\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T} \end{aligned} \end{equation}\tag{8}
E(xn+1yn+1T)=E(xn+1(yn+1−Hn+1x^n+1∣n)T)=E(xn+1(Hn+1xn+1−Hn+1x^n+1∣n+wn+1)T)=E(xn+1(xn+1−x^n+1∣n)T)Hn+1T=E((xn+1−x^n+1∣n)(xn+1−x^n+1∣n)T)Hn+1T=Pn+1∣nHn+1T(8)
在(8)中关键的化简步骤是第3个等号到第4个等号的变换,这边利用了
x
^
n
+
1
∣
n
\hat{\boldsymbol{x}}_{n+1|n}
x^n+1∣n和
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})
(xn+1−x^n+1∣n)正交的性质。
E
(
y
‾
n
+
1
y
‾
n
+
1
T
)
=
E
(
(
y
n
+
1
−
H
n
+
1
x
^
n
+
1
∣
n
)
(
y
n
+
1
−
H
n
+
1
x
^
n
+
1
∣
n
)
T
)
=
E
(
(
H
n
+
1
x
n
+
1
−
H
n
+
1
x
^
n
+
1
∣
n
+
w
n
+
1
)
(
H
n
+
1
x
n
+
1
−
H
n
+
1
x
^
n
+
1
∣
n
+
w
n
+
1
)
T
)
=
H
n
+
1
E
(
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
T
)
H
n
+
1
T
+
E
(
w
n
+
1
w
n
+
1
T
)
=
H
n
+
1
P
n
+
1
∣
n
H
n
+
1
T
+
R
n
+
1
(9)
\begin{equation} \begin{aligned} {\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})&={\rm E}((\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\\ &={\rm E}((\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{w}_{n+1})(\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{w}_{n+1})^{\rm T})\\ &=\boldsymbol{H}_{n+1}{\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\boldsymbol{H}_{n+1}^{\rm T}+{\rm E}(\boldsymbol{w}_{n+1}\boldsymbol{w}_{n+1}^T)\\ &=\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}+\boldsymbol{R}_{n+1} \end{aligned} \end{equation}\tag{9}
E(yn+1yn+1T)=E((yn+1−Hn+1x^n+1∣n)(yn+1−Hn+1x^n+1∣n)T)=E((Hn+1xn+1−Hn+1x^n+1∣n+wn+1)(Hn+1xn+1−Hn+1x^n+1∣n+wn+1)T)=Hn+1E((xn+1−x^n+1∣n)(xn+1−x^n+1∣n)T)Hn+1T+E(wn+1wn+1T)=Hn+1Pn+1∣nHn+1T+Rn+1(9)
在(9)中
R
n
+
1
\boldsymbol{R}_{n+1}
Rn+1表示观测噪声协方差矩阵。所以
K
n
+
1
=
E
(
x
n
+
1
y
‾
n
+
1
T
)
[
E
(
y
‾
n
+
1
y
‾
n
+
1
T
)
]
−
1
=
P
n
+
1
∣
n
H
n
+
1
T
(
H
n
+
1
P
n
+
1
∣
n
H
n
+
1
T
+
R
n
+
1
)
−
1
(10)
\begin{equation} \begin{aligned} \boldsymbol{K}_{n+1}&={\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\\ &=\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}(\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}+\boldsymbol{R}_{n+1})^{-1} \end{aligned} \end{equation}\tag{10}
Kn+1=E(xn+1yn+1T)[E(yn+1yn+1T)]−1=Pn+1∣nHn+1T(Hn+1Pn+1∣nHn+1T+Rn+1)−1(10)
上面似乎完成了系统状态向量的预测和修正过程,但是需要注意的是, P n + 1 ∣ n \boldsymbol{P}_{n+1|n} Pn+1∣n表示状态向量预测误差的协方差矩阵,这个东西无法从已知的可用的信息中获取,因此上面状态向量的更新和迭代过程,其实并没有真正完成。因此我们需要思考如何能够获得 P n + 1 ∣ n \boldsymbol{P}_{n+1|n} Pn+1∣n?由于系统真实的状态向量没有办法得知,因此直接根据定义是没有办法计算 P n + 1 ∣ n \boldsymbol{P}_{n+1|n} Pn+1∣n的,这时我们自然想到能不能仿照状态向量的更新迭代过程,用迭代的形式来获取 P n + 1 ∣ n \boldsymbol{P}_{n+1|n} Pn+1∣n,我们希望完成下面的过程: P n ∣ n → P n + 1 ∣ n → P n + 1 ∣ n + 1 \boldsymbol{P}_{n|n} \rightarrow \boldsymbol{P}_{n+1|n} \rightarrow \boldsymbol{P}_{n+1|n+1} Pn∣n→Pn+1∣n→Pn+1∣n+1。
(4)
P
n
∣
n
→
P
n
+
1
∣
n
\boldsymbol{P}_{n|n} \rightarrow \boldsymbol{P}_{n+1|n}
Pn∣n→Pn+1∣n的推导过程
P
n
+
1
∣
n
=
E
(
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
T
)
=
E
(
(
F
n
x
n
−
x
^
n
+
1
∣
n
+
v
n
)
(
F
n
x
n
−
x
^
n
+
1
∣
n
+
v
n
)
T
)
=
E
(
(
F
n
x
n
−
F
n
x
^
n
∣
n
+
v
n
)
(
F
n
x
n
−
F
n
x
^
n
∣
n
+
v
n
)
T
)
=
F
n
E
(
(
x
n
−
x
^
n
∣
n
)
(
x
n
−
x
^
n
∣
n
)
T
)
F
n
T
+
E
(
v
n
v
n
)
T
=
F
n
P
n
∣
n
F
n
T
+
Q
n
(11)
\begin{equation} \begin{aligned} \boldsymbol{P}_{n+1|n}&={\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\\ &={\rm E}((\boldsymbol{F}_n \boldsymbol{x}_n-\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{v}_n)(\boldsymbol{F}_n \boldsymbol{x}_n-\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{v}_n)^{\rm T})\\ &={\rm E}((\boldsymbol{F}_n \boldsymbol{x}_n-\boldsymbol{F}_n \hat{\boldsymbol{x}}_{n|n}+\boldsymbol{v}_n)(\boldsymbol{F}_n \boldsymbol{x}_n-\boldsymbol{F}_n \hat{\boldsymbol{x}}_{n|n}+\boldsymbol{v}_n)^{\rm T})\\ &=\boldsymbol{F}_n{\rm E}((\boldsymbol{x}_n-\hat{\boldsymbol{x}}_{n|n})(\boldsymbol{x}_n-\hat{\boldsymbol{x}}_{n|n})^{\rm T})\boldsymbol{F}_n^{\rm T}+{\rm E}(\boldsymbol{v}_n\boldsymbol{v}_n)^{\rm T}\\ &=\boldsymbol{F}_n \boldsymbol{P}_{n|n}\boldsymbol{F}_n^{\rm T}+\boldsymbol{Q}_n \end{aligned} \end{equation}\tag{11}
Pn+1∣n=E((xn+1−x^n+1∣n)(xn+1−x^n+1∣n)T)=E((Fnxn−x^n+1∣n+vn)(Fnxn−x^n+1∣n+vn)T)=E((Fnxn−Fnx^n∣n+vn)(Fnxn−Fnx^n∣n+vn)T)=FnE((xn−x^n∣n)(xn−x^n∣n)T)FnT+E(vnvn)T=FnPn∣nFnT+Qn(11)
在(11)的推导过程中用到了(1)的推导结果。
Q
n
\boldsymbol{Q}_n
Qn表示状态噪声协方差矩阵。
(5)
P
n
+
1
∣
n
→
P
n
+
1
∣
n
+
1
\boldsymbol{P}_{n+1|n} \rightarrow \boldsymbol{P}_{n+1|n+1}
Pn+1∣n→Pn+1∣n+1的推导过程
P
n
+
1
∣
n
+
1
=
E
(
(
x
n
+
1
−
x
^
n
+
1
∣
n
+
1
)
(
x
n
+
1
−
x
^
n
+
1
∣
n
+
1
)
T
)
(12)
\begin{equation} \begin{aligned} \boldsymbol{P}_{n+1|n+1}&={\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n+1})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n+1})^{\rm T}) \end{aligned} \end{equation}\tag{12}
Pn+1∣n+1=E((xn+1−x^n+1∣n+1)(xn+1−x^n+1∣n+1)T)(12)
其中
x
n
+
1
−
x
^
n
+
1
∣
n
+
1
=
x
n
+
1
−
x
^
n
+
1
∣
n
−
K
n
+
1
(
y
n
+
1
−
H
n
+
1
x
^
n
+
1
∣
n
)
=
x
n
+
1
−
x
^
n
+
1
∣
n
−
K
n
+
1
(
H
n
+
1
x
n
+
1
−
H
n
+
1
x
^
n
+
1
∣
n
+
w
n
+
1
)
=
(
I
−
K
n
+
1
H
n
+
1
)
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
−
K
n
+
1
w
n
+
1
(13)
\begin{equation} \begin{aligned} \boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n+1}&=\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n}-\boldsymbol{K}_{n+1}(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})\\ &=\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n}-\boldsymbol{K}_{n+1}(\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{w}_{n+1})\\ &=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})-\boldsymbol{K}_{n+1}\boldsymbol{w}_{n+1} \end{aligned} \end{equation}\tag{13}
xn+1−x^n+1∣n+1=xn+1−x^n+1∣n−Kn+1(yn+1−Hn+1x^n+1∣n)=xn+1−x^n+1∣n−Kn+1(Hn+1xn+1−Hn+1x^n+1∣n+wn+1)=(I−Kn+1Hn+1)(xn+1−x^n+1∣n)−Kn+1wn+1(13)
所以
P
n
+
1
∣
n
+
1
=
E
(
(
(
I
−
K
n
+
1
H
n
+
1
)
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
−
K
n
+
1
w
n
+
1
)
(
(
I
−
K
n
+
1
H
n
+
1
)
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
−
K
n
+
1
w
n
+
1
)
T
)
=
(
I
−
K
n
+
1
H
n
+
1
)
E
(
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
(
x
n
+
1
−
x
^
n
+
1
∣
n
)
T
)
(
I
−
K
n
+
1
H
n
+
1
)
T
+
K
n
+
1
E
(
w
n
+
1
w
n
+
1
T
)
K
n
+
1
T
=
(
I
−
K
n
+
1
H
n
+
1
)
P
n
+
1
∣
n
(
I
−
K
n
+
1
H
n
+
1
)
T
+
K
n
+
1
R
n
+
1
K
n
+
1
T
=
P
n
+
1
∣
n
−
P
n
+
1
∣
n
H
n
+
1
T
K
n
+
1
T
−
K
n
+
1
H
n
+
1
P
n
+
1
∣
n
+
K
n
+
1
H
n
+
1
P
n
+
1
∣
n
H
n
+
1
T
K
n
+
1
T
+
K
n
+
1
R
n
+
1
K
n
+
1
T
=
P
n
+
1
∣
n
−
P
n
+
1
∣
n
H
n
+
1
T
K
n
+
1
T
−
K
n
+
1
H
n
+
1
P
n
+
1
∣
n
+
K
n
+
1
(
H
n
+
1
P
n
+
1
∣
n
H
n
+
1
T
+
R
n
+
1
)
K
n
+
1
T
=
(
I
−
K
n
+
1
H
n
+
1
)
P
n
+
1
∣
n
(14)
\begin{equation} \begin{aligned} \boldsymbol{P}_{n+1|n+1}&={\rm E}(((\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})-\boldsymbol{K}_{n+1}\boldsymbol{w}_{n+1})((\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})-\boldsymbol{K}_{n+1}\boldsymbol{w}_{n+1})^{\rm T})\\ &=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1}){\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})^{\rm T}+\boldsymbol{K}_{n+1}{\rm E}(\boldsymbol{w}_{n+1}\boldsymbol{w}_{n+1}^{\rm T})\boldsymbol{K}_{n+1}^{\rm T}\\ &=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})\boldsymbol{P}_{n+1|n}(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})^{\rm T}+\boldsymbol{K}_{n+1}\boldsymbol{R}_{n+1}\boldsymbol{K}_{n+1}^{\rm T}\\ &=\boldsymbol{P}_{n+1|n}-\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}\boldsymbol{K}_{n+1}^{\rm T}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}+\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}\boldsymbol{K}_{n+1}^{\rm T}+\boldsymbol{K}_{n+1}\boldsymbol{R}_{n+1}\boldsymbol{K}_{n+1}^{\rm T}\\ &=\boldsymbol{P}_{n+1|n}-\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}\boldsymbol{K}_{n+1}^{\rm T}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}+\boldsymbol{K}_{n+1}(\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}+\boldsymbol{R}_{n+1})\boldsymbol{K}_{n+1}^{\rm T}\\ &=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})\boldsymbol{P}_{n+1|n} \end{aligned} \end{equation}\tag{14}
Pn+1∣n+1=E(((I−Kn+1Hn+1)(xn+1−x^n+1∣n)−Kn+1wn+1)((I−Kn+1Hn+1)(xn+1−x^n+1∣n)−Kn+1wn+1)T)=(I−Kn+1Hn+1)E((xn+1−x^n+1∣n)(xn+1−x^n+1∣n)T)(I−Kn+1Hn+1)T+Kn+1E(wn+1wn+1T)Kn+1T=(I−Kn+1Hn+1)Pn+1∣n(I−Kn+1Hn+1)T+Kn+1Rn+1Kn+1T=Pn+1∣n−Pn+1∣nHn+1TKn+1T−Kn+1Hn+1Pn+1∣n+Kn+1Hn+1Pn+1∣nHn+1TKn+1T+Kn+1Rn+1Kn+1T=Pn+1∣n−Pn+1∣nHn+1TKn+1T−Kn+1Hn+1Pn+1∣n+Kn+1(Hn+1Pn+1∣nHn+1T+Rn+1)Kn+1T=(I−Kn+1Hn+1)Pn+1∣n(14)
需要注意的是在(14)中最后一个等式的化简过程中,用到了(3)的推导结果,并进行合并同类项。
至此,我们完成了卡尔曼滤波的整个推导过程,对上面整个推导过程进行整合,可以得到卡尔曼滤波包含以下5个关系式:
x
^
n
+
1
∣
n
=
F
n
x
^
n
∣
n
x
^
n
+
1
∣
n
+
1
=
x
^
n
+
1
∣
n
+
K
n
+
1
(
y
n
+
1
−
H
n
+
1
x
^
n
+
1
∣
n
)
K
n
+
1
=
P
n
+
1
∣
n
H
n
+
1
T
(
H
n
+
1
P
n
+
1
∣
n
H
n
+
1
+
R
n
+
1
)
−
1
P
n
+
1
∣
n
=
F
n
P
n
∣
n
F
n
T
+
Q
n
P
n
+
1
∣
n
+
1
=
(
I
−
K
n
+
1
H
n
+
1
)
P
n
+
1
∣
n
(15)
\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n}&=\boldsymbol{F}_n \hat{\boldsymbol{x}}_{n|n}\\ \hat{\boldsymbol{x}}_{n+1|n+1}&=\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{K}_{n+1}(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})\\ \boldsymbol{K}_{n+1}&=\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}(\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}+\boldsymbol{R}_{n+1})^{-1}\\ \boldsymbol{P}_{n+1|n}&=\boldsymbol{F}_n\boldsymbol{P}_{n|n}\boldsymbol{F}_n^{\rm T}+\boldsymbol{Q}_n\\ \boldsymbol{P}_{n+1|n+1}&=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})\boldsymbol{P}_{n+1|n} \end{aligned} \end{equation}\tag{15}
x^n+1∣nx^n+1∣n+1Kn+1Pn+1∣nPn+1∣n+1=Fnx^n∣n=x^n+1∣n+Kn+1(yn+1−Hn+1x^n+1∣n)=Pn+1∣nHn+1T(Hn+1Pn+1∣nHn+1+Rn+1)−1=FnPn∣nFnT+Qn=(I−Kn+1Hn+1)Pn+1∣n(15)
可以看出,上述5个方程描述了两个方面的内容,一个是状态向量的预测和修正(前三个方程),另一个是预测协方差矩阵的迭代调整(相当于预测误差的实时调整)。在实际应用过程中需要给定状态向量初始值、状态向量预测误差协方差矩阵初始值、状态噪声协方差矩阵、观测噪声协方差矩阵,然后系统模型(状态转移矩阵)和观测模型(观测矩阵)即可按上式进行迭代处理。