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

引用知乎陈光
引用AdamShan

背景知识

车辆运动模型：直接上图了

• 扩展卡尔曼
  

正文

毫米波雷达的数据

毫米波雷达观察世界的方式与激光雷达有所不同。激光雷达测量的原理是光的直线传播，因此在测量时能直接获得障碍物在笛卡尔坐标系下x方向、y方向和z方向上的距离；而毫米波雷达的原理是多普勒效应，它所测量的数据都是在极坐标系下的。
如下图所示，毫米波雷达能够测量障碍物在极坐标下离雷达的距离ρ、方向角ϕ以及距离的变化率（径向速度）ρ’，如下图所示。

一，初始化

初始化扩展卡尔曼滤波器时需要输入一个初始的状态量x_in，用以表示障碍物最初的位置和速度信息，一般直接使用第一次的测量结果。

二、预测(Prediction)
完成初始化后，我们开始写Prediction部分的代码。首先是公式
$\ x^{'} = \ Fx +u \tag{1}$ x′= Fx+u(1)
现在问题来，CTRV中，preocess model是非线性：
$x^{'}=f(x, u)$x′=f(x,u)对比下式的线性化关系，从上面的公式很容易看出，等式两边的转化是非线性的，并不存在一个常数矩阵H，能够使得等式两边成立。
如果将高斯分布作为输入，输入到一个非线性函数中，得到的结果将不再符合高斯分布，也就将导致卡尔曼滤波器的公式不再适用。因此我们需要将上面的非线性函数转化为近似的线性函数求解。
$x^{'}=Fx_k+W_k$x′=Fxk​+Wk​

还需要考虑当$\omega=0$ω=0时，F状态转移矩阵的情况；
经过一阶泰勒展开得到F后，再来看预测模块的第二个公式：
$P^{'}=FPF^{T} + Q \tag{2}$P′=FPFT+Q(2)

(关于P，Q，u的初始化见前文，还有一些疑问)

代码观测：
观测的第一个公式: 该公式计算残差
$\ y=z-Hx^{'}\tag{3}$ y=z−Hx′(3)
对于：

1. 激光雷达忽略测量矩阵H的线性化求解过程：
   激光雷达的测量模型仍然是线性的，其测量矩阵为：
   $H_L=\left[ \begin{matrix} 1&0&0&0&0 \\ 0&1&0&0&0 \end{matrix} \right] \$HL​=[10​01​00​00​00​] 

2. 前面提到过毫米波雷达观测值z的数据特性，如下图所示：
   

由图可知观测值z的数据维度为3×1，为了能够实现矩阵运算，y和Hx’的数据维度也都为3×1。

为了简化表达，我们用px，py以及vx和vy表示预测后的位置及速度，如下所示：

毫米波雷达在转换时涉及到笛卡尔坐标系和极坐标系的位置、速度转换，这个转化过程是非线性的。因而在处理类似毫米波雷达这种非线性的模型时，习惯于将计算差值y的公式写成如下，以作线性和非线性模型的区分。
$\ y=z-H(x^{'})\tag{3}$ y=z−H(x′)(3)
对应上面的式子，这里的h(x’)为：

再看卡尔曼滤波器接下来的两个公式:
$\ 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只是为了简化公式，写的一个临时变量，不用太在意。

由于求得卡尔曼增益K需要使用到测量矩阵H，因此接下来的任务就是得到H。

毫米波雷达观测z是包含位置、角度和径向速度的3x1的列向量，状态向量x’是包含位置和速度信息的4x1的列向量，根据公式y=z-Hx’可知测量矩阵（Measurement Matrix）H的维度是3行4列。即：

我们将求偏导数的公式与我们的之前推导的公式对应起来看x的系数，会发现这里的测量矩阵H其实就是泰勒公式中的雅可比式。

多维的雅可比式的推导过程有兴趣的同学可以自己研究一下，这里我们直接使用其结论：

经过一系列计算，最终得到测量矩阵H为：

再看卡尔曼滤波器的最后两个公式:
$\ x=x^{'}+Ky \tag{6}$ x=x′+Ky(6)
$\ P=(I-KH)P^{'}\tag{7}$ P=(I−KH)P′(7)

相应的python代码地址是：
EKF_Adamshan