一看就懂的卡尔曼滤波五个公式
任何物体的运动都有一个运动方程：
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通过（3）式可得：
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−1ek−1′A′+Q=Apk−1A′+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=HpkH′+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′=HpkH′从而我们可以建立一个权重表达式：
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
==HpkH′/(HpkH′+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^+HpkH′/(HpkH′+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′=pkH′/(HpkH′+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)
随便弄个跟踪图：
