Kalman滤波是什么？
Kalman滤波主要是用来跟踪某一个变量的值，跟踪的依据是首先根据系统的运动方程来对该值进行预测，比如我们知道一个物体的运动速度，那么下面时刻它的位置按照道理是可以预测出来的，不过该预测肯定有误差，只能作为跟踪的依据。另一个依据是可以用测量手段来测量那个变量的值，当然该测量也是有误差的，也只能作为依据，不过这两个依据的权重比例不同。Kalman滤波就是利用这两个依据进行一系列迭代进行目标跟踪的。
Kalman滤波：预测未来、修正当下
卡尔曼滤波器本质是一个“optimal recursive data processing algorithm“（最优自回归数据处理算法）。
Kalman滤波能用来做什么？
Kalman滤波是用来对状态进行估计的算法，本质上是利用状态过程噪声和测量噪声对状态进行估计。
Kalman滤波需要满足的条件：
注意：此处Kalman滤波指的都是线性Kalman滤波
线性随机微分系统、过程和测量噪声都是高斯白噪声
Kalman滤波过程
Kalman滤波算法分为两个阶段：
预测阶段：卡尔曼滤波器使用由当前点计算的协方差来估计目标的新位置；
更新阶段：卡尔曼滤波器记录目标的位置，并为下一次循环计算修正协方差。
卡尔曼滤波算法中包含系统预测过程（根据系统状态方程对系统状态参数进行预测）和系统测量（即系统观测量，观测的状态参量不一定和系统状态参量一样）两个重要部分。
首先引入一个离散控制过程的系统，该系统能够用一个线性随机微分方程描述（即系统状态方程）：
X(k)=AX(k−1)+BU(k)+W(k)
X
(
k
)
=
A
X
(
k
−
1
)
+
B
U
(
k
)
+
W
(
k
)
A——状态转移矩阵；
B——控制输入矩阵；
U(k)——k时刻对系统的控制量；
W(k)——系统过程噪声，为高斯白噪声，协方差为Q。
再加上系统的测量值（即观测量）：
Z(k)=HX(k)+V(k)
Z
(
k
)
=
H
X
(
k
)
+
V
(
k
)
H——系统测量矩阵；
V(k)——测量噪声，为高斯白噪声，协方差为R；
Z(k)——k时刻的测量值，也即对系统能够观测的量。对于系统状态变量而言，并不一定能够观测所有的量。
整个系统更新思路包括：状态更新和协方差更新
Kalman滤波的五个关键公式：
X(k|k−1)=AX(k−1|k−1)+BU(k)
X
(
k
|
k
−
1
)
=
A
X
(
k
−
1
|
k
−
1
)
+
B
U
(
k
)
……….. (1)
P(k|k−1)=AP(k−1|k−1)A′+Q
P
(
k
|
k
−
1
)
=
A
P
(
k
−
1
|
k
−
1
)
A
′
+
Q
……… (2) 预测的协方差更新
公式(1)和(2)表示对系统的预测值的计算；
Kg(k)=P(k|k−1)H′/(HP(k|k−1)H′+R)
K
g
(
k
)
=
P
(
k
|
k
−
1
)
H
′
/
(
H
P
(
k
|
k
−
1
)
H
′
+
R
)
……… (3) Kg为卡尔曼增益
X(k|k)=X(k|k−1)+Kg(k)(Z(k)−HX(k|k−1))
X
(
k
|
k
)
=
X
(
k
|
k
−
1
)
+
K
g
(
k
)
(
Z
(
k
)
−
H
X
(
k
|
k
−
1
)
)
……… (4)
公式(3)和(4)表示对系统的测量；
由以上四个公式，已经得到了k状态下的最优估计值
X(k|k)
X
(
k
|
k
)
。
但是为了使卡尔曼滤波器不断的运行下去直到系统过程结束，需要更新k状态下X(k|k)的协方差：
P(k|k)=（I−Kg(k)H）P(k|k−1)
P
(
k
|
k
)
=
（
I
−
K
g
(
k
)
H
）
P
(
k
|
k
−
1
)
……… (5)
其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。
运行过程：(1)(2)-(3)-(4)(5)-(1)(2)……
输出量：X(k|k)——k时刻系统状态最优估计值。
必要解释：
P是系统的误差协方差矩阵（也叫后验错误协方差矩阵），整个公式的思想和目的就是找到一个合适的Kalman增益，使得误差协方差矩阵最小。
