卡尔曼的理论
一、初始化
我们认为小车在第1秒时的状态x与测量值z相等
二、预测(Prediction)
完成初始化后，我们开始写Prediction部分的代码。首先是公式:
x
′
=
F
x
+
u
(1)
\ x^{'} = \ Fx +u \tag{1}
x′= Fx+u(1)
这里的x为状态向量，通过左乘一个矩阵F，再加上外部的影响u，得到预测的状态向量x’。这里的F被称作状态转移矩阵（state transistion matrix）。以2维的匀速运动为例，这里的x为:
x
=
[
p
x
p
y
v
x
v
y
]
x=\left[ \begin{matrix} p_x \\ p_y \\ v_x \\ v_y \end{matrix} \right] \
x=⎣⎢⎢⎡pxpyvxvy⎦⎥⎥⎤
再来看看预测模块的第二个公式：
(因为cv模型是线性模型，因此F是线性的，因此不需要用talor展开线性化，但是CTRV需要)
P
′
=
F
P
F
T
+
Q
(2)
P^{'}=FPF^{T} + Q \tag{2}
P′=FPFT+Q(2)
该公式中P表示系统的不确定程度，这个不确定程度，在卡尔曼滤
波器初始化时会很大，随着越来越多的数据注入滤波器中，不确
定程度会变小，P的专业术语叫状态协方差矩阵（state covariance matrix）；
这里的Q表示过程噪声（process covariance matrix），即无法用
x'=Fx+u表示的噪声，比如车辆运动时突然到了上坡，这个影响是
无法用之前的状态转移方程估计的。
x'时在加一个噪声来描述我们的过程模型与实际运动的差异，这个噪声我们称之为过程噪声 (ProcessProcess NoiseNoise)
二、观测(Measurement)
观测的第一个公式: 该公式计算残差
y
=
z
−
H
x
′
(3)
\ y=z-Hx^{'}\tag{3}
y=z−Hx′(3)
这个公式计算的是实际观测到的测量值z(z的表达中有一个噪声W由传感器决定，z=Hx+w, 这里注意是Hx这个x是状态向量不是x_hat, 后文中s的噪声R是w的协方差)与预测值x’之间差值y。不同传感器的测量值一般不同，比如激光雷达测量的位置信号为x方向和y方向上的距离，毫米波雷达测量的是位置和角度信息。因此需要将状态向量左乘一个矩阵H，才能与测量值进行相应的运算，这个H被称为测量矩阵（Measurement Matrix）。
激光雷达的测量值为：
z
=
[
x
m
y
m
]
z=\left[ \begin{matrix} x_m \\ y_m \end{matrix} \right] \
z=[xmym]
由于x’是一个41的列向量，如果要与一个21的列向量z进行减运算，需要左乘一个2*4的矩阵才行，因此整个公式最终要写成：
(如果是毫米波雷达，由于毫米波雷达的测量矩阵没法线性化，所以此处还需要加一个泰勒展开)
再看接下里的两个公式
S
=
H
P
′
H
T
+
R
(4)
\ S=HP^{'}H^{T}+R \tag{4}
S=HP′HT+R(4)
K
=
P
′
H
T
S
−
1
(5)
\ K=P^{'}H^{T}S^{-1}\tag{5}
K=P′HTS−1(5)
这两个公式求的是卡尔曼滤波器中一个很重要的量——卡尔曼增益K（Kalman Gain），用人话讲就是求y值的权值。第一个公式中的R是测量噪声矩阵（measurement covariance matrix），这个表示的是测量值与真值之间的差值。一般情况下，传感器的厂家会提供该值。S只是为了简化公式，写的一个临时变量，不要太在意。
看最后两个公式：
x
=
x
′
+
K
y
(6)
\ x=x^{'}+Ky \tag{6}
x=x′+Ky(6)
P
=
(
I
−
K
H
)
P
′
(7)
\ P=(I-KH)P^{'}\tag{7}
P=(I−KH)P′(7)
这两个公式，实际上完成了卡尔曼滤波器的闭环，第一个公式是完成了当前状态向量x的更新，不仅考虑了上一时刻的预测值，也考虑了测量值，和整个系统的噪声，第二个公式根据卡尔曼增益，更新了系统的不确定度P，用于下一个周期的运算，该公式中的I为与状态向量同维度的单位矩阵。