文章详细介绍了卡尔曼滤波算法在离散状态空间中的应用，包括系统状态的表示、噪声的处理以及卡尔曼增益矩阵的计算。通过对先验估计和测量估计的组合，找到最优的系统状态估计值，同时展示了如何计算后验误差的协方差矩阵，以确保估计值接近真实值。
在离散状态空间表示下，加入噪声的系统可以有如下表示：
{
X
k
=
A
X
k
−
1
+
B
u
k
+
w
k
Z
k
=
H
X
k
+
v
k
\begin{cases} X_k=AX_{k-1}+Bu_k+w_k \\ Z_k=HX_k+v_k \end{cases}
{Xk=AXk−1+Buk+wkZk=HXk+vk
其中
w
k
∼
N
(
0
,
Q
)
w_k \sim \text{N}(0, Q)
wk∼N(0,Q)为过程噪声，
v
k
∼
N
(
0
,
R
)
v_k \sim \text{N}(0, R)
vk∼N(0,R)为测量噪声，
Q
、
R
Q、R
Q、R分别为
w
k
、
v
k
w_k、v_k
wk、vk的协方差矩阵
提醒：协方差矩阵是对称矩阵，其转置和自身相等
卡尔曼滤波是一种递归估计的思想，即我们假设在 k − 1 k-1 k−1时刻得到了系统状态 X k − 1 X_{k-1} Xk−1的最优估计值 X ^ k − 1 \hat{X}_{k-1} X^k−1，我们如何推导出 k k k时刻的系统状态 X k X_k Xk的最优估计值 X ^ k \hat{X}_{k} X^k
定义
X
^
k
−
1
\hat{X}_{k-1}
X^k−1为
k
−
1
k-1
k−1时刻的后验估计值（最优估计值），则我们可以得到
k
k
k时刻的两种系统状态的估计值
X
^
k
−
=
A
X
^
k
−
1
+
B
u
k
X
^
k
m
e
a
=
H
−
1
Z
k
\begin{aligned} \hat{X}_k^{-}&=A\hat{X}_{k-1}+Bu_k \\ \hat{X}_{kmea}&=H^{-1}Z_k \end{aligned}
X^k−X^kmea=AX^k−1+Buk=H−1Zk
其中
X
^
k
−
\hat{X}_k^{-}
X^k−称为
k
k
k时刻的先验估计，它忽略了系统的过程噪声，
X
^
k
m
e
a
\hat{X}_{kmea}
X^kmea称为
k
k
k时刻的测量估计，它忽略了系统的测量噪声
现在我们有了两个关于
X
k
X_k
Xk的估计值，我们需要将其组合起来获取最优的那个估计值，即选取合适的
G
k
G_k
Gk，使得
X
^
k
=
X
^
k
−
+
G
k
(
X
^
k
m
e
a
−
X
^
k
−
)
\hat{X}_k=\hat{X}_k^-+G_k(\hat{X}_{kmea}-\hat{X}_k^-)
X^k=X^k−+Gk(X^kmea−X^k−)
更趋近于
X
k
X_k
Xk
我们令
K
k
=
G
k
H
K_k=G_kH
Kk=GkH为卡尔曼增益矩阵，同时带入
X
^
k
m
e
a
=
H
−
1
Z
k
\hat{X}_{kmea}=H^{-1}Z_k
X^kmea=H−1Zk，有
X
^
k
=
X
^
k
−
+
K
k
(
Z
k
−
H
X
^
k
−
)
\hat{X}_k=\hat{X}_k^-+K_k(Z_k-H\hat{X}_k^-)
X^k=X^k−+Kk(Zk−HX^k−)
定义后验误差
e
^
k
=
X
k
−
X
^
k
\hat{e}_k=X_k-\hat{X}_k
e^k=Xk−X^k
显然有
e
^
k
∼
N
(
0
,
P
k
^
)
\hat{e}_k \sim \text{N}(0, \hat{P_k})
e^k∼N(0,Pk^)，
P
k
^
\hat{P_k}
Pk^为后验误差的协方差矩阵，其形式如下
P
k
^
=
[
σ
e
1
2
σ
e
1
e
2
⋯
σ
e
1
e
n
σ
e
2
e
1
σ
e
2
2
⋯
σ
e
2
e
n
⋮
⋮
⋱
⋮
σ
e
n
e
1
σ
e
n
e
2
⋯
σ
e
n
2
]
\hat{P_k}=\begin{bmatrix} \sigma e_1^2 & \sigma e_1e_2 & \cdots & \sigma e_1e_n \\ \sigma e_2e_1 & \sigma e_2^2 & \cdots & \sigma e_2e_n \\ \vdots & \vdots & \ddots & \vdots\\ \sigma e_ne_1 & \sigma e_ne_2 & \cdots &\sigma e_n^2\end{bmatrix}
Pk^=
σe12σe2e1⋮σene1σe1e2σe22⋮σene2⋯⋯⋱⋯σe1enσe2en⋮σen2
由方差的定义我们可以知道，协方差矩阵对角线之外的协方差项仅表示误差分量的变化方向趋势，只有对角线上的方差项表示误差关于某一个值的变化程度
所以若我们想要 X ^ k \hat{X}_k X^k尽可能趋近于 X k X_k Xk，即有后验误差的协方差矩阵 P k ^ \hat{P_k} Pk^对角线上的方差项之和尽可能小，即 tr ( P k ^ ) \text{tr}(\hat{P_k}) tr(Pk^)最小
由协方差矩阵的计算方法我们可以得到（下式中的
E
(
)
\text{E}()
E()表示期望）
P
k
^
=
E
(
e
^
k
e
^
k
T
)
=
E
(
(
X
k
−
X
^
k
)
(
X
k
−
X
^
k
)
T
)
=
E
(
(
X
k
−
X
^
k
−
−
K
k
(
Z
k
−
H
X
^
k
−
)
)
(
X
k
−
X
^
k
−
−
K
k
(
Z
k
−
H
X
^
k
−
)
)
T
)
=
E
(
(
X
k
−
X
^
k
−
−
K
k
(
H
X
k
+
v
k
−
H
X
^
k
−
)
(
X
k
−
X
^
k
−
−
K
k
(
H
X
k
+
v
k
−
H
X
^
k
−
)
T
)
=
E
(
(
(
I
−
K
k
H
)
(
X
k
−
X
^
k
−
)
−
K
k
v
k
)
(
(
I
−
K
k
H
)
(
X
k
−
X
^
k
−
)
−
K
k
v
k
)
T
)
\begin{aligned} \hat{P_k} &=\text{E}(\hat{e}_k\hat{e}_k^T) \\ &=\text{E}((X_k-\hat{X}_k)(X_k-\hat{X}_k)^T) \\ &=\text{E}((X_k-\hat{X}_k^--K_k(Z_k-H\hat{X}_k^-))(X_k-\hat{X}_k^--K_k(Z_k-H\hat{X}_k^-))^T) \\ &=\text{E}((X_k-\hat{X}_k^--K_k(HX_k+v_k-H\hat{X}_k^-)(X_k-\hat{X}_k^--K_k(HX_k+v_k-H\hat{X}_k^-)^T) \\ &=\text{E}(((I-K_kH)(X_k-\hat{X}_k^{-})-K_kv_k)((I-K_kH)(X_k-\hat{X}_k^{-})-K_kv_k)^T) \end{aligned}
Pk^=E(e^ke^kT)=E((Xk−X^k)(Xk−X^k)T)=E((Xk−X^k−−Kk(Zk−HX^k−))(Xk−X^k−−Kk(Zk−HX^k−))T)=E((Xk−X^k−−Kk(HXk+vk−HX^k−)(Xk−X^k−−Kk(HXk+vk−HX^k−)T)=E(((I−KkH)(Xk−X^k−)−Kkvk)((I−KkH)(Xk−X^k−)−Kkvk)T)
定义先验误差
e
^
k
−
=
X
k
−
X
^
k
−
\hat{e}_k^{-}=X_k-\hat{X}_k^-
e^k−=Xk−X^k−
有
P
k
^
=
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
−
−
K
k
v
k
)
(
(
e
^
k
−
)
T
(
I
−
H
T
K
k
T
)
−
v
k
T
K
k
T
)
)
=
E
(
(
I
−
K
k
H
)
e
^
k
−
(
e
^
k
−
)
T
(
I
−
H
T
K
k
T
)
−
K
k
v
k
(
e
^
k
−
)
T
(
I
−
H
T
K
k
T
)
−
(
I
−
K
k
H
)
e
^
k
−
v
k
T
K
k
T
+
K
k
v
k
v
k
T
K
k
T
)
=
(
I
−
K
k
H
)
E
(
e
^
k
−
(
e
^
k
−
)
T
)
(
I
−
H
T
K
k
T
)
−
K
k
E
(
v
k
(
e
^
k
−
)
T
)
(
I
−
H
T
K
k
T
)
−
(
I
−
K
k
H
)
E
(
e
^
k
−
v
k
T
)
K
k
T
+
K
k
E
(
v
k
v
k
T
)
K
k
T
\begin{aligned} \hat{P_k} &=\text{E}(((I-K_kH)\hat{e}_k^{-}-K_kv_k)((I-K_kH)\hat{e}_k^{-}-K_kv_k)^T) \\ &=\text{E}(((I-K_kH)\hat{e}_k^{-}-K_kv_k)((\hat{e}_k^{-})^T(I-H^TK_k^T)-v_k^TK_k^T)) \\ &=\text{E}((I-K_kH)\hat{e}_k^{-}(\hat{e}_k^{-})^T(I-H^TK_k^T) - K_kv_k(\hat{e}_k^{-})^T(I-H^TK_k^T) - (I-K_kH)\hat{e}_k^{-}v_k^TK_k^T + K_kv_kv_k^TK_k^T) \\ &=(I-K_kH)\text{E}(\hat{e}_k^{-}(\hat{e}_k^{-})^T)(I-H^TK_k^T)-K_k\text{E}(v_k(\hat{e}_k^{-})^T)(I-H^TK_k^T)-(I-K_kH)\text{E}(\hat{e}_k^{-}v_k^T)K_k^T+K_k\text{E}(v_kv_k^T)K_k^T \end{aligned}
Pk^=E(((I−KkH)e^k−−Kkvk)((I−KkH)e^k−−Kkvk)T)=E(((I−KkH)e^k−−Kkvk)((e^k−)T(I−HTKkT)−vkTKkT))=E((I−KkH)e^k−(e^k−)T(I−HTKkT)−Kkvk(e^k−)T(I−HTKkT)−(I−KkH)e^k−vkTKkT+KkvkvkTKkT)=(I−KkH)E(e^k−(e^k−)T)(I−HTKkT)−KkE(vk(e^k−)T)(I−HTKkT)−(I−KkH)E(e^k−vkT)KkT+KkE(vkvkT)KkT
在前面我们定义了后验误差的协方差矩阵
P
k
^
=
E
(
e
^
k
e
^
k
T
)
\hat{P_k} =\text{E}(\hat{e}_k\hat{e}_k^T)
Pk^=E(e^ke^kT)，这里我们定义先验误差的协方差矩阵
P
k
−
^
=
E
(
e
^
k
−
(
e
^
k
−
)
T
)
\hat{P^{-}_k} =\text{E}(\hat{e}_k^{-}(\hat{e}_k^{-})^T)
Pk−^=E(e^k−(e^k−)T)
同时我们知道
v
k
、
(
e
^
k
−
)
T
v_k、(\hat{e}_k^{-})^T
vk、(e^k−)T互相独立，
e
^
k
−
、
v
k
T
\hat{e}_k^{-}、v_k^T
e^k−、vkT互相独立，又有
E
(
v
k
)
=
E
(
v
k
T
)
=
0
\text{E}(v_k)=\text{E}(v_k^T)=0
E(vk)=E(vkT)=0所以有
E
(
v
k
(
e
^
k
−
)
T
)
=
E
(
v
k
)
E
(
(
e
^
k
−
)
T
)
=
0
E
(
e
^
k
−
v
k
T
)
=
E
(
e
^
k
−
)
E
(
v
k
T
)
=
0
\begin{aligned} \text{E}(v_k(\hat{e}_k^{-})^T)&=\text{E}(v_k)\text{E}((\hat{e}_k^{-})^T)=0 \\ \text{E}(\hat{e}_k^{-}v_k^T)&=\text{E}(\hat{e}_k^{-})\text{E}(v_k^T)=0 \end{aligned}
E(vk(e^k−)T)E(e^k−vkT)=E(vk)E((e^k−)T)=0=E(e^k−)E(vkT)=0
此外，有
E
(
v
k
v
k
T
)
=
R
\text{E}(v_kv_k^T)=R
E(vkvkT)=R
则有
P
k
^
=
(
I
−
K
k
H
)
P
k
−
^
(
I
−
H
T
K
k
T
)
+
K
k
R
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
\begin{aligned} \hat{P_k} &=(I-K_kH)\hat{P_k^{-}}(I-H^TK_k^T)+K_kRK_k^T \\ &=(\hat{P_k^{-}}-K_kH\hat{P_k^{-}})(I-H^TK_k^T)+K_kRK_k^T \\ &=\hat{P_k^{-}}-K_kH\hat{P_k^{-}}-\hat{P_k^{-}}H^TK_k^T+K_kH\hat{P_k^{-}}H^TK_k^T+K_kRK_k^T \\ \end{aligned}
Pk^=(I−KkH)Pk−^(I−HTKkT)+KkRKkT=(Pk−^−KkHPk−^)(I−HTKkT)+KkRKkT=Pk−^−KkHPk−^−Pk−^HTKkT+KkHPk−^HTKkT+KkRKkT
我们计算
tr
(
P
k
^
)
\text{tr}(\hat{P_k})
tr(Pk^)
tr
(
P
k
^
)
=
tr
(
P
k
−
^
)
−
tr
(
K
k
H
P
k
−
^
)
−
tr
(
P
k
−
^
H
T
K
k
T
)
+
tr
(
K
k
H
P
k
−
^
H
T
K
k
T
)
+
tr
(
K
k
R
K
k
T
)
\begin{align} \text{tr}(\hat{P_k}) &=\text{tr}(\hat{P_k^{-}})-\text{tr}(K_kH\hat{P_k^{-}})-\text{tr}(\hat{P_k^{-}}H^TK_k^T)+\text{tr}(K_kH\hat{P_k^{-}}H^TK_k^T)+\text{tr}(K_kRK_k^T) \end{align}
tr(Pk^)=tr(Pk−^)−tr(KkHPk−^)−tr(Pk−^HTKkT)+tr(KkHPk−^HTKkT)+tr(KkRKkT)
我们考虑
K
k
H
P
k
−
^
K_kH\hat{P_k^{-}}
KkHPk−^和
P
k
−
^
H
T
K
k
T
\hat{P_k^{-}}H^TK_k^T
Pk−^HTKkT两个矩阵
(
P
k
−
^
H
T
K
k
T
)
T
=
K
k
H
(
P
k
−
^
)
T
=
K
k
H
P
k
−
^
\begin{align} (\hat{P_k^{-}}H^TK_k^T)^{T}=K_kH(\hat{P_k^{-}})^T=K_kH\hat{P_k^{-}} \end{align}
(Pk−^HTKkT)T=KkH(Pk−^)T=KkHPk−^
所以上面两个矩阵互为转置，那么其秩就是一样的，所以有
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
)
\text{tr}(\hat{P_k}) =\text{tr}(\hat{P_k^{-}})-2\text{tr}(K_kH\hat{P_k^{-}})+\text{tr}(K_kH\hat{P_k^{-}}H^TK_k^T)+\text{tr}(K_kRK_k^T)
tr(Pk^)=tr(Pk−^)−2tr(KkHPk−^)+tr(KkHPk−^HTKkT)+tr(KkRKkT)
要使
tr
(
P
k
^
)
\text{tr}(\hat{P_k})
tr(Pk^)最小，我们让
tr
(
P
k
^
)
\text{tr}(\hat{P_k})
tr(Pk^)对
K
k
K_k
Kk求导
dtr
(
P
k
^
)
d
K
k
=
d
(
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
)
)
d
K
k
=
dtr
(
P
k
−
^
)
d
K
k
−
2
dtr
(
K
k
H
P
k
−
^
)
d
K
k
+
dtr
(
K
k
H
P
k
−
^
H
T
K
k
T
)
d
K
k
+
dtr
(
K
k
R
K
k
T
)
d
K
k
\begin{aligned} \frac{\text{d}\text{tr}(\hat{P_k})}{\text{d}K_k} &=\frac{\text{d}(\text{tr}(\hat{P_k^{-}})-2\text{tr}(K_kH\hat{P_k^{-}})+\text{tr}(K_kH\hat{P_k^{-}}H^TK_k^T)+\text{tr}(K_kRK_k^T))}{\text{d}K_k} \\ &=\frac{\text{d}\text{tr}(\hat{P_k^{-}})}{\text{d}K_k}-2\frac{\text{d}\text{tr}(K_kH\hat{P_k^{-}})}{\text{d}K_k}+\frac{\text{d}\text{tr}(K_kH\hat{P_k^{-}}H^TK_k^T)}{\text{d}K_k}+\frac{\text{d}\text{tr}(K_kRK_k^T)}{\text{d}K_k} \\ \end{aligned}
dKkdtr(Pk^)=dKkd(tr(Pk−^)−2tr(KkHPk−^)+tr(KkHPk−^HTKkT)+tr(KkRKkT))=dKkdtr(Pk−^)−2dKkdtr(KkHPk−^)+dKkdtr(KkHPk−^HTKkT)+dKkdtr(KkRKkT)
现在讨论每一项
显然 tr ( P k − ^ ) \text{tr}(\hat{P_k^{-}}) tr(Pk−^)跟 K k K_k Kk没有任何关系，所以第一项为 0 0 0
对于后面几项，我们有以下关于矩阵的迹的求导公式
dtr
(
A
B
)
d
A
=
dtr
(
B
A
)
d
A
=
B
T
dtr
(
A
B
A
T
)
d
A
=
A
B
+
A
B
T
\begin{aligned} \frac{\text{dtr}(AB)}{\text{d}A}&=\frac{\text{dtr}(BA)}{\text{d}A}=B^T \\ \frac{\text{dtr}(ABA^T)}{\text{d}A}&=AB+AB^T \\ \end{aligned}
dAdtr(AB)dAdtr(ABAT)=dAdtr(BA)=BT=AB+ABT
故有
dtr
(
P
k
^
)
d
K
k
=
−
2
(
H
P
k
−
^
)
T
+
K
k
H
P
k
−
^
H
T
+
K
k
(
H
P
k
−
^
H
T
)
T
+
K
k
R
+
K
k
R
T
=
−
2
P
k
−
^
H
T
+
2
K
k
H
P
k
−
^
H
T
+
2
K
k
R
\begin{aligned} \frac{\text{d}\text{tr}(\hat{P_k})}{\text{d}K_k} &=-2(H\hat{P_k^{-}})^T+K_kH\hat{P_k^{-}}H^T+K_k(H\hat{P_k^{-}}H^T)^T+K_kR+K_kR^T \\ &=-2\hat{P_k^{-}}H^T+2K_kH\hat{P_k^{-}}H^T+2K_kR \end{aligned}
dKkdtr(Pk^)=−2(HPk−^)T+KkHPk−^HT+Kk(HPk−^HT)T+KkR+KkRT=−2Pk−^HT+2KkHPk−^HT+2KkR
我们令
dtr
(
P
k
^
)
d
K
k
=
0
\frac{\text{d}\text{tr}(\hat{P_k})}{\text{d}K_k}=0
dKkdtr(Pk^)=0
有
−
2
P
k
−
^
H
T
+
2
K
k
H
P
k
−
^
H
T
+
2
K
k
R
=
0
⇒
K
k
(
H
P
k
−
^
H
T
+
R
)
=
P
k
−
^
H
T
⇒
K
k
=
P
k
−
^
H
T
H
P
k
−
^
H
T
+
R
\begin{aligned} & &-2\hat{P_k^{-}}H^T+2K_kH\hat{P_k^{-}}H^T+2K_kR &=0 \\ &\Rightarrow & K_k(H\hat{P_k^{-}}H^T+R) &=\hat{P_k^{-}}H^T \\ &\Rightarrow & K_k&=\frac{\hat{P_k^{-}}H^T}{H\hat{P_k^{-}}H^T+R} \end{aligned}
⇒⇒−2Pk−^HT+2KkHPk−^HT+2KkRKk(HPk−^HT+R)Kk=0=Pk−^HT=HPk−^HT+RPk−^HT
至此，我们得到了卡尔曼增益矩阵的表示方法，这能让我们的后验估计最接近真实值
但卡尔曼增益矩阵中的 P k − ^ \hat{P_k^{-}} Pk−^项仍然未知，所以我们现在计算这一项
由定义知
P
k
−
^
=
E
(
e
^
k
−
(
e
^
k
−
)
T
)
=
E
(
(
X
k
−
X
^
k
−
)
(
X
k
−
X
^
k
−
)
T
)
=
E
(
(
A
X
k
−
1
+
B
u
k
+
w
k
−
A
X
^
k
−
1
+
B
u
k
)
(
A
X
k
−
1
+
B
u
k
+
w
k
−
A
X
^
k
−
1
+
B
u
k
)
T
)
=
E
(
(
A
e
^
k
−
1
+
w
k
)
(
A
e
^
k
−
1
+
w
k
)
T
)
=
E
(
(
A
e
^
k
−
1
+
w
k
)
(
w
k
T
+
e
^
k
−
1
T
A
T
)
)
=
E
(
A
e
^
k
−
1
w
k
T
+
A
e
^
k
−
1
e
^
k
−
1
T
A
T
+
w
k
w
k
T
+
w
k
e
^
k
−
1
T
A
T
)
=
A
E
(
e
^
k
−
1
w
k
T
)
+
A
E
(
e
^
k
−
1
e
^
k
−
1
T
)
A
T
+
E
(
w
k
w
k
T
)
+
E
(
w
k
e
^
k
−
1
T
)
A
T
\begin{aligned} \hat{P^{-}_k} &=\text{E}(\hat{e}_k^{-}(\hat{e}_k^{-})^T) \\ &=\text{E}((X_k-\hat{X}_k^-)(X_k-\hat{X}_k^-)^T) \\ &=\text{E}((AX_{k-1}+Bu_k+w_k-A\hat{X}_{k-1}+Bu_k)(AX_{k-1}+Bu_k+w_k-A\hat{X}_{k-1}+Bu_k)^T) \\ &=\text{E}((A\hat{e}_{k-1}+w_k)(A\hat{e}_{k-1}+w_k)^T) \\ &=\text{E}((A\hat{e}_{k-1}+w_k)(w_k^T+\hat{e}_{k-1}^TA^T)) \\ &=\text{E}(A\hat{e}_{k-1}w_k^T+A\hat{e}_{k-1}\hat{e}_{k-1}^TA^T+w_kw_k^T+w_k\hat{e}_{k-1}^TA^T) \\ &=A\text{E}(\hat{e}_{k-1}w_k^T)+A\text{E}(\hat{e}_{k-1}\hat{e}_{k-1}^T)A^T+\text{E}(w_kw_k^T)+\text{E}(w_k\hat{e}_{k-1}^T)A^T \\ \end{aligned}
Pk−^=E(e^k−(e^k−)T)=E((Xk−X^k−)(Xk−X^k−)T)=E((AXk−1+Buk+wk−AX^k−1+Buk)(AXk−1+Buk+wk−AX^k−1+Buk)T)=E((Ae^k−1+wk)(Ae^k−1+wk)T)=E((Ae^k−1+wk)(wkT+e^k−1TAT))=E(Ae^k−1wkT+Ae^k−1e^k−1TAT+wkwkT+wke^k−1TAT)=AE(e^k−1wkT)+AE(e^k−1e^k−1T)AT+E(wkwkT)+E(wke^k−1T)AT
我们同样是分开看每一项，显然
e
^
k
−
1
、
w
k
T
\hat{e}_{k-1}、w_k^T
e^k−1、wkT互相独立，
w
k
、
e
^
k
−
1
T
w_k、\hat{e}_{k-1}^T
wk、e^k−1T互相独立，又有
E
(
w
k
T
)
=
E
(
w
k
)
=
0
\text{E}(w_k^T)=\text{E}(w_k)=0
E(wkT)=E(wk)=0，所以有
E
(
e
^
k
−
1
w
k
T
)
=
E
(
e
^
k
−
1
)
E
(
w
k
T
)
=
0
E
(
w
k
e
^
k
−
1
T
)
=
E
(
w
k
)
E
(
e
^
k
−
1
T
)
=
0
\begin{aligned} \text{E}(\hat{e}_{k-1}w_k^T)&=\text{E}(\hat{e}_{k-1})\text{E}(w_k^T)=0 \\ \text{E}(w_k\hat{e}_{k-1}^T)&=\text{E}(w_k)\text{E}(\hat{e}_{k-1}^T)=0 \\ \end{aligned}
E(e^k−1wkT)E(wke^k−1T)=E(e^k−1)E(wkT)=0=E(wk)E(e^k−1T)=0
此外，有
E
(
w
k
w
k
T
)
=
Q
E
(
e
^
k
−
1
e
^
k
−
1
T
)
=
P
^
k
−
1
\text{E}(w_kw_k^T)=Q \\ \text{E}(\hat{e}_{k-1}\hat{e}_{k-1}^T)=\hat{P}_{k-1}
E(wkwkT)=QE(e^k−1e^k−1T)=P^k−1
则有
P
k
−
^
=
A
P
^
k
−
1
A
T
+
Q
\hat{P^{-}_k}=A\hat{P}_{k-1}A^T+Q
Pk−^=AP^k−1AT+Q
下面我们计算后验误差的协方差矩阵$\hat{P_k} $
P
k
^
=
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
=
(
I
−
K
k
H
)
P
k
−
^
−
(
P
k
−
^
H
T
−
K
k
H
P
k
−
^
H
T
−
K
k
R
)
K
k
T
=
(
I
−
K
k
H
)
P
k
−
^
−
(
P
k
−
^
H
T
−
K
k
(
H
P
k
−
^
H
T
+
R
)
)
K
k
T
=
(
I
−
K
k
H
)
P
k
−
^
−
(
P
k
−
^
H
T
−
P
k
−
^
H
T
)
K
k
T
=
(
I
−
K
k
H
)
P
k
−
^
\begin{aligned} \hat{P_k} &=\hat{P_k^{-}}-K_kH\hat{P_k^{-}}-\hat{P_k^{-}}H^TK_k^T+K_kH\hat{P_k^{-}}H^TK_k^T+K_kRK_k^T \\ &=(I-K_kH)\hat{P_k^{-}}-(\hat{P_k^{-}}H^T-K_kH\hat{P_k^{-}}H^T-K_kR)K_k^T \\ &=(I-K_kH)\hat{P_k^{-}}-(\hat{P_k^{-}}H^T-K_k(H\hat{P_k^{-}}H^T+R))K_k^T \\ &=(I-K_kH)\hat{P_k^{-}}-(\hat{P_k^{-}}H^T-\hat{P_k^{-}}H^T)K_k^T \\ &=(I-K_kH)\hat{P_k^{-}} \end{aligned}
Pk^=Pk−^−KkHPk−^−Pk−^HTKkT+KkHPk−^HTKkT+KkRKkT=(I−KkH)Pk−^−(Pk−^HT−KkHPk−^HT−KkR)KkT=(I−KkH)Pk−^−(Pk−^HT−Kk(HPk−^HT+R))KkT=(I−KkH)Pk−^−(Pk−^HT−Pk−^HT)KkT=(I−KkH)Pk−^
至此，我们得到了卡尔曼的五个公式，其使用顺序如下
初始状态 k = 0 k=0 k=0的情况下，我们有给定的初始值 X ^ 0 \hat{X}_{0} X^0与初始后验误差协方差矩阵 P 0 ^ \hat{P_0} P0^，这两个初始值矩阵一般为零，还有设定的过程噪声的协方差矩阵 Q Q Q，测量噪声的协方差矩阵 R R R，这两个矩阵需要我们给定一个初始值
当 k ⩾ 1 k \geqslant 1 k⩾1时，有
第一步，计算此时的先验估计
X
^
k
−
=
A
X
^
k
−
1
+
B
u
k
\hat{X}_k^{-}=A\hat{X}_{k-1}+Bu_k
X^k−=AX^k−1+Buk
第二步，计算此时先验误差的协方差矩阵
P
k
−
^
=
A
P
^
k
−
1
A
T
+
Q
\hat{P^{-}_k}=A\hat{P}_{k-1}A^T+Q
Pk−^=AP^k−1AT+Q
第三步，计算此时的卡尔曼增益
K
k
=
P
k
−
^
H
T
H
P
k
−
^
H
T
+
R
K_k=\frac{\hat{P_k^{-}}H^T}{H\hat{P_k^{-}}H^T+R}
Kk=HPk−^HT+RPk−^HT
第四步，计算此时的后验估计（此时刻系统状态的最优估计）
X
^
k
=
X
^
k
−
+
K
k
(
Z
k
−
H
X
^
k
−
)
\hat{X}_k=\hat{X}_k^-+K_k(Z_k-H\hat{X}_k^-)
X^k=X^k−+Kk(Zk−HX^k−)
第五步，计算此时后验误差的协方差矩阵
P
k
^
=
(
I
−
K
k
H
)
P
k
−
^
\hat{P_k}=(I-K_kH)\hat{P_k^{-}}
Pk^=(I−KkH)Pk−^
1611
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
