卡尔曼滤波与目标跟踪(由cv模型的kf推理到CTRV模型的radar与lidar))(一)

引用AdamShan
引用知乎陈光

基于cv模型的行人状态预测/卡尔曼滤波与目标追踪

卡尔曼的理论

一、初始化
我们认为小车在第1秒时的状态x与测量值z相等

二、预测(Prediction)
完成初始化后，我们开始写Prediction部分的代码。首先是公式:
$\ x^{'} = \ Fx +u \tag{1}$ x′= Fx+u(1)
这里的x为状态向量，通过左乘一个矩阵F，再加上外部的影响u，得到预测的状态向量x’。这里的F被称作状态转移矩阵（state transistion matrix）。以2维的匀速运动为例，这里的x为:
$x=\left[ \begin{matrix} p_x \\ p_y \\ v_x \\ v_y \end{matrix} \right] \$x=⎣⎢⎢⎡​px​py​vx​vy​​⎦⎥⎥⎤​ 

再来看看预测模块的第二个公式：
(因为cv模型是线性模型，因此F是线性的，因此不需要用talor展开线性化，但是CTRV需要)
$P^{'}=FPF^{T} + Q \tag{2}$P′=FPFT+Q(2)

该公式中P表示系统的不确定程度，这个不确定程度，在卡尔曼滤
波器初始化时会很大，随着越来越多的数据注入滤波器中，不确
定程度会变小，P的专业术语叫状态协方差矩阵（state covariance matrix）；
这里的Q表示过程噪声（process covariance matrix），即无法用
x'=Fx+u表示的噪声，比如车辆运动时突然到了上坡，这个影响是
无法用之前的状态转移方程估计的。

• 重点强调一下，这个u噪声是process model的噪声，这个是我们的经验（比如说一些运动模型，牛顿力学等等），那么我们在计算x'时在加一个噪声来描述我们的过程模型与实际运动的差异，这个噪声我们称之为过程噪声 (ProcessProcess NoiseNoise)
  在直线运动中，过程噪声就是行人的加减速，如下：
  
  关于Q的组成还有点疑问：
  

二、观测(Measurement)
观测的第一个公式: 该公式计算残差
$\ y=z-Hx^{'}\tag{3}$ y=z−Hx′(3)

这个公式计算的是实际观测到的测量值z(z的表达中有一个噪声W由传感器决定，z=Hx+w,后文中s的噪声R是w的协方差)与预测值x’之间差值y。不同传感器的测量值一般不同，比如激光雷达测量的位置信号为x方向和y方向上的距离，毫米波雷达测量的是位置和角度信息。因此需要将状态向量左乘一个矩阵H，才能与测量值进行相应的运算，这个H被称为测量矩阵（Measurement Matrix）。

激光雷达的测量值为：
$z=\left[ \begin{matrix} x_m \\ y_m \end{matrix} \right] \$z=[xm​ym​​] 
由于x’是一个41的列向量，如果要与一个21的列向量z进行减运算，需要左乘一个2*4的矩阵才行，因此整个公式最终要写成：

(如果是毫米波雷达，由于毫米波雷达的测量矩阵没法线性化，所以此处还需要加一个泰勒展开)

再看接下里的两个公式
$\ S=HP^{'}H^{T}+R \tag{4}$ S=HP′HT+R(4)
$\ K=P^{'}H^{T}S^{-1}\tag{5}$ K=P′HTS−1(5)

这两个公式求的是卡尔曼滤波器中一个很重要的量——卡尔曼增益K（Kalman Gain），用人话讲就是求y值的权值。第一个公式中的R是测量噪声矩阵（measurement covariance matrix），这个表示的是测量值与真值之间的差值。一般情况下，传感器的厂家会提供该值。S只是为了简化公式，写的一个临时变量，不要太在意。

看最后两个公式：
$\ x=x^{'}+Ky \tag{6}$ x=x′+Ky(6)
$\ P=(I-KH)P^{'}\tag{7}$ P=(I−KH)P′(7)

这两个公式，实际上完成了卡尔曼滤波器的闭环，第一个公式是完成了当前状态向量x的更新，不仅考虑了上一时刻的预测值，也考虑了测量值，和整个系统的噪声，第二个公式根据卡尔曼增益，更新了系统的不确定度P，用于下一个周期的运算，该公式中的I为与状态向量同维度的单位矩阵。

KF_Adamshan