上一篇文章从概率密度函数的角度推导了卡尔曼滤波公式（卡尔曼滤波公式推导（1）），接下来从矩阵的最小二乘法的角度来推导。
在预测和更新阶段分别能得到两个近似状态向量真实值的值，记为 x ^ p r e d i c t = x ^ k − \hat x^{predict}=\hat x_k^{-} x^predict=x^k−和 x ^ m e a s u r e = H k − 1 z k \hat x^{measure}=H_k^{-1}z_k x^measure=Hk−1zk，通过加权平均（互补滤波）得到最终的后验估计值 x ^ k = x ^ p r e d i c t + G ( x ^ m e a s u r e − x ^ p r e d i c t ) \hat x_k=\hat x^{predict}+G(\hat x^{measure}-\hat x^{predict}) x^k=x^predict+G(x^measure−x^predict)，即 x ^ k = x ^ k − + G ( H k − 1 z k − x ^ k − ) \hat x_k=\hat x_k^{-}+G(H_k^{-1}z_k-\hat x_k^{-}) x^k=x^k−+G(Hk−1zk−x^k−)，为了避免矩阵求逆，令 G = K H k G=KH_k G=KHk，则 x ^ k = x ^ k − + K ( z k − H k x ^ k − ) \hat x_k=\hat x_k^{-}+K(z_k-H_k\hat x_k^{-}) x^k=x^k−+K(zk−Hkx^k−)。令后验估计值和真实值的误差为 e k = x k − x ^ k e_k=x_k-\hat x_k ek=xk−x^k，为了让估计值和真实值最接近，我们需要让误差最小。假设 e k e_k ek满足正态分布，即 e k ∼ N ( 0 , P k ) e_k\sim N(0, P_k) ek∼N(0,Pk)， P k = E ( e k e k T ) P_k=E(e_ke_k^{T}) Pk=E(ekekT)，最小二乘法是一种最优线性无偏估计算法，此处将问题转化为求矩阵 P k P_k Pk的迹 t r ( P k ) tr(P_k) tr(Pk)最小。
公式推导过程中需要用到一些知识点辅助，下面我们先来了解几个公式（每个公式标注了序号，后面推导会用到）。
- 一个 n ∗ n n*n n∗n矩阵 A n A_n An主对角线上所有元素的和称为矩阵的迹，记为 t r ( A n ) tr(A_n) tr(An)，并且有 t r ( A T ) = t r ( A ) ( 1 ) tr(A^{T})=tr(A)_{(1)} tr(AT)=tr(A)(1)， d t r ( A B ) d A = B ( 2 ) T \frac{\mathrm{d}tr(AB)}{\mathrm{d}A}=B^{T}_{(2)} dAdtr(AB)=B(2)T， d t r ( A B A T ) d A = 2 A B ( 3 ) \frac{\mathrm{d}tr(ABA^{T})}{\mathrm{d}A}=2AB_{(3)} dAdtr(ABAT)=2AB(3)
- 两个相互独立的变量相乘的期望等于它们的期望相乘，即 E ( A B ) = E ( A ) E ( B ) ( 4 ) E(AB)=E(A)E(B)_{(4)} E(AB)=E(A)E(B)(4)
- 协方差矩阵为对称矩阵，即有 P = P ( 5 ) T P=P^{T}_{(5)} P=P(5)T，且其主对角线元素为各个变量的方差。
下面继续推导，将变量代入
e
k
=
x
k
−
x
^
k
e_k=x_k-\hat x_k
ek=xk−x^k，有
e
k
=
x
k
−
(
x
^
k
−
+
K
(
z
k
−
H
k
x
^
k
−
)
e_k=x_k-(\hat x_k^{-}+K(z_k-H_k\hat x_k^{-})
ek=xk−(x^k−+K(zk−Hkx^k−)，因为
z
k
=
H
k
x
k
−
v
k
z_k=H_kx_k-v_k
zk=Hkxk−vk，所以
e
k
=
x
k
−
(
x
^
k
−
+
K
(
H
k
x
k
−
v
k
−
H
k
x
^
k
−
)
)
e_k=x_k-(\hat x_k^{-}+K(H_kx_k-v_k-H_k\hat x_k^{-}))
ek=xk−(x^k−+K(Hkxk−vk−Hkx^k−))，展开并合并同类项得
e
k
=
(
I
−
K
H
k
)
(
x
k
−
x
^
k
−
)
+
K
v
k
e_k=(I-KH_k)(x_k-\hat x_k^{-})+Kv_k
ek=(I−KHk)(xk−x^k−)+Kvk，又因为先验估计值和真实值误差
e
^
k
−
=
x
k
−
x
^
k
−
\hat e_k^{-}=x_k-\hat x_k^{-}
e^k−=xk−x^k−，所以，
e
k
=
(
I
−
K
H
k
)
e
^
k
−
+
K
v
k
e_k=(I-KH_k)\hat e_k^{-}+Kv_k
ek=(I−KHk)e^k−+Kvk，则:
e
k
e
k
T
=
(
(
I
−
K
H
k
)
e
^
k
−
+
K
v
k
)
(
(
I
−
K
H
k
)
e
^
k
−
+
K
v
k
)
T
=
(
(
I
−
K
H
k
)
e
^
k
−
+
K
v
k
)
(
e
^
k
−
T
(
I
−
H
k
T
K
T
)
+
v
k
T
K
T
)
=
(
I
−
K
H
k
)
e
^
k
−
e
^
k
−
T
(
I
−
H
k
T
K
T
)
+
K
v
k
e
^
k
−
T
(
I
−
H
k
T
K
T
)
+
(
I
−
K
H
k
)
e
^
k
−
K
v
k
+
+
K
v
k
v
k
T
K
T
\begin{aligned} e_ke_k^{T}= & ((I-KH_k)\hat e_k^{-}+Kv_k)((I-KH_k)\hat e_k^{-}+Kv_k)^{T} \\ =& ((I-KH_k)\hat e_k^{-}+Kv_k)(\hat e_k^{-T}(I-H_k^{T}K^{T})+v_k^{T}K^{T}) \\ = & (I-KH_k)\hat e_k^{-}\hat e_k^{-T}(I-H_k^{T}K^{T}) \\ & +Kv_k\hat e_k^{-T}(I-H_k^{T}K^{T})\\ & +(I-KH_k)\hat e_k^{-}Kv_k+ \\ & +Kv_kv_k^{T}K^{T} \end{aligned}
ekekT===((I−KHk)e^k−+Kvk)((I−KHk)e^k−+Kvk)T((I−KHk)e^k−+Kvk)(e^k−T(I−HkTKT)+vkTKT)(I−KHk)e^k−e^k−T(I−HkTKT)+Kvke^k−T(I−HkTKT)+(I−KHk)e^k−Kvk++KvkvkTKT
则：
P
k
=
E
(
e
k
e
k
T
)
=
(
I
−
K
H
k
)
E
(
e
^
k
−
e
^
k
−
T
)
(
I
−
H
k
T
K
T
)
+
K
E
(
v
k
)
E
(
e
^
k
−
T
)
(
I
−
H
k
T
K
T
)
根据公式(4)
+
(
I
−
K
H
k
)
E
(
e
^
k
−
)
K
E
(
v
k
)
根据公式(4)
+
K
E
(
v
k
v
k
T
)
K
T
\begin{aligned} P_k= & E(e_ke_k^{T}) \\ = & (I-KH_k)E(\hat e_k^{-}\hat e_k^{-T})(I-H_k^{T}K^{T}) \\ & +KE(v_k)E(\hat e_k^{-T})(I-H_k^{T}K^{T}) &\text{根据公式(4)} \\ & +(I-KH_k)E(\hat e_k^{-})KE(v_k) &\text{根据公式(4)}\\ & +KE(v_kv_k^{T})K^{T} \end{aligned}
Pk==E(ekekT)(I−KHk)E(e^k−e^k−T)(I−HkTKT)+KE(vk)E(e^k−T)(I−HkTKT)+(I−KHk)E(e^k−)KE(vk)+KE(vkvkT)KT根据公式(4)根据公式(4)
基于假设
v
k
∼
N
(
0
,
R
k
)
v_k\sim N(0, R_k)
vk∼N(0,Rk)，
e
^
k
−
∼
N
(
0
,
P
k
−
)
\hat e_k^{-}\sim N(0, P_k^{-})
e^k−∼N(0,Pk−)，有
E
(
v
k
)
=
0
E(v_k)=0
E(vk)=0 ，
E
(
e
^
k
−
)
=
0
E(\hat e_k^{-})=0
E(e^k−)=0，
E
(
v
k
v
k
T
)
=
R
k
E(v_kv_k^{T})=R_k
E(vkvkT)=Rk ，
E
(
e
^
k
−
e
^
k
−
T
)
=
P
k
−
E(\hat e_k^{-}\hat e_k^{-T})=P_k^{-}
E(e^k−e^k−T)=Pk−，所以：
P
k
=
(
I
−
K
H
k
)
P
k
−
(
I
−
H
k
T
K
T
)
+
K
R
k
K
T
=
P
k
−
−
K
H
k
P
k
−
−
P
k
−
H
k
T
K
T
+
K
H
k
P
k
−
H
k
T
K
T
+
K
R
k
K
T
\begin{aligned} P_k= & (I-KH_k)P_k^{-}(I-H_k^{T}K^{T}) \\ & +KR_kK^{T} \\ = & P_k^{-}-KH_kP_k^{-}-P_k^{-}H_k^{T}K^{T}+KH_kP_k^{-}H_k^{T}K^{T}+KR_kK^{T} \end{aligned}
Pk==(I−KHk)Pk−(I−HkTKT)+KRkKTPk−−KHkPk−−Pk−HkTKT+KHkPk−HkTKT+KRkKT
为了让
P
k
P_k
Pk的迹
t
r
(
P
k
)
tr(P_k)
tr(Pk)最小，可以令
t
r
(
P
k
)
tr(P_k)
tr(Pk)对
K
K
K求导的结果为0，即：
d
t
r
(
P
k
)
d
K
=
d
P
k
−
d
K
(a)
−
d
K
H
k
P
k
−
d
K
(b)
−
d
P
k
−
H
k
T
K
T
d
K
(c)
+
d
K
H
k
P
k
−
H
k
T
K
T
d
K
(d)
+
d
K
R
k
K
T
d
K
(e)
=
0
\begin{aligned} \frac{\mathrm{d}tr(P_k)}{\mathrm{d}K}=& \frac{\mathrm{d}P_k^{-}}{\mathrm{d}K} & \text{(a)}\\ & -\frac{\mathrm{d}KH_kP_k^{-}}{\mathrm{d}K} & \text{(b)}\\ & -\frac{\mathrm{d}P_k^{-}H_k^{T}K^{T}}{\mathrm{d}K} & \text{(c)}\\ & +\frac{\mathrm{d}KH_kP_k^{-}H_k^{T}K^{T}}{\mathrm{d}K} & \text{(d)} \\ & +\frac{\mathrm{d}KR_kK^{T}}{\mathrm{d}K} & \text{(e)} \\ & =0 \end{aligned}
dKdtr(Pk)=dKdPk−−dKdKHkPk−−dKdPk−HkTKT+dKdKHkPk−HkTKT+dKdKRkKT=0(a)(b)(c)(d)(e)
其中a项为0；b项根据公式(2)有
−
d
K
H
k
P
k
−
d
K
=
−
P
k
−
T
H
k
T
-\frac{\mathrm{d}KH_kP_k^{-}}{\mathrm{d}K}=-P_k^{-T}H_k^{T}
−dKdKHkPk−=−Pk−THkT，又因为公式(5)有
−
P
k
−
T
H
k
T
=
−
P
k
−
H
k
T
-P_k^{-T}H_k^{T}=-P_k^{-}H_k^{T}
−Pk−THkT=−Pk−HkT；c项根据公式(1)有
−
d
P
k
−
H
k
T
K
T
d
K
=
−
d
K
H
k
P
k
−
T
d
K
=
−
P
k
−
H
k
T
-\frac{\mathrm{d}P_k^{-}H_k^{T}K^{T}}{\mathrm{d}K}=-\frac{\mathrm{d}KH_kP_k^{-T}}{\mathrm{d}K}=-P_k^{-}H_k^{T}
−dKdPk−HkTKT=−dKdKHkPk−T=−Pk−HkT；d项、e项根据公式(3)分别有
d
K
H
k
P
k
−
H
k
T
K
T
d
K
=
−
2
K
H
k
P
k
−
H
k
T
\frac{\mathrm{d}KH_kP_k^{-}H_k^{T}K^{T}}{\mathrm{d}K}=-2KH_kP_k^{-}H_k^{T}
dKdKHkPk−HkTKT=−2KHkPk−HkT、
d
K
R
k
K
T
d
K
=
2
K
R
k
\frac{\mathrm{d}KR_kK^{T}}{\mathrm{d}K}=2KR_k
dKdKRkKT=2KRk，所以：
d
t
r
(
P
k
)
d
K
=
−
2
P
k
−
H
k
T
+
2
K
H
k
P
k
−
H
k
T
+
2
K
R
k
=
0
\frac{\mathrm{d}tr(P_k)}{\mathrm{d}K}=-2P_k^{-}H_k^{T}+2KH_kP_k^{-}H_k^{T}+2KR_k=0
dKdtr(Pk)=−2Pk−HkT+2KHkPk−HkT+2KRk=0
经过简单的代数运算可以得到：
K
=
P
k
−
H
k
T
(
H
k
P
k
−
H
k
T
+
R
k
)
−
1
K=P_k^{-}H_k^{T}(H_kP_k^{-}H_k^{T}+R_k)^{-1}
K=Pk−HkT(HkPk−HkT+Rk)−1
到这里卡尔曼增益K已经推导出来了，可以将
K
K
K代入上面
P
k
P_k
Pk的等式最后两项，得：
P
k
=
P
k
−
−
K
H
k
P
k
−
−
P
k
−
H
k
T
K
T
+
K
(
H
k
P
k
−
H
k
T
K
T
+
R
k
)
K
T
=
P
k
−
−
K
H
k
P
k
−
−
P
k
−
H
k
T
K
T
+
P
k
−
H
k
T
(
H
k
P
k
−
H
k
T
+
R
k
)
−
1
(
H
k
P
k
−
H
k
T
+
R
k
)
K
T
=
P
k
−
−
K
H
k
P
k
−
−
P
k
−
H
k
T
K
T
+
P
k
−
H
k
T
K
T
=
P
k
−
−
K
H
k
P
k
−
\begin{aligned} P_k= & P_k^{-}-KH_kP_k^{-}-P_k^{-}H_k^{T}K^{T}+K(H_kP_k^{-}H_k^{T}K^{T}+R_k)K^{T} \\ = & P_k^{-}-KH_kP_k^{-}-P_k^{-}H_k^{T}K^{T}+P_k^{-}H_k^{T}(H_kP_k^{-}H_k^{T}+R_k)^{-1}(H_kP_k^{-}H_k^{T}+R_k)K^{T} \\ = & P_k^{-}-KH_kP_k^{-}-P_k^{-}H_k^{T}K^{T}+P_k^{-}H_k^{T}K^{T} \\ = & P_k^{-}-KH_kP_k^{-} \end{aligned}
Pk====Pk−−KHkPk−−Pk−HkTKT+K(HkPk−HkTKT+Rk)KTPk−−KHkPk−−Pk−HkTKT+Pk−HkT(HkPk−HkT+Rk)−1(HkPk−HkT+Rk)KTPk−−KHkPk−−Pk−HkTKT+Pk−HkTKTPk−−KHkPk−
而
P
k
−
=
E
(
e
^
k
−
e
^
k
−
T
)
=
E
(
(
x
k
−
x
^
k
−
)
(
x
k
−
x
^
k
−
)
T
)
P_k^{-}=E(\hat e_k^{-}\hat e_k^{-T})=E((x_k-\hat x_k^{-})(x_k-\hat x_k^{-})^{T})
Pk−=E(e^k−e^k−T)=E((xk−x^k−)(xk−x^k−)T)，可以将
x
k
=
A
k
x
k
−
1
+
B
k
u
k
+
w
k
x_k=A_kx_{k-1}+B_ku_{k}+w_k
xk=Akxk−1+Bkuk+wk、
x
^
k
−
=
A
k
x
^
k
−
1
+
B
k
u
k
\hat x_k^{-}=A_k\hat x_{k-1}+B_ku_k
x^k−=Akx^k−1+Bkuk代入得到：
P
k
−
=
E
(
(
A
k
x
k
−
1
+
B
k
u
k
+
w
k
−
A
k
x
^
k
−
1
−
B
k
u
k
)
(
A
k
x
k
−
1
+
B
k
u
k
+
w
k
−
A
k
x
^
k
−
1
−
B
k
u
k
)
T
)
=
E
(
(
A
k
(
x
k
−
1
−
x
^
k
−
1
)
+
w
k
)
(
A
k
(
x
k
−
1
−
x
^
k
−
1
)
+
w
k
)
T
)
=
E
(
(
A
k
e
^
k
−
1
+
w
k
)
(
A
k
e
^
k
−
1
+
w
k
)
T
)
=
E
(
(
A
k
e
^
k
−
1
+
w
k
)
(
e
^
k
−
1
T
A
k
T
+
w
k
T
)
)
=
E
(
A
k
e
^
k
−
1
e
^
k
−
1
T
A
k
T
+
w
k
e
^
k
−
1
T
A
k
T
+
A
k
e
^
k
−
1
w
k
T
+
w
k
w
k
T
)
=
E
(
A
k
e
^
k
−
1
e
^
k
−
1
T
A
k
T
)
+
E
(
w
k
e
^
k
−
1
T
A
k
T
)
+
E
(
A
k
e
^
k
−
1
w
k
T
)
+
E
(
w
k
w
k
T
)
=
A
k
P
k
−
1
A
k
T
+
0
+
0
+
Q
k
=
A
k
P
k
−
1
A
k
T
+
Q
k
\begin{aligned} P_k^{-}= & E((A_kx_{k-1}+B_ku_{k}+w_k-A_k\hat x_{k-1}-B_ku_k)(A_kx_{k-1}+B_ku_{k}+w_k-A_k\hat x_{k-1}-B_ku_k)^{T}) \\ = & E((A_k(x_{k-1}-\hat x_{k-1})+w_k)(A_k(x_{k-1}-\hat x_{k-1})+w_k)^{T}) \\ = & E((A_k\hat e_{k-1}+w_k)(A_k\hat e_{k-1}+w_k)^{T}) \\ = & E((A_k\hat e_{k-1}+w_k)(\hat e_{k-1}^{T}A_k^{T}+w_k^{T})) \\ = & E(A_k\hat e_{k-1}\hat e_{k-1}^{T}A_k^{T}+w_k\hat e_{k-1}^{T}A_k^{T}+A_k\hat e_{k-1}w_k^{T}+w_kw_k^{T}) \\ = & E(A_k\hat e_{k-1}\hat e_{k-1}^{T}A_k^{T})+E(w_k\hat e_{k-1}^{T}A_k^{T})+E(A_k\hat e_{k-1}w_k^{T})+E(w_kw_k^{T}) \\ = & A_kP_{k-1}A_k^{T}+0+0+Q_k \\ = & A_kP_{k-1}A_k^{T}+Q_k \end{aligned}
Pk−========E((Akxk−1+Bkuk+wk−Akx^k−1−Bkuk)(Akxk−1+Bkuk+wk−Akx^k−1−Bkuk)T)E((Ak(xk−1−x^k−1)+wk)(Ak(xk−1−x^k−1)+wk)T)E((Ake^k−1+wk)(Ake^k−1+wk)T)E((Ake^k−1+wk)(e^k−1TAkT+wkT))E(Ake^k−1e^k−1TAkT+wke^k−1TAkT+Ake^k−1wkT+wkwkT)E(Ake^k−1e^k−1TAkT)+E(wke^k−1TAkT)+E(Ake^k−1wkT)+E(wkwkT)AkPk−1AkT+0+0+QkAkPk−1AkT+Qk
P
k
−
P_k^{-}
Pk−的推导基于假设
w
k
∼
N
(
0
,
Q
k
)
w_k\sim N(0, Q_k)
wk∼N(0,Qk)、
e
^
k
∼
N
(
0
,
P
k
)
\hat e_k\sim N(0, P_k)
e^k∼N(0,Pk)。
以上推导完卡尔曼滤波的所有公式。在推导的过程中我们也发现了应用卡尔曼滤波的前提条件：
（1）系统应该是线性的
（2）过程噪声和测量噪声是相互独立的，且都必须满足正态分布
如果满足以上条件，则卡尔曼滤波是一个线性最优无偏估计算法。
在实际的工程应用中，在误差允许范围内，有的系统模型可以通过一些近似运算来构造成线性系统，但是有的系统非线性程度比较严重，直接应用卡尔曼滤波会出现较大的误差，对于这种系统，可以使用EKF（扩展卡尔曼滤波器）、UKF（无迹卡尔曼滤波器）或者PF（粒子滤波），后续文章会继续介绍这几种滤波器。
655
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
