一看就懂的卡尔曼滤波五个公式
任何物体的运动都有一个运动方程：
x
k
^
=
A
x
k
?
1
+
u
+
w
,
w
∈
(
0
,
Q
)
(1)
\hat{x_k} =Ax_{k-1}+u+w ,w\in (0,Q) \tag{1}
xk?^?=Axk?1?+u+w,w∈(0,Q)(1)式中，k为某一个时刻，u为输入量，x为k-1时刻系统真实状态,w为方差Q的高斯误差，
x
k
^
\hat{x_k}
xk?^?为k时刻估计状态。
至此，根据这个系统状态我们有一个测量方程：
z
k
^
=
H
x
k
^
(2)
\hat{z_k} =H\hat{x_k}\tag{2}
zk?^?=Hxk?^?(2)式（2）被人称为先验。
这两个方程由于实际上 很多情况达不到标准导致了会有误差，然后方程变为：
z
k
=
H
x
k
^
+
v
,
v
∈
(
0
,
R
)
(3)
\\z_k=H\hat{x_k}+v,v\in (0,R)\tag{3}
zk?=Hxk?^?+v,v∈(0,R)(3)式中，v为方差R的高斯误差。
一般来说，式（3）中的
z
k
z_k
zk?是通过传感器测量出来的（测量值），式（2）中的
z
k
^
\hat{z_k}
zk?^?是我们通过运动方程转化过来的（理论值）。针对这两个值我们到底应该相信谁呢？当然是谁的表现好谁的权重大，从而可以建立一个方程：
H
X
n
e
w
=
(
1
?
K
)
H
x
k
^
+
K
z
k
=
H
x
k
^
+
K
(
z
k
?
H
x
k
^
)
(5)
HX_{new}=(1-K)H\hat{x_k}+Kz_k=H\hat{x_k}+K(z_k-H\hat{x_k})\tag{5}
HXnew?=(1?K)Hxk?^?+Kzk?=Hxk?^?+K(zk??Hxk?^?)(5)式中，
H
X
n
e
w
HX_{new}
HXnew?为后验，K为权重。然后会发现K如何取值，前面说了谁的表现好就取谁的多一点，而衡量一个好学生表现好的标准是他成绩好然后保持稳定，成绩好就是卡尔曼算法的初值需要好，稳定就是协方差要小，说白了就是需要保持在90分以上而不是一会儿70一会儿100的。所以我们需要对协方差建模，首先是
x
k
^
\hat{x_k}
xk?^?的协方差
p
k
￣
\overline{p_k}
pk??通过（1）式可得：
p
k
￣
=
A
e
k
?
1
e
k
?
1
′
A
′
+
Q
=
A
p
k
?
1
A
′
+
Q
(6)
\overline{p_k}=Ae_{k-1}e_{k-1}'A'+Q=Ap_{k-1}A'+Q\tag{6}
pk??=Aek?1?ek?1′?A′+Q=Apk?1?A′+Q(6)然后zk的协方差
p
k
~
\widetilde{p_k}
pk?
?也可以根据（3）求出：
p
k
~
=
H
e
k
^
e
k
^
′
H
′
+
R
=
H
p
k
￣
H
′
+
R
(7)
\widetilde{p_k}=H\hat{e_k}\hat{e_k}'H'+R=H\overline{p_k}H'+R\tag{7}
pk?
?=Hek?^?ek?^?′H′+R=Hpk??H′+R(7)而先验协方差
p
k
^
\hat{p_k}
pk?^?很容易从式（2）得出：
p
k
^
=
H
e
k
^
e
k
^
′
H
′
=
H
p
k
￣
H
′
\hat{p_k}=H\hat{e_k}\hat{e_k}'H'=H\overline{p_k}H'
pk?^?=Hek?^?ek?^?′H′=Hpk??H′从而我们可以建立一个权重表达式：
K
=
p
k
^
/
p
k
~
=
=
H
p
k
￣
H
′
/
(
H
p
k
￣
H
′
+
R
)
(8)
K= \hat{p_k}/ \widetilde{p_k}==H\overline{p_k}H'/(H\overline{p_k}H'+R)\tag{8}
K=pk?^?/pk?
?==Hpk??H′/(Hpk??H′+R)(8)从该市可看出理论值的协方差
p
k
^
\hat{p_k}
pk?^?越大，K越大，式（5）中理论值的比重越小，间接说明了式（5）的正确性，把K值代入式（5）我们可以得到：
H
X
n
e
w
=
H
x
k
^
+
H
p
k
￣
H
′
/
(
H
p
k
￣
H
′
+
R
)
?
(
z
k
?
H
x
k
^
)
(9)
HX_{new}=H\hat{x_k}+H\overline{p_k}H'/(H\overline{p_k}H'+R)*(z_k-H\hat{x_k})\tag{9}
HXnew?=Hxk?^?+Hpk??H′/(Hpk??H′+R)?(zk??Hxk?^?)(9)但是我们想要估计的是状态X_{new}，设
K
′
=
p
k
￣
H
′
/
(
H
p
k
￣
H
′
+
R
)
K'=\overline{p_k}H'/(H\overline{p_k}H'+R)
K′=pk??H′/(Hpk??H′+R)，从而式（5）可以两边除以H可得：
X
n
e
w
=
x
k
^
+
K
′
(
z
k
?
H
x
k
^
)
(10)
X_{new}=\hat{x_k}+K'(z_k-H\hat{x_k}) \tag{10}
Xnew?=xk?^?+K′(zk??Hxk?^?)(10)式中。
X
n
e
w
X_{new}
Xnew?估计值与真实值X_t的误差为：
x
t
?
X
n
e
w
=
x
t
?
x
k
^
?
K
′
(
H
x
t
?
H
x
k
^
+
v
)
=
e
k
?
K
′
H
e
k
+
K
′
v
(11)
x_t-X_{new}=x_t-\hat{x_k}-K'(Hx_t-H\hat{x_k}+v)=e_k-K'He_k+K'v \tag{11}
xt??Xnew?=xt??xk?^??K′(Hxt??Hxk?^?+v)=ek??K′Hek?+K′v(11)从而更新协方差方程：
P
k
=
（
I
?
K
′
H
）
p
k
￣
(12)
Pk=（I-K'H）\overline{p_k} \tag{12}
Pk=（I?K′H）pk??(12)
随便弄个跟踪图：
