本文深入探讨了卡尔曼滤波的基础，包括状态空间方程、卡尔曼增益的推导以及卡尔曼滤波的几何解释。通过线性系统和高斯噪声的分析，阐述了为何系统噪声必须是高斯白噪声，并详细展示了卡尔曼增益公式如何确保最优估计。最后，利用几何方法直观地解释了卡尔曼滤波的最小方差无偏估计特性。
一、状态空间方程
推导过程请看上一篇博客
状态方程：
x
k
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
−
1
(1)
\begin{gathered} \boldsymbol{x}_{k}=\boldsymbol{A} \boldsymbol{x}_{k-1}+B u_{k-1}+\boldsymbol{w}_{k-1}\tag1 \end{gathered}
xk=Axk−1+Buk−1+wk−1(1)
观测方程：
z
k
=
H
x
k
+
v
k
(2)
\begin{gathered} \boldsymbol{z}_{k}=H \boldsymbol{x}_{k}+\boldsymbol{v}_{k}\tag2 \end{gathered}
zk=Hxk+vk(2)
高斯噪音：
p
(
ω
k
)
∼
N
(
0
,
Q
)
p
(
v
k
)
∼
N
(
0
,
R
)
\begin{aligned} &p\left(\omega_{k}\right) \sim N(0, Q) \\ &p\left(v_{k}\right) \sim N(0, R) \end{aligned}
p(ωk)∼N(0,Q)p(vk)∼N(0,R)
其中， 0 为期望值，
Q
Q
Q 为协方差矩阵。根据协方差定义
Q
=
E
(
w
w
T
)
Q=E\left(\begin{array}{ll}ww^{T}\end{array}\right)
Q=E(wwT), 可得:
Q
=
E
[
[
w
1
w
2
]
[
w
1
w
2
]
]
=
[
E
w
1
2
E
(
w
1
w
2
)
E
(
w
2
w
1
)
E
w
2
2
]
⟶
已
知
V
a
r
(
w
)
=
E
(
w
2
)
−
(
E
w
)
2
又
E
w
=
0
⇒
V
a
r
(
w
)
=
E
(
w
2
)
=
σ
w
2
=
[
σ
w
1
2
σ
w
1
σ
w
2
σ
w
2
σ
w
1
σ
w
2
2
]
⟶
负
对
角
线
σ
w
1
σ
w
2
不
是
两
个
标
准
差
相
乘
，
而
是
代
表
了
w
1
和
w
2
的
协
方
差
。
\begin{aligned} Q &=E\left[\left[\begin{array}{l} w_{1} \\ w_{2} \end{array}\right]\left[\begin{array}{ll} w_{1} & w_{2} \end{array}\right]\right] \\ &=\left[\begin{array}{cc} E w_{1}^{2} & E (w_{1} w_{2}) \\ E (w_{2} w_{1}) & E w_{2}^{2} \end{array}\right] \\ &\longrightarrow 已知Var(w)=E(w^2)-(Ew)^2又Ew=0\Rightarrow Var(w)=E(w^2)= \sigma^2_w\\ &=\left[\begin{array}{cc} \sigma_{w_{1}}^{2} & \sigma_{w_{1}} \sigma_{w_{2}} \\ \sigma_{w_{2}} \sigma_{w_{1}} & \sigma_{w_{2}}^{2} \end{array}\right]\\ &\longrightarrow 负对角线\sigma_{w_{1}} \sigma_{w_{2}}不是两个标准差相乘，而是代表了w1和w2的协方差。 \end{aligned}
Q=E[[w1w2][w1w2]]=[Ew12E(w2w1)E(w1w2)Ew22]⟶已知Var(w)=E(w2)−(Ew)2又Ew=0⇒Var(w)=E(w2)=σw2=[σw12σw2σw1σw1σw2σw22]⟶负对角线σw1σw2不是两个标准差相乘，而是代表了w1和w2的协方差。
为什么给系统加入的噪声必须是高斯白噪声？
- 高斯噪声在工程实践过程中非常常见；
- 卡尔曼滤波是针对线性系统而言的，而高斯噪声通过线性系统后其依旧服从高斯分布，对于后续比较好操作。
高斯噪声的线性变换
主要符号表
| 符号 | 意义 | 备注 |
|---|---|---|
| x k x_{k} xk | 系统状态矩阵(真实值) | 计算得到 |
| z k z_{k} zk | 状态阵的观测量 | 实际测量 |
| A A A | 状态转移矩阵 | – |
| B B B | 控制输入矩阵 | – |
| H H H | 状态观测矩阵 | – |
| w k − 1 w_{k-1} wk−1 | 过程噪声 | 服从正态分布 |
| v k v_{k} vk | 测量噪声 | 服从正态分布 |
| Q Q Q | 过程噪声协方差矩阵 | – |
| R R R | 测量噪声协方差矩阵 | E [ v k v k T ] E[v_{k} v_{k}^{T}] E[vkvkT] |
| x ^ k − \hat{\boldsymbol{x}}_{k}^{-} x^k− | 先验估计值 | 未处理的估计值 |
| x ^ k m e a \hat{\boldsymbol{x}}_{k m e a} x^kmea | 系统状态矩阵 | 实际测量 |
| x ^ k \hat{x}_{k} x^k | 系统状态矩阵估计值 | 估计得到 |
| K k K_{k} Kk | 卡尔曼增益 | – |
| e k e_{k} ek | 状态误差 | x k − x ^ k x_{k}-\hat{x}_{k} xk−x^k |
| P P P | 误差协方差矩阵 | E [ e e T ] E[e e^{T}] E[eeT] |
| e k − e^-_{k} ek− | 先验状态误差 | x k − x ^ k − x_{k}-\hat{x}_{k}^{-} xk−x^k− |
| I I I | 单位阵 | – |
| P k − P_{k}^{-} Pk− | 先验误差协方差矩阵 | E [ e k − e k − T ] E[e_{k}^{-} e_{k}^{-T}] E[ek−ek−T] |
| z k z_{k} zk | 状态阵的观测量 | 实际测量 |
| P k P_{k} Pk | 后验误差协方差矩阵 | E [ e k e k T ] E[e_{k} e_{k}^{T}] E[ekekT] |
二、卡尔曼增益公式推导
前几篇博客都是对线性系统的卡尔曼进行了了解，而在生产生活中有太多的不确定性，对于大多数实际的控制系统而言，它并不是一个严格的线性时变系统(Linear Time System)，亦或系统结构参数的不确定性，导致估计的状态值存在偏差，而这个偏差值由过程噪声来表征。对于状态观测方程而言，我们知道采集的信号往往是包含噪声及干扰信号的，亦或观测矩阵自身的观测偏差，从而导致了测得的实际信号（观测值）与真实值之间存在一定的偏差。
不考虑噪声时，状态空间方程为：（一般情况下我们无法对噪声进行建模）
x
^
k
−
=
A
x
^
k
−
1
+
B
u
k
−
1
(3)
\hat{\boldsymbol{x}}_{k}^{-}=A \hat{\boldsymbol{x}}_{k-1}+B u_{k-1}\tag3
x^k−=Ax^k−1+Buk−1(3)
事实上噪声是无法辟免的，因此公式
(
3
)
(3)
(3) 求得的计算结果为未处理的估计值（未加入过程噪声只能是估计值），我们叫做先验估计值, 记作
x
^
−
\hat{\boldsymbol{x}}^{-}
x^− 。
对于测量值
z
k
\boldsymbol{z}_{k}
zk, 不考虑噪声时：
z
k
=
H
x
k
(4)
\boldsymbol{z}_{k}=H \boldsymbol{x}_{k}\tag4
zk=Hxk(4)
根据公式 ( 4 ) 可求得：
x
^
k
m
e
a
=
H
−
z
k
⟶
H
−
为
矩
阵
的
逆
(5)
\hat{\boldsymbol{x}}_{k m e a}=H^{-} \boldsymbol{z}_{k}\tag5 \longrightarrow H^{-}为矩阵的逆
x^kmea=H−zk⟶H−为矩阵的逆(5)
因为公式 ( 3 ) ( 5 ) 均未考虑噪声，因此
x
^
k
−
,
x
^
k
m
e
a
\hat{\boldsymbol{x}}_{k}^{-}, \hat{x}_{k m e a}
x^k−,x^kmea 都是不准确的，都是缺少噪声的估计值。
前面博客已经提到：
x
^
k
=
x
^
k
−
1
+
G
(
z
k
−
x
^
k
−
1
)
(6)
\hat{x}_{k}=\hat{x}_{k-1}+G\left(z_{k}-\hat{x}_{k-1}\right)\tag6
x^k=x^k−1+G(zk−x^k−1)(6)
在本例中
z
k
=
x
^
k
m
e
a
,
x
^
k
−
1
=
x
^
k
−
z_{k}=\hat{x}_{k m e a}, \hat{x}_{k-1}=\hat{x}_{k}^{-}
zk=x^kmea,x^k−1=x^k−, 将式
(
5
)
(5)
(5) 代入式
(
6
)
(6)
(6) 得：
x
^
k
=
x
^
k
−
+
G
(
H
−
z
k
−
x
^
k
−
)
(7)
\hat{x}_{k}=\hat{x}_{k}^{-}+G\left(H^{-} z_{k}-\hat{x}_{k}^{-}\right)\tag7
x^k=x^k−+G(H−zk−x^k−)(7)
在教科书上一般表示
G
=
K
k
H
G=K_{k} H
G=KkH, 代入式
(
7
)
(7)
(7) 中得后验估计：
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
H
x
^
k
−
)
(8)
\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right)\tag8
x^k=x^k−+Kk(zk−Hx^k−)(8)
其中,
K
k
∈
[
0
,
H
−
]
K_{k} \in\left[0, H^{-}\right]
Kk∈[0,H−], 当
K
k
=
0
,
x
^
k
=
x
^
k
−
（
算
出
来
的
结
果
）
;
K_{k}=0, \hat{x}_{k}=\hat{x}_{k}^{-}（算出来的结果） ;
Kk=0,x^k=x^k−（算出来的结果）; 当
K
k
=
H
−
,
x
^
k
=
H
−
z
k
0
K_{k}=H^{-}, \hat{x}_{k}=H^{-} z_{k_{0}}
Kk=H−,x^k=H−zk0（测出来的结果）
我们的目标是寻找
K
k
K_{k}
Kk 使得
x
^
k
→
x
k
,
x
k
\hat{x}_{k} \rightarrow x_{k}, x_{k}
x^k→xk,xk 为真实值，即求与真实值最接近的估计值。真实情况下噪声是无法避免的，所以我们只能使
x
^
k
→
x
k
,
x
k
\hat{x}_{k} \rightarrow x_{k}, x_{k}
x^k→xk,xk 和
x
^
k
\hat{x}_{k}
x^k 之间存在误差，假设误差为
e
k
=
x
k
−
x
^
k
(9)
e_{k}=x_{k}-\hat{x}_{k}\tag9
ek=xk−x^k(9)
误差服从正态分布，即
p
(
e
k
)
∼
(
0
,
P
)
,
P
p\left(e_{k}\right) \sim(0, P), P
p(ek)∼(0,P),P 为协方差矩阵 :
P
=
E
[
e
e
T
]
=
[
σ
e
1
2
σ
e
1
σ
e
2
σ
e
2
σ
e
1
σ
e
2
2
]
(10)
P=E\left[\begin{array}{ll} e & e^{T} \end{array}\right]=\left[\begin{array}{cc} \sigma_{e _1}^{2} & \sigma_{e_{1}} \sigma_{e_{2}} \\ \sigma_{e_{2}} \sigma_{e_{1}} & \sigma_{e_{2}}^{2}\tag{10} \end{array}\right]
P=E[eeT]=[σe12σe2σe1σe1σe2σe22](10)
根据方差的性质可知，方差越小，误差越小，估计值越接近真实值。我们希望选取合适的
K
k
K_k
Kk 使得方差最小，即使得协方差矩阵
P
P
P 的对角线之和——迹（Trace）的值最小。
tr
(
P
)
=
σ
e
1
2
+
σ
e
2
2
(11)
\operatorname{tr}(P)=\sigma_{e_{1}}^{2}+\sigma_{e_{2}}^{2}\tag{11}
tr(P)=σe12+σe22(11)
为了求解
K
k
K_{k}
Kk 使得
tr
(
P
)
\operatorname{tr}(P)
tr(P) 最小, 首先对
P
P
P 进行详细推导：
P
k
=
E
[
e
k
e
k
T
]
=
E
[
(
x
k
−
x
^
k
)
(
x
k
−
x
^
k
)
T
]
(12)
\begin{aligned} P_{k} &=E\left[\begin{array}{ll} e_k & e^{T}_k \end{array}\right] \\ &=E\left[\left(x_{k}-\hat{x}_{k}\right)\left(x_{k}-\hat{x}_{k}\right)^{T}\right]\tag{12} \end{aligned}
Pk=E[ekekT]=E[(xk−x^k)(xk−x^k)T](12)
在这里，我们应该先求出误差
x
k
−
x
^
k
x_{k}-\hat{x}_{k}
xk−x^k 的值, 将公式
(
8
)
(8)
(8) 代入公式
(
9
)
(9)
(9) 得
:
:
:
x
k
−
x
^
k
=
x
k
−
(
x
^
k
−
+
K
k
(
z
k
−
H
x
^
k
−
)
)
=
x
k
−
x
^
k
−
−
K
k
z
k
+
K
k
H
x
^
k
−
(13)
\begin{aligned} x_{k}-\hat{x}_{k} &=x_{k}-\left(\hat{x}_{k}^{-}+K_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right)\right) \\ &=x_{k}-\hat{x}_{k}^{-}-K_{k} z_{k}+K_{k} H \hat{x}_{k}^{-}\tag{13} \end{aligned}
xk−x^k=xk−(x^k−+Kk(zk−Hx^k−))=xk−x^k−−Kkzk+KkHx^k−(13)
将
z
k
=
H
x
k
+
v
k
z_{k}=H x_{k}+v_{k}
zk=Hxk+vk 代入上式得：
x
k
−
x
^
k
=
x
k
−
x
^
k
−
−
K
k
H
x
k
−
K
k
v
k
+
K
k
H
x
^
k
=
(
x
k
−
x
^
k
−
)
−
K
k
H
(
x
k
−
x
^
k
−
)
−
K
k
v
k
=
(
I
−
K
k
H
)
(
x
k
−
x
^
k
−
)
−
K
k
v
k
(14)
\begin{aligned} x_{k}-\hat{x}_{k} &=x_{k}-\hat{x}_{k}^{-}-K_{k} H x_{k}-K_{k} v_{k}+K_{k} H \hat{x}_{k} \\ &=\left(x_{k}-\hat{x}_{k}^{-}\right)-K_{k} H\left(x_{k}-\hat{x}_{k}^{-}\right)-K_{k} v_{k} \\ &=\left(I-K_{k} H\right)\left(x_{k}-\hat{x}_{k}^{-}\right)-K_{k} v_{k}\tag{14} \end{aligned}
xk−x^k=xk−x^k−−KkHxk−Kkvk+KkHx^k=(xk−x^k−)−KkH(xk−x^k−)−Kkvk=(I−KkH)(xk−x^k−)−Kkvk(14)
其中，把
x
k
−
x
^
k
−
x_{k}-\hat{x}_{k}^{-}
xk−x^k− 叫做先验误差，记作
e
k
−
e_{k}^{-}
ek−, 则上式为 :
x
k
−
x
^
k
=
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
(15)
x_{k}-\hat{x}_{k}=\left(I-K_{k} H\right) e_{k}^{-}-K_{k} v_{k}\tag{15}
xk−x^k=(I−KkH)ek−−Kkvk(15)
将式
(
15
)
(15)
(15) 代入式
(
12
)
(12)
(12) 中得：
P
k
=
E
[
e
e
T
]
=
E
[
(
x
k
−
x
^
k
)
(
x
k
−
x
^
k
)
T
]
=
E
[
[
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
]
[
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
]
T
]
=
E
[
(
I
−
K
k
H
)
e
k
−
e
k
−
T
(
I
−
K
k
H
)
T
−
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
−
K
k
v
k
e
k
−
T
(
I
−
K
k
H
)
T
+
K
k
v
k
v
k
T
K
k
T
]
=
E
[
(
I
−
K
k
H
)
e
k
−
e
k
−
T
(
I
−
K
k
H
)
T
]
−
E
[
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
]
−
E
[
K
k
v
k
e
k
−
T
(
I
−
K
k
H
)
T
]
+
E
[
K
k
v
k
v
k
T
K
k
T
]
(16)
\begin{gathered} P_{k}=E\left[\begin{array}{cc} e & e^{T} \end{array}\right] \\ =E\left[\left(x_{k}-\hat{x}_{k}\right)\left(x_{k}-\hat{x}_{k}\right)^{T}\right] \\ =E\left[\left[\left(I-K_{k} H\right) e_{k}^{-}-K_{k} v_{k}\right]\left[\left(I-K_{k} H\right) e_{k}^{-}-K_{k} v_{k}\right]^{T}\right] \\ =E\left[\left(I-K_{k} H\right) e_{k}^{-} e_{k}^{-T}\left(I-K_{k} H\right)^{T}-\left(I-K_{k} H\right) e_{k}^{-} v_{k}^{T} K_{k}^{T}-K_{k} v_{k} e_{k}^{-T}\left(I-K_{k} H\right)^{T}+K_{k} v_{k} v_{k}^{T} K_{k}^{T}\right]\\ =E\left[\left(I-K_{k} H\right) e_{k}^{-} e_{k}^{-T}\left(I-K_{k} H\right)^{T}\right]-E\left[\left(I-K_{k} H\right) e_{k}^{-} v_{k}^{T} K_{k}^{T}\right] \\ -E\left[K_{k} v_{k} e_{k}^{-T}\left(I-K_{k} H\right)^{T}\right]+E\left[K_{k} v_{k} v_{k}^{T} K_{k}^{T}\right]\tag{16} \end{gathered}
Pk=E[eeT]=E[(xk−x^k)(xk−x^k)T]=E[[(I−KkH)ek−−Kkvk][(I−KkH)ek−−Kkvk]T]=E[(I−KkH)ek−ek−T(I−KkH)T−(I−KkH)ek−vkTKkT−Kkvkek−T(I−KkH)T+KkvkvkTKkT]=E[(I−KkH)ek−ek−T(I−KkH)T]−E[(I−KkH)ek−vkTKkT]−E[Kkvkek−T(I−KkH)T]+E[KkvkvkTKkT](16)
因为
K
k
,
I
−
K
k
H
K_{k}, I-K_{k} H
Kk,I−KkH 为常数, 所以有：
E
[
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
]
=
(
I
−
K
k
H
)
E
[
e
k
−
v
k
T
]
K
k
T
E
[
K
k
v
k
e
k
−
T
(
I
−
K
k
H
)
T
]
=
K
k
E
[
v
k
e
k
−
T
]
(
I
−
K
k
H
)
T
(17)
\begin{aligned} E\left[\left(I-K_{k} H\right) e_{k}^{-} v_{k}^{T} K_{k}^{T}\right] &=\left(I-K_{k} H\right) E\left[e_{k}^{-} v_{k}^{T}\right] K_{k}^{T} \\ E\left[K_{k} v_{k} e_{k}^{-T}\left(I-K_{k} H\right)^{T}\right] &=K_{k} E\left[v_{k} e_{k}^{-T}\right]\left(I-K_{k} H\right)^{T}\tag{17} \end{aligned}
E[(I−KkH)ek−vkTKkT]E[Kkvkek−T(I−KkH)T]=(I−KkH)E[ek−vkT]KkT=KkE[vkek−T](I−KkH)T(17)
e
k
−
e_{k}^{-}
ek− 为先验误差，
v
k
v_{k}
vk 为测量误差，两者相互独立，所以:
E
[
e
k
−
v
k
T
]
=
0
E
[
v
k
e
k
−
T
]
=
0
\begin{aligned} &E\left[e_{k}^{-} v_{k}^{T}\right]=0 \\ &E\left[v_{k} e_{k}^{-T}\right]=0 \end{aligned}
E[ek−vkT]=0E[vkek−T]=0
代入式 (17)，有：
P
k
=
E
[
(
I
−
K
k
H
)
e
k
−
e
k
−
T
(
I
−
K
k
H
)
T
]
−
0
−
0
+
E
[
K
k
v
k
v
k
T
K
k
T
]
=
(
I
−
K
k
H
)
E
[
e
k
−
e
k
−
T
]
(
I
−
K
k
H
)
T
+
K
k
E
[
v
k
v
k
T
]
K
k
T
(18)
\begin{aligned} P_{k} &=E\left[\left(I-K_{k} H\right) e_{k}^{-} e_{k}^{-T}\left(I-K_{k} H\right)^{T}\right]-0-0+E\left[K_{k} v_{k} v_{k}^{T} K_{k}^{T}\right] \\ &=\left(I-K_{k} H\right) E\left[e_{k}^{-} e_{k}^{-T}\right]\left(I-K_{k} H\right)^{T}+K_{k} E\left[v_{k} v_{k}^{T}\right] K_{k}^{T}\tag{18} \end{aligned}
Pk=E[(I−KkH)ek−ek−T(I−KkH)T]−0−0+E[KkvkvkTKkT]=(I−KkH)E[ek−ek−T](I−KkH)T+KkE[vkvkT]KkT(18)
式中,
E
[
e
k
−
e
k
−
T
]
E\left[e_{k}^{-} e_{k}^{-T}\right]
E[ek−ek−T] 为
P
k
P_{k}
Pk 的先验部分, 记作
P
k
−
P_{k}^{-}
Pk−, 则：
P
k
=
(
I
−
K
k
H
)
P
k
−
(
I
−
K
k
H
)
T
+
K
k
E
[
v
k
v
k
T
]
K
k
T
=
(
P
k
−
−
K
k
H
P
k
−
)
(
I
−
H
T
K
k
T
)
+
K
k
R
K
k
T
=
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
K
k
H
P
k
−
H
T
K
k
T
+
K
k
R
K
k
T
(19)
\begin{aligned} P_{k} &=\left(I-K_{k} H\right) P_{k}^{-}\left(I-K_{k} H\right)^{T}+K_{k} E\left[v_{k} v_{k}^{T}\right] K_{k}^{T}\\ &=\left(P_{k}^{-}-K_{k} H P_{k}^{-}\right)\left(I-H^{T} K_{k}^{T}\right)+K_{k} R K_{k}^{T}\\ &=P_{k}^{-}-K_{k} H P_{k}^{-}-P_{k}^{-} H^{T} K_{k}^{T}+K_{k} H P_{k}^{-} H^{T} K_{k}^{T}+K_{k} R K_{k}^{T}\tag{19} \end{aligned}
Pk=(I−KkH)Pk−(I−KkH)T+KkE[vkvkT]KkT=(Pk−−KkHPk−)(I−HTKkT)+KkRKkT=Pk−−KkHPk−−Pk−HTKkT+KkHPk−HTKkT+KkRKkT(19)
我们的目标是使
tr
(
P
k
)
\operatorname{tr}\left(P_{k}\right)
tr(Pk) 最小，下面我们对
P
k
P_{k}
Pk 的各部分分别进行讨论。
(
(
P
k
−
H
T
)
K
k
T
)
T
=
K
k
(
P
k
−
H
T
)
T
=
K
k
H
P
k
−
T
(20)
\left(\left(P_{k}^{-} H^{T}\right) K_{k}^{T}\right)^{T}=K_{k}\left(P_{k}^{-} H^{T}\right)^{T}=K_{k} H P_{k}^{-T}\tag{20}
((Pk−HT)KkT)T=Kk(Pk−HT)T=KkHPk−T(20)
转置后对角线上的值不变, 因此可得：
tr
(
K
k
H
P
k
−
)
=
tr
(
(
P
k
−
H
T
K
k
T
)
T
)
=
tr
(
P
k
−
H
T
K
k
T
)
(21)
\operatorname{tr}\left(K_{k} H P_{k}^{-}\right)=\operatorname{tr}\left(\left(P_{k}^{-} H^{T} K_{k}^{T}\right)^{T}\right)=\operatorname{tr}\left(P_{k}^{-} H^{T} K_{k}^{T}\right)\tag{21}
tr(KkHPk−)=tr((Pk−HTKkT)T)=tr(Pk−HTKkT)(21)
对
P
k
P_{k}
Pk 求迹等于对
P
k
P_{k}
Pk 的各部分分别求迹, 即 :
tr
(
P
k
)
=
tr
(
P
k
−
)
−
2
tr
(
K
k
H
P
k
−
)
+
tr
(
K
k
H
P
k
−
H
T
K
k
T
)
+
tr
(
K
k
R
K
k
T
)
(22)
\operatorname{tr}\left(P_{k}\right)=\operatorname{tr}\left(P_{k}^{-}\right)-2 \operatorname{tr}\left(K_{k} H P_{k}^{-}\right)+\operatorname{tr}\left(K_{k} H P_{k}^{-} H^{T} K_{k}^{T}\right)+\operatorname{tr}\left(K_{k} R K_{k}^{T}\right)\tag{22}
tr(Pk)=tr(Pk−)−2tr(KkHPk−)+tr(KkHPk−HTKkT)+tr(KkRKkT)(22)
再次虽调我们的目的是求得
K
k
K_{k}
Kk 使得
tr
(
P
k
)
\operatorname{tr}\left(P_{k}\right)
tr(Pk) 最小, 所以将
tr
(
P
k
)
\operatorname{tr}\left(P_{k}\right)
tr(Pk) 对
K
k
K_{k}
Kk 求导, 且导数为 0 时，
tr
(
P
k
)
\operatorname{tr}\left(P_{k}\right)
tr(Pk) 求得极小值。 已知
d
t
r
(
A
B
)
d
A
=
B
T
,
d
t
r
(
A
B
A
T
)
d
A
=
A
（
B
+
B
T
）
=
2
A
B
(
\frac{dt r(A B)}{d A}=B^{T}, \frac{d t r\left(A B A^{T}\right)}{d A}=A（B+B^T）=2 A B(
dAdtr(AB)=BT,dAdtr(ABAT)=A（B+BT）=2AB( 推导过程省略
)
)
), 且
tr
(
P
k
−
)
\operatorname{tr}\left(P_{k}^{-}\right)
tr(Pk−) 与
K
k
K_{k}
Kk无关，有：
d
tr
(
P
k
)
d
K
k
=
0
−
2
(
H
P
k
−
)
T
+
2
K
k
H
P
k
−
H
T
+
2
K
k
R
=
0
⇒
−
P
k
−
T
H
T
+
K
k
H
P
k
−
H
T
+
K
k
R
=
0
⇒
−
P
k
−
T
H
T
+
K
k
(
H
P
k
−
H
T
+
R
)
=
0
⟶
P
k
−
T
=
P
k
−
⇒
K
k
(
H
P
k
−
H
T
+
R
)
=
P
k
−
T
H
T
⇒
K
k
=
P
k
−
T
H
T
H
P
k
−
H
T
+
R
(23)
\begin{aligned} \frac{d \operatorname{tr}\left(P_{k}\right)}{d K_{k}} &=0-2\left(H P_{k}^{-}\right)^{T}+2 K_{k} H P_{k}^{-} H^{T}+2 K_{k} R=0 \\ & \Rightarrow-P_{k}^{-T} H^{T}+K_{k} H P_{k}^{-} H^{T}+K_{k} R=0 \\ & \Rightarrow-P_{k}^{-T} H^{T}+K_{k}\left(H P_{k}^{-} H^{T}+R\right)=0 \longrightarrow P_{k}^{-T}=P_{k}^{-}\\ & \Rightarrow K_{k}\left(H P_{k}^{-} H^{T}+R\right)=P_{k}^{-T} H^{T} \\ & \Rightarrow K_{k}=\frac{P_{k}^{-T} H^{T}}{H P_{k}^{-} H^{T}+R} \\ \tag{23} \end{aligned}
dKkdtr(Pk)=0−2(HPk−)T+2KkHPk−HT+2KkR=0⇒−Pk−THT+KkHPk−HT+KkR=0⇒−Pk−THT+Kk(HPk−HT+R)=0⟶Pk−T=Pk−⇒Kk(HPk−HT+R)=Pk−THT⇒Kk=HPk−HT+RPk−THT(23)
其中,
K
k
∈
[
0
,
H
−
]
K_{k} \in\left[0, H^{-}\right]
Kk∈[0,H−], 当
R
R
R 值很大（测量噪声很大）时，
K
k
=
0
,
x
^
k
=
x
^
k
−
K_{k}=0, \hat{x}_{k}=\hat{x}_{k}^{-}
Kk=0,x^k=x^k− （算出来的结果） ; 当
R
R
R 值很小（测量噪声很小）时，
K
k
=
H
−
,
x
^
k
=
H
−
z
k
0
K_{k}=H^{-}, \hat{x}_{k}=H^{-} z_{k_{0}}
Kk=H−,x^k=H−zk0（测出来的结果）。
三、Kalman滤波的几何解释
Kalman滤波公式可以用几何方式进行形象化的描述，这有助于增强对Kalman滤波含义的直观理解。
如下图所示，已知上一时刻的状态估计
x
^
k
\hat{x}_{k}
x^k和当前时刻的量测
z
k
z_k
zk，若两者之间不共线,则可以共同确定一个平面，记为
o
α
β
o\alpha \beta
oαβ ，在此平面基础上建立
o
α
β
γ
o\alpha \beta\gamma
oαβγ 空间直角坐标系。
x
^
k
\hat{x}_{k}
x^k和
z
k
z_k
zk经伸缩(线性变换)之后分别变成
G
k
x
^
k
G_k\hat{x}_{k}
Gkx^k和
K
k
z
k
K_kz_k
Kkzk再合成为当前时刻的状态估计义，所以
x
^
k
\hat{x}_{k}
x^k是
x
^
k
−
1
\hat{x}_{k-1}
x^k−1和
z
k
z_k
zk的线性组合，
x
^
k
\hat{x}_{k}
x^k也必定在
o
α
β
o\alpha \beta
oαβ平面内。但是,当前时刻的状态真值
x
k
x_{k}
xk不一定恰好在
o
α
β
o\alpha \beta
oαβ平面内，只有当估计
x
^
k
\hat{x}_{k}
x^k等于真值
x
k
x_{k}
xk在
o
α
β
o\alpha \beta
oαβ平面上的正交投影时,估计误差
e
k
=
x
k
−
x
^
k
e_k=x_{k}-\hat{x}_{k}
ek=xk−x^k才会最小。实现这一过程的关键在于确定系数矩阵
K
k
K_k
Kk。根据以上描述容易看出,估计误差
x
^
k
\hat{x}_{k}
x^k必定同时垂直(不相关)于
x
^
k
−
1
\hat{x}_{k-1}
x^k−1和
z
k
z_k
zk，使用统计公式表示如下:
E
[
e
k
z
k
T
]
=
0
E
[
e
k
x
k
−
1
T
]
=
0
\begin{aligned} E\left [ e_k z_k^\mathsf{T} \right ] =0\\ E\left [ e_k x_{k-1}^\mathsf{T} \right ] =0 \end{aligned}
E[ekzkT]=0E[ekxk−1T]=0
Kalman滤波的几何图示
实际上，上述几何方式描述的正是Kalman滤波(线性最小方差无偏估计)的正交投影性质，即状态 x k x_{k} xk的最优估计 x ^ k \hat{x}_{k} x^k是 x k x_{k} xk在由 x ^ k − 1 \hat{x}_{k-1} x^k−1和 Z k Z_k Zk构成的线性空间上的正交投影。
扫一扫
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
