多传感器融合定位1（激光雷达+毫米波雷达）

前言

LZ最近在看Udacity的无人驾驶课程，该课程主要分为三部分，第一部分的课程主要使用Python实现的车道线识别、车牌识别等计算机视觉项目。由于我对定位、建图等方面有些知识储备，所以先从第二部分课程开始。
本节将用最简洁的话讲解卡尔曼滤波KF、非线性卡尔曼滤波EKF等知识点，并就此实现一个多传感器融合定位的小demo，后面会就粒子滤波PF专门开一个章节讲解。

对于匀速运动模型，KF和EKF的状态预测（Predict）过程是一样的；KF和EKF唯一区别的地方在于测量值更新（Update）这一步。在测量值更新中，KF使用的测量矩阵H是不变的，而EKF的测量矩阵H是Jacobian矩阵。

本文约束：匀速运动；固定位置的传感器对小车不断测量。

一、卡尔曼滤波 KF

1、引子

1）两个传感器测量同一个信号，为了减小误差我们可以采用取平均的方式，
2）进一步的我们采用加权平均（由方差大小分配），但加权平均是一种静态分配方式。3）方差是随外界环境而变的，加权值也应该随之改变，这就是卡尔曼滤波出现的原因，它是一种动态更新加权值，不断迭代的算法。

卡尔曼滤波器就是根据上一时刻x(k-1)的状态，预测当前时刻x(k)的状态，将预测的状态x^(k)与当前时刻的测量值z进行加权，加权后的结果才认为是当前的实际状态。

2、对象：线性高斯模型，这个模型最好的地方在于两个高斯分布的乘积仍然是一高斯分布，由此实现模型的动态迭代。

3、本质：参数化的贝叶斯模型。先验估计：对下一时刻系统初步状态估计；结合先验估计以及测量反馈，得到后验估计。

4、应用：最优化自回归数据处理算法、机器人导航、雷达系统、图像处理等

2、黄金公式及推导

直接上结论，下面这个公式分为两部分，上面的是预测，下面是更新。

具体公式推导，我竟然发现了我之前的笔记，下面直接上图解释。
为了避免推导晦涩难懂，采用小汽车的模型进行一步一步的推导。

1、建立模型

2、简单一维小车

假设有个小车匀速运动，我们在左侧安装了一个测量小车距离和速度传感器，每秒测一次小车位置s和速度v。我们用向量$x_t$状态向量。

由于测量误差的存在，无法获得真实值，我们可以以一个高斯分布来表示结果在各个地方出现的概率，如下图所示，在真值附近的概率最高。

$x_{t-1}=\left[\begin{array}{c}{s}_{t-1}\\ v_{t-1}\end{array}\right]$
接下来是使用历史信息对未来的位置进行推测，以速度v匀速行走$\Delta t$后，小车位置的红色区域范围变大了，这是因为预测时加入了速度估计的噪声，放大了不确定性。

$\left[\begin{array}{c}{s}_t\\ v_t\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{s}_{t-1}\\ v_{t-1}\end{array}\right]$
此时传感器对小车做了一次观测,结果为 z

上图蓝色区域为此时观测结果，红色为预测结果，那么最终的结果是怎样的呢？下图绿色部分给出了答案。


我们对预测和观测结果用不同的权重得到最终结果，两个权值的计算是根据预测结果和观测结果的不确定性来的，这个不确定性就是高斯分布中的方差的大小，方差越大，波形分布越广，不确定性越高，这样一来给的权值就会越低。

3、预测（Predict）

上面是一维小车，状态向量里只有它的位置和速度，当二维小车在平面时它的状态向量有分别是位置和速度的x、y方向（x,y,vx,vy）
$x_{}=\left[\begin{array}{c}x\\ y\\ vx\\ vy\end{array}\right]$

和上面的一维对应
$[\begin{array}{c}{s}_{}\end{array}$