轨迹预处理（轨迹清洗）-卡尔曼滤波、扩展的卡尔曼滤波、粒子滤波在轨迹清洗中的应用

1.卡尔曼滤波、扩展的卡尔曼滤波、粒子滤波的理论与应用

       GPS是全球卫星定位系统，可以帮助我们获得经纬度坐标。GPS在汽车上的广泛装备与应用，可以很方便地帮助我们获得海量的汽车轨迹信息。然而，GPS设备有时存在定位误差较大的问题[1],导致轨迹中存在一些漂移点、回跳点、缺点等。这些点将会影响我们对路网拓扑结构的研究。因此，本文利用卡尔曼滤波对轨迹进行清洗。

  简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。它广泛应用在各个领域，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。我们可以将卡尔曼滤波理解为一个估计状态的算法（如估计汽车的位置，房间的温度等）。当我们需要获知一个物体的状态时，我们可以采用两种方法：1）观测法：采用传感器测得该物体的状态。2）预测法：通过构建一个模型来预测物体的状态。这两种方法都是存在误差的，那我们应该相信哪种方法呢？还是对两种方法的结果取平均？卡尔曼模型就是通过结合方法1和方法2给出一个估计值，而且如果方法1的误差小，就相信方法1多一点，反之亦然。接下来对卡尔曼模型进行详细介绍。我们知道两种方法都会存在误差，卡尔曼模型认为方法1，方法2的误差都符合高斯分布。方法1的结果误差我们定义为高斯模型A，方法2的结果误差我们定义为高斯模型B。接下来我们求出A与B的联合分布C。联合分布C的均值，其实就是我们的估计值。同时，C就可以作为方法2的输入来预测下一个状态。这样卡尔曼模型就能不断地循环下去了，直至完成。简而言之，卡尔曼的本质是：模型的预测值为高斯分布，系统的观测值为高斯分布。估计值为它们两个的联合高斯分布。求联合高斯分布就是求其均值与方差。该联合高斯分布又作为模型下一个预测值的输入，从而使卡尔曼能够迭代地运行下去。

        要想将我们的卡尔曼模型应用到实际轨迹清洗问题中，我们首先需要构建构建一个预测模型，该预测模型能够清楚的表达出系统的前一个状态和当前状态的关系。如假设一个目标在进行匀速直线运动，我们要对其进行追踪。该质点的坐标为（x,y）是直接观测到的，质点在x轴和y轴的速度为Vx，Vy，那么系统状态变量，