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

引用AdamShan
引用知乎陈光

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

卡尔曼的理论

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

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

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

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

• 重点强调一下，这个u噪声是process model的噪声，这个是我们的经验（比如说一些运动模型，牛顿力学等等），那么我们在计算x'时在加一个噪声来描述我们的过程模型与实际运动的差异，这个噪声我们称之为过程噪声 (ProcessProcess NoiseNoise)
  在直线运动中，过程噪声就是行人的加减速，如下：
  
  关于Q的组成还有点疑问：
  
  关于协方差矩阵还要插几句：
  引用
  过程噪声v~N(0,Q)为均值为0的高斯白噪声，对于4维的X来说，Q是4x4，如何化简Q，请看：
  
  
  
  
  
  
  

二、观测(Measurement)
观测的第一个公式: 该公式计算残差
$$y=z-H{x}^{{}^{\prime }}\text{\hspace{1em}(3)}$$

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

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

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

再看接下里的两个公式
$$S=H{P}^{{}^{\prime }}{H}^T+R\text{\hspace{1em}(4)}$$
$$K=P^{{}^{\prime }}{H}^T{S}^{-1}\text{\hspace{1em}(5)}$$

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

看最后两个公式：
$$x=x^{{}^{\prime }}+Ky\text{\hspace{1em}(6)}$$
$$P=(I-KH)P^{{}^{\prime }}\text{\hspace{1em}(7)}$$

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

KF_Adamshan