准备知识
矩阵的期望:
正交矩阵:如果
A
A
T
AA^T
AAT=E(E为单位矩阵,
A
T
A^T
AT表示“矩阵A的转置矩阵”)或
A
T
A
=
E
A^TA=E
ATA=E,则n阶实矩阵A称为正交矩阵。
协方差矩阵:越线性相关,协方差越大, 两个变量完全线性无关,协方差为零
1、状态空间模型
X
n
+
1
=
A
n
+
1
,
n
X
n
+
W
n
(式1)
X_{n+1} = A{n+1},nX_n + W_{n}\tag {式1}
Xn+1=An+1,nXn+Wn(式1)
Y
n
=
B
n
X
n
+
V
n
(式2)
Y_n = B_nX_n + V_n \tag {式2}
Yn=BnXn+Vn(式2)
该模型涉及的参数如下:
(1)状态转移矩阵
A
n
+
1
,
n
A_{n+1,n}
An+1,n,它是可逆的。(在线性代数中,给定一个n阶方阵A,若存在一n阶方阵B使得AB=BA=E(或AB=E、BA=E任满足一个),其中E为n阶单位矩阵,则称A是可逆的,且B是A的逆阵,记作
A
−
1
A^{-1}
A−1);
(2)测量矩阵
B
n
B_n
Bn,通常情况下它是长方形矩阵;
(3)高斯动态噪声
W
n
W_n
Wn,假设它有零均值且有协方差矩阵
Q
w
,
n
Q_{w,n}
Qw,n;
(4)高斯测量噪声
V
n
V_n
Vn,假设它有零均值且有协方差矩阵
Q
v
,
n
Q_{v,n}
Qv,n;
2、新息过程
处理此类优化估计问题,是利用观察量
y
n
y_n
yn的所谓新息过程。其定义如下:
a
n
=
y
n
−
y
^
n
∣
n
−
1
(式3)
a_n = y_n-\hat y_{n|n-1} \tag {式3}
an=yn−y^n∣n−1(式3)
其中
y
^
n
∣
n
−
1
\hat y_{n|n-1}
y^n∣n−1是在给定至
n
−
1
n-1
n−1时刻所有观测值的情况下,对
y
n
y_n
yn的最小均方差的估计。
可以说:
新息过程
a
n
a_n
an是包含在测量值
y
n
y_n
yn但不在
y
^
n
∣
n
−
1
\hat y_{n|n-1}
y^n∣n−1的预测部分的新信息的测量,因为
y
n
y_n
yn可以预测的部分(记为
y
^
n
∣
n
−
1
\hat y_{n|n-1}
y^n∣n−1)是完全由序列
{
y
k
}
k
=
1
n
−
1
\lbrace y_k\rbrace_{k=1} ^{n-1}
{yk}k=1n−1决定的。
新息过程有如下的重要性质:
性质 1:
与观测值
y
n
y_n
yn有关的新息过程
a
n
a_n
an与之前的所有观测值
y
1
,
y
2
,
.
.
.
,
y
n
−
1
y_1,y_2,...,y_{n-1}
y1,y2,...,yn−1正交,表示为:
E
[
a
n
y
k
T
]
=
0
,
0
≤
k
≤
n
−
1
(式4)
E[a_n y_k ^T] = 0, 0\leq k \leq n-1 \tag {式4}
E[anykT]=0,0≤k≤n−1(式4)
性质 2:
新息过程由一系列相互正交的随机向量构成,表示为:
E
[
a
n
a
k
T
]
=
0
,
0
≤
k
≤
n
−
1
(式5)
E[a_n a_k ^T] = 0, 0\leq k \leq n-1 \tag {式5}
E[anakT]=0,0≤k≤n−1(式5)
性质3:
代表观测数据的随机向量序列
{
y
1
,
y
2
,
.
.
.
.
,
y
n
−
1
}
\lbrace y_1,y_2,....,y_{n-1} \rbrace
{y1,y2,....,yn−1},与表示更新过程的序列
a
1
,
a
2
,
.
.
.
,
a
n
a_1,a_2,...,a_n
a1,a2,...,an一 一对应。因此,通过能够保证线性稳定并且不丢失任何信息的操作,可以从一个序列得到另一个序列,因此可以写作:
{
y
1
,
y
2
,
.
.
.
,
y
n
}
⟺
{
a
1
,
a
2
,
.
.
.
,
a
n
}
(式6)
\lbrace y_1,y_2,...,y_n\rbrace \iff { \lbrace a_1,a_2,...,a_n \rbrace} \tag {式6}
{y1,y2,...,yn}⟺{a1,a2,...,an}(式6)
总结:
该步骤就是给定给 n − 1 n-1 n−1时刻所有观测值,然后 n n n时刻的观察值做出估计,得到 y ^ n ∣ n − 1 \hat y_{n|n-1} y^n∣n−1,得到的估计值与观察值,理论上是存在误差的,定义为新息过程 a n a_n an。
3、新息过程的协方差矩阵
从初始状态
x
0
x_0
x0开始,可以用式1所描述的系统模型表示
k
k
k时刻的系统状态:
X
k
=
A
k
,
0
X
0
+
∑
i
=
1
k
−
1
A
k
,
i
W
i
(式7)
X_k = A_{k,0}X_0 + \sum _{i=1} ^{k-1}A{k,i} W_i \tag {式7}
Xk=Ak,0X0+i=1∑k−1Ak,iWi(式7)
式7表明状态
X
k
X_k
Xk是
X
0
X_0
X0以及
W
1
,
W
2
,
W
3
,
.
.
.
,
W
n
W_1,W_2,W_3,...,W_n
W1,W2,W3,...,Wn的线性组合。
根据假设,测量噪声
V
n
V_n
Vn与初始状态
X
0
X_0
X0以及动态噪声
W
i
W_i
Wi无关。因此,在式7两边同时乘以
V
n
T
V_n ^T
VnT后得到:
E
[
X
k
V
n
T
]
=
0
,
k
,
n
≥
0
(式8)
E[X_k V_n ^T] = 0 ,k,n \geq 0 \tag{式8}
E[XkVnT]=0,k,n≥0(式8)
同理可以从测量公式2得到:
E
[
Y
k
V
n
T
]
=
0
,
0
≤
k
≤
n
−
1
(式9)
E[Y_k V_n ^T] = 0 ,0 \leq k\leq n-1 \tag{式9}
E[YkVnT]=0,0≤k≤n−1(式9)
和
E
[
Y
k
W
n
T
]
=
0
,
0
≤
k
≤
n
(式10)
E[Y_k W_n ^T] = 0 ,0 \leq k\leq n \tag{式10}
E[YkWnT]=0,0≤k≤n(式10)
给定先前的观测值
y
1
,
.
.
.
,
y
k
y_1,...,y_k
y1,...,yk,我们可以从测量公式式2中得出当前的观测值
y
n
y_n
yn的最小均方估计为:
y
^
n
∣
n
−
1
=
B
x
^
n
∣
n
−
1
+
v
^
n
∣
n
−
1
(式11)
\hat y_{n|n-1} = B \hat x_{n|n-1} + \hat v_{n|n-1} \tag{式11}
y^n∣n−1=Bx^n∣n−1+v^n∣n−1(式11)
其中
v
^
n
∣
n
−
1
\hat v_{n|n-1}
v^n∣n−1是给先前的观测值
y
1
,
.
.
.
,
y
n
−
1
y_1,...,y_{n-1}
y1,...,yn−1后所对应的测量噪声估计。因为根据式9,
v
n
v_n
vn与先前的观测值是正交的,因此估计值
v
^
n
∣
n
−
1
\hat v_{n|n-1}
v^n∣n−1为零。于是化式11得到:
y
^
n
∣
n
−
1
=
B
n
x
^
n
∣
n
−
1
(式12)
\hat y_{n|n-1} = B_n \hat x_{n|n-1} \tag{式12}
y^n∣n−1=Bnx^n∣n−1(式12)
由于 (
y
n
=
b
n
x
n
+
v
n
y_n = b_nx_n + v_n
yn=bnxn+vn),再根据式12代入式3(
a
n
=
y
n
−
y
^
n
∣
n
−
1
a_n = y_n-\hat y_{n|n-1}
an=yn−y^n∣n−1),将项合并得:
a
n
=
B
n
ε
n
∣
n
−
1
+
v
n
(式13)
a_n = B_n \varepsilon _{n|n-1} + v_n \tag{式13}
an=Bnεn∣n−1+vn(式13)
其中,新引入的项
ε
n
∣
n
−
1
\varepsilon _{n|n-1}
εn∣n−1是状态预测误差向量。其定义为:
ε
n
∣
n
−
1
=
x
n
−
x
^
n
∣
n
−
1
(式14)
\varepsilon _{n|n-1} = x_n - \hat x_{n|n-1} \tag{式14}
εn∣n−1=xn−x^n∣n−1(式14)
ε
n
∣
n
−
1
\varepsilon _{n|n-1}
εn∣n−1与动态噪声
w
n
w_n
wn以及测量噪声
v
n
v_n
vn均是正交的。由此定义零均值新息过程
a
n
a_n
an的协方差矩阵为:
R
n
=
E
[
a
n
a
n
T
]
(式15)
R_n = E[a_n a_n ^T]\tag{式15}
Rn=E[ananT](式15)
利用式13,能得到:
R
n
=
B
n
P
n
∣
n
−
1
B
n
T
+
Q
n
(式16)
R_n = B_n P_{n|n-1}B_n^T + Q_n\tag{式16}
Rn=BnPn∣n−1BnT+Qn(式16)
其中
Q
v
,
n
Q_{v,n}
Qv,n是测量噪声
v
n
v_n
vn的协方差矩阵,新引入的项:
P
n
∣
n
−
1
=
E
[
ε
n
∣
n
−
1
ε
n
∣
n
−
1
T
]
(式17)
P_{n|n-1} = E[\varepsilon _{n|n-1} \varepsilon ^T_{n|n-1}] \tag{式17}
Pn∣n−1=E[εn∣n−1εn∣n−1T](式17)
为预测误差协方差矩阵,式16为卡尔曼滤波算法的第一步。
总结:
4 、利用新息过程进行滤波状态估计:预测-修正公式( X ^ n ∣ n = X ^ n ∣ n − 1 + G n a n \hat X_{n|n } = \hat X_{n|n - 1} +G_n a_n X^n∣n=X^n∣n−1+Gnan )
接下来需要做的是利用新息过程实现任意时刻
i
i
i系统状态
x
i
x_i
xi的最小均方误差估计,为此,给定新息序列
a
1
,
a
2
,
.
.
.
,
a
n
a_1,a_2,...,a_n
a1,a2,...,an,首先线性展开的形式表示对状态
x
i
x_i
xi的估计:
x
^
i
∣
n
=
∑
k
=
1
n
C
i
,
k
a
k
(式18)
\hat x _{i|n} = \sum _{k=1}^n C_{i,k} a_k \tag{式18}
x^i∣n=k=1∑nCi,kak(式18)
其中
{
C
j
,
k
}
k
=
1
n
\lbrace C_{j,k} \rbrace_{k=1} ^n
{Cj,k}k=1n是i时刻的展开式系数矩阵的集合,状态预测误差与新息过程满足下述正交条件:
E
[
ε
i
∣
n
a
k
T
]
=
0
当
k
=
1
,
2
,
.
.
.
,
n
且
i
≤
n
(式19)
E[\varepsilon _{i|n} a_k ^T]=0 当k=1,2,...,n且i\leq n \tag{式19}
E[εi∣nakT]=0当k=1,2,...,n且i≤n(式19)
因此将式18代入式19,使用式5所描述的信息过程的正交性,可得:
E
[
x
i
a
k
T
]
=
C
i
,
k
R
k
(式20)
E[x_i a_k ^T] = C_{i,k}R_k \tag{式20}
E[xiakT]=Ci,kRk(式20)
R
n
R_n
Rn是新息过程的协方差,解此方程的系数矩阵
C
i
,
k
C_{i,k}
Ci,k,得到:
C
i
,
k
=
E
[
x
i
a
k
T
]
R
k
−
1
(式21)
C_{i,k} = E[x_i a_k ^T] R_k ^{-1} \tag{式21}
Ci,k=E[xiakT]Rk−1(式21)
再利用式18得到:
x
^
i
∣
n
=
∑
k
=
1
n
E
[
x
i
a
k
T
]
R
k
−
1
a
k
(式22)
\hat x _{i|n} = \sum _{k=1}^nE[x_i a_k ^T] R_k ^{-1} a_k \tag{式22}
x^i∣n=k=1∑nE[xiakT]Rk−1ak(式22)
当i=n时,为滤波过程,因此式26描述该状态的滤波估计为:
x
^
n
∣
n
=
∑
k
=
1
n
E
[
x
n
a
k
T
]
R
k
−
1
a
k
=
∑
k
=
1
n
−
1
E
[
x
n
a
k
T
]
R
k
−
1
a
k
+
E
[
x
n
a
k
T
]
R
k
−
1
a
k
(式23)
\hat x _{n|n} = \sum _{k=1}^nE[x_n a_k ^T] R_k ^{-1} a_k = \sum _{k=1}^{n-1}E[x_n a_k ^T] R_k ^{-1} a_k+ E[x_n a_k ^T] R_k ^{-1} a_k \tag{式23}
x^n∣n=k=1∑nE[xnakT]Rk−1ak=k=1∑n−1E[xnakT]Rk−1ak+E[xnakT]Rk−1ak(式23)
把等式的第二项,
k
=
n
k=n
k=n的项从求和中分离出来,首先将:
x
^
n
∣
n
−
1
=
∑
k
=
1
n
−
1
E
[
x
n
a
k
T
]
R
k
−
1
a
k
v
(式24)
\hat x _{n|n-1} = \sum _{k=1}^{n-1}E[x_n a_k ^T] R_k ^{-1} a_kv \tag{式24}
x^n∣n−1=k=1∑n−1E[xnakT]Rk−1akv(式24)
其次引入下述定义:
G
n
=
E
[
x
n
a
k
T
]
R
k
−
1
(式25)
G_n = E[x_n a_k ^T] R_k ^{-1} \tag{式25}
Gn=E[xnakT]Rk−1(式25)
由此我们可以将状态滤波估计表示为下述递归的形式:
x
^
n
∣
n
=
x
^
n
∣
n
−
1
+
G
n
a
n
(式26)
\hat x _{n|n} = \hat x _{n|n-1} + G_n a_n \tag{式26}
x^n∣n=x^n∣n−1+Gnan(式26)
(1)
x
^
n
∣
n
−
1
\hat x _{n|n-1}
x^n∣n−1 表示单步预测,其表示在给定
n
−
1
n-1
n−1时刻前所有观测值的基础上对状态
x
n
x_n
xn的预测估计。
(2)
G
n
a
n
G_na_n
Gnan表示修正项,新息过程
a
n
a_n
an表示由观测值
y
n
y_n
yn引入滤波过程的新新息,乘以"增益因子"。因此,
G
n
G_n
Gn通常被称为卡尔曼增益矩阵。
5、卡尔曼增益的计算 G n = P n ∣ n − 1 B n T R n − 1 G_n=P_{n|n-1}B_n^TR_n^{-1} Gn=Pn∣n−1BnTRn−1
式26中引入了卡尔曼增益矩阵,需要得到相应的计算公式。
应用式13得:
E
[
X
n
a
n
T
]
=
E
[
X
n
(
B
n
ε
n
∣
n
−
1
+
v
n
)
T
]
=
E
[
X
n
ε
n
∣
n
−
1
T
]
B
n
T
E[X_na_n^T] = E[X_n(B_n \varepsilon _{n|n-1} +v_n)^T] = E[X_n \varepsilon _{n|n-1}^T]B_n^T
E[XnanT]=E[Xn(Bnεn∣n−1+vn)T]=E[Xnεn∣n−1T]BnT
在上式的第二行
E
[
X
n
(
B
n
ε
n
∣
n
−
1
+
v
n
)
T
]
E[X_n(B_n \varepsilon _{n|n-1} +v_n)^T]
E[Xn(Bnεn∣n−1+vn)T],利用状态
X
n
X_n
Xn与测量噪声
v
n
v_n
vn无关性,根据正交原理,状态预测误差向量
ε
n
∣
n
−
1
\varepsilon _{n|n-1}
εn∣n−1与状态估计
X
^
n
∣
n
−
1
\hat X_{n|n-1}
X^n∣n−1是正交的,因此
ε
n
∣
n
−
1
\varepsilon _{n|n-1}
εn∣n−1与
X
^
n
∣
n
−
1
\hat X_{n|n-1}
X^n∣n−1外积的期望为零,进而我们用
ε
n
∣
n
−
1
\varepsilon _{n|n-1}
εn∣n−1代替
X
n
X_n
Xn不影响期望
E
[
X
n
a
n
T
]
E[X_na_n^T]
E[XnanT],由此可得:
E
[
X
n
a
n
T
]
=
E
[
ε
n
∣
n
−
1
ε
n
∣
n
−
1
T
]
B
n
T
=
P
n
∣
n
−
1
B
n
T
E[X_na_n^T] = E[\varepsilon _{n|n-1}\varepsilon _{n|n-1}^T]B_n^T=P_{n|n-1}B_n^T
E[XnanT]=E[εn∣n−1εn∣n−1T]BnT=Pn∣n−1BnT
因此式25卡尔曼增益矩阵
G
n
G_n
Gn可以表示为:
G
n
=
P
n
∣
n
−
1
B
n
T
R
n
−
1
(式27)
G_n= P_{n|n-1}B_n^T R_n^{-1} \tag{式27}
Gn=Pn∣n−1BnTRn−1(式27)
6、用于更新预测误差协方差矩阵的黎卡堤查分方程
为了解决状态估计,将式14中的n替换为n+1,得到:
ε
n
+
1
∣
n
=
x
n
+
1
−
x
^
n
+
1
∣
n
\varepsilon _{n+1|n} = x_{n+1} - \hat x_{n+1|n}
εn+1∣n=xn+1−x^n+1∣n
通过上式可以看出含有滤波估计的项
x
^
n
+
1
∣
n
\hat x_{n+1|n}
x^n+1∣n表示状态的预测估计是有益的。故而将式24中的n替换为n+1,并运用式1,可得:
x
^
n
+
1
∣
n
=
∑
k
=
1
n
E
[
x
n
+
1
a
k
T
]
R
k
−
1
a
k
=
∑
k
=
1
n
E
[
A
n
+
1
,
n
X
n
+
W
n
a
k
T
]
R
k
−
1
a
k
=
A
n
+
1
,
n
∑
k
=
1
n
E
[
X
n
a
n
T
]
R
k
−
1
a
k
=
A
n
+
1
,
n
X
^
n
∣
n
\hat x_{n+1|n} = \sum_{k=1}^nE[x_{n+1} a_k ^T]R_k^{-1}a_k=\sum _{k=1}^nE[A_{n+1,n}X_n+W_na_k ^T]R_k^{-1}a_k=A_{n+1,n}\sum _{k=1}^nE[X_na_n ^T]R_k ^{-1}a_k=A_{n+1,n}\hat X_{n|n}
x^n+1∣n=k=1∑nE[xn+1akT]Rk−1ak=k=1∑nE[An+1,nXn+WnakT]Rk−1ak=An+1,nk=1∑nE[XnanT]Rk−1ak=An+1,nX^n∣n
(式28)
\tag{式28}
(式28)
因为动态噪声
W
n
W_n
Wn与观测值是相互独立的,故期望
E
[
w
k
a
k
T
]
E[w_ka_k^T]
E[wkakT]为零。对滤波估计
x
^
n
∣
n
\hat x_{n|n}
x^n∣n,应用式23,以及式28对状态
x
n
x_n
xn的预测滤波估计的关系,利用
ε
n
+
1
∣
n
\varepsilon _{n+1|n}
εn+1∣n的公式得到:
ε
n
+
1
∣
n
=
(
A
n
+
1
,
n
X
n
+
W
n
)
−
A
n
+
1
,
n
X
^
n
∣
n
=
A
n
+
1
,
n
(
X
n
−
X
^
n
∣
n
)
+
W
n
=
A
n
+
1
,
n
ε
n
∣
n
+
W
n
(式29)
\varepsilon _{n+1|n} = (A_{n+1,n}X_n+W_n) - A_{n+1,n} \hat X_{n|n} = A_{n+1,n}(X_n-\hat X_{n|n})+W_n=A_{n+1,n} \varepsilon _{n|n} + W_n \tag{式29}
εn+1∣n=(An+1,nXn+Wn)−An+1,nX^n∣n=An+1,n(Xn−X^n∣n)+Wn=An+1,nεn∣n+Wn(式29)
滤波误差向量定义为:
ε
n
∣
n
=
X
n
−
X
^
n
∣
n
(式30)
\varepsilon _{n|n} =X_n-\hat X_{n|n} \tag{式30}
εn∣n=Xn−X^n∣n(式30)
因为滤波误差向量
ε
n
∣
n
\varepsilon _{n|n}
εn∣n与动态噪声
w
n
w_n
wn是无关,可以将预测误差协方差矩阵表示为:
P
n
+
1
,
n
=
E
[
ε
n
+
1
∣
n
ε
n
+
1
∣
n
T
]
=
A
n
+
1
,
n
P
n
∣
n
A
n
+
1
T
+
Q
m
,
n
(式31)
P_{n+1,n} = E[\varepsilon _{n+1|n}\varepsilon _{n+1|n}^T]=A_{n+1,n}P_{n|n}A_{n+1}^T+Q_{m,n} \tag{式31}
Pn+1,n=E[εn+1∣nεn+1∣nT]=An+1,nPn∣nAn+1T+Qm,n(式31)
其中
Q
m
,
n
Q_{m,n}
Qm,n为动态噪声
w
n
w_n
wn的误差协方差矩阵。在式30中引入了最后一个参数,称为滤波误差协方差矩阵,其定义为:
P
n
∣
n
=
E
[
ε
n
∣
n
ε
n
∣
n
T
]
(式32)
P_{n|n}= E[\varepsilon _{n|n} \varepsilon _{n|n}^T] \tag{式32}
Pn∣n=E[εn∣nεn∣nT](式32)
为了计算误差协方差矩阵
P
n
∣
n
P_{n|n}
Pn∣n的式子,因此将式26代入式30得:
ε
n
∣
n
=
X
n
−
X
^
n
∣
n
−
1
−
G
n
a
n
=
ε
n
∣
n
−
1
−
G
n
a
n
\varepsilon _{n|n} = X_n-\hat X_{n|n-1}-G_na_n=\varepsilon _{n|n-1}-G_na_n
εn∣n=Xn−X^n∣n−1−Gnan=εn∣n−1−Gnan
然后应用式32,得到:
P
n
∣
n
=
E
[
(
ε
n
∣
n
−
1
−
G
n
a
n
)
(
(
ε
n
∣
n
−
1
−
G
n
a
n
)
T
]
P_{n|n}=E[(\varepsilon _{n|n-1}-G_na_n)((\varepsilon _{n|n-1}-G_na_n)^T ]
Pn∣n=E[(εn∣n−1−Gnan)((εn∣n−1−Gnan)T]
=
E
[
ε
n
∣
n
−
1
ε
n
∣
n
−
1
T
]
−
G
n
E
[
a
n
ε
n
∣
n
−
1
T
]
−
E
[
ε
n
∣
n
−
1
a
n
T
]
G
n
T
+
G
n
E
[
a
n
a
n
T
]
G
n
T
=E[\varepsilon _{n|n-1}\varepsilon _{n|n-1}^T]-G_nE[a_n\varepsilon _{n|n-1}^T]-E[\varepsilon _{n|n-1}a_n^T]G_n^T+G_nE[a_na_n^T]G_n^T
=E[εn∣n−1εn∣n−1T]−GnE[anεn∣n−1T]−E[εn∣n−1anT]GnT+GnE[ananT]GnT
=
P
n
∣
n
−
1
−
G
n
E
[
a
n
ε
n
∣
n
−
1
T
]
−
E
[
ε
n
∣
n
−
1
a
n
T
]
+
G
n
R
n
G
n
T
(式33)
=P_{n|n-1}-G_nE[a_n \varepsilon _{n|n-1}^T]-E[ \varepsilon _{n|n-1} a_n^T]+G_nR_nG_n^T \tag{式33}
=Pn∣n−1−GnE[anεn∣n−1T]−E[εn∣n−1anT]+GnRnGnT(式33)
因为
X
^
n
∣
n
−
1
\hat X_{n|n-1}
X^n∣n−1与新息过程
a
n
a_n
an正交,于是得到:
E
[
ε
n
∣
n
−
1
a
n
T
]
=
E
[
(
X
n
−
X
^
n
∣
n
−
1
)
a
n
T
]
=
E
[
X
n
a
n
T
]
E[\varepsilon _{n|n-1}a_n^T] = E[(X_n-\hat X_{n|n-1})a_n^T]=E[X_na_n^T]
E[εn∣n−1anT]=E[(Xn−X^n∣n−1)anT]=E[XnanT]
同理:
E
[
a
n
ε
n
∣
n
−
1
T
]
=
E
[
a
n
X
n
T
]
E[a_n\varepsilon _{n|n-1}^T]=E[a_nX_n^T]
E[anεn∣n−1T]=E[anXnT]
利用式27中对卡尔曼增益的定义,易得:
G
n
E
[
a
n
ε
n
∣
n
−
1
T
]
=
E
[
ε
n
∣
n
−
1
a
n
T
]
G
n
T
=
G
n
R
n
G
n
T
G_nE[a_n\varepsilon _{n|n-1}^T]=E[\varepsilon _{n|n-1}a_n^T]G_n^T=G_nR_nG_n^T
GnE[anεn∣n−1T]=E[εn∣n−1anT]GnT=GnRnGnT
根据式33化简得
P
n
∣
n
=
P
n
∣
n
−
1
−
G
n
R
n
G
n
T
P_{n|n}=P_{n|n-1}-G_nR_nG_n^T
Pn∣n=Pn∣n−1−GnRnGnT
利用式27以及协方差矩阵
R
n
R_n
Rn和
P
n
∣
n
−
1
P_{n|n-1}
Pn∣n−1的对称性得到:
P
n
∣
n
=
P
n
∣
n
−
1
−
G
n
B
n
P
n
∣
n
−
1
(式33)
P_{n|n}=P_{n|n-1}-G_nB_nP_{n|n-1} \tag{式33}
Pn∣n=Pn∣n−1−GnBnPn∣n−1(式33)