轨迹预处理（轨迹清洗）-卡尔曼滤波、扩展的卡尔曼滤波、粒子滤波在轨迹清洗中的应用
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，那么系统状态变量，系统观测变量 ，系统没有控制输入，所以状态方程为：
（3.3.1）
其中A为状态转移矩阵，w为偏差。其中状态方程转移矩阵A根据运动学公式确定，方程如公式3.3.2所示。为了充分理解公式3.3.2，本文将其展开，展开公式如公式3.3.3所示。
由于该目标在进行匀速直线运动，有上述公式我们可以看到，我们可以很容易的构建状态转移方差，他们是线形关系，且“斜率”为A。很快我们遇到了一个新的问题，当汽车处于变加速直线运动时，我们无法通过运动学公式直接求出汽车的下一个状态的位置，这也就意味着我们无法构建一个预测模型，即我们无法将”斜率A“求出来。卡尔曼模型无法处理非线性模型。
扩展卡尔曼滤波（EKF）应运而生，它用雅克比矩阵将期望和方差线性化，从而将卡尔曼滤波扩展到非线性系统，但是EKF由于考虑了泰勒级数的展开，运算量大大增加。简而言之，扩展的卡尔曼模型与卡尔曼模型的唯一差别就在于，EFK进行了局部线形化的操作，使我们可以构建状态转移方程。EKF可以应用到非线形系统中。然而，我们遇到了另一个问题，卡尔曼模型与扩展的卡尔曼模型只适用与高斯分布。
粒子滤波模型很好的解决了误差不符合高斯分布的情况。当系统的结果误差不满足高斯分布时，或者我们不知道系统的误差满足什么分布时，我们可以对系统的误差分布进行采样，也即采用蒙特卡洛采样法，通过放置大量的粒子来估计系统的误差分布，也即估计方法2的结果。这些粒子距离方法1的结果的距离是不一样的，距离越近，权重越高。为了得到估计值，我们将方法1的结果看作似然分布，将方法2的结果看作先验分布，通过贝叶斯公式，我们可以得到后验分布，也即估计值。求得后验分布的粒子，在求其均值与方差的时候，我们发现有的粒子的权重已经退化，我们需要对粒子进行重采样。重采样之后再计算期望，期望作为我们的估计值。重采样之后的粒子可以作为方法2的输入。与之前的卡尔曼模型类似。粒子滤波很好地克服非高斯分布问题，同时也能够解决非线形问题。但是需要大量的粒子来保证采样的准确性，从而导致运行时间较长。
本文依托扩展的卡尔曼滤波模型进行了轨迹清洗。汽车的行驶过程是一个变加速行驶过程。我们采用的变加速直线运动来估计。本文用（x,y）代表汽车经纬度，代表汽车的行驶方向，v代表汽车速度，a代表加速度。本文将汽车下一个时刻的状态（经纬度，方向角度等），下一时刻的状态是
。公式3.3.4即是我们通过泰勒公式与雅可比式求得的“斜率A”。
对于系统状态和测量状态而言，还需要为它们设置误差。需要设置系统状态的位置误差sGPS、角度误差sCourse、速度误差sVelocity、加速度误差sAccel，以及测量状态的位置误差varGPS、速度误差varspeed、加速度误差varacc =5等。在参数设置时，首先要了解GPS误差在行业内的标准是多少，或者计算点到路的平均距离时多少。以此为基础进行调整。其次我们其实更加相信测量系统，所以我们将其误差调小。具体结果将在实验部分讨论。
本文还根据扩展的卡尔曼滤波模型进行了轨迹清洗。本文构建了N（N=500）个粒子，且这些粒子符合高斯分布。粒子包含3个属性的高斯分布：经纬度，方向。在构建粒子的状态转移方程时，采用的与扩展的卡尔曼滤波模型的非常相似。还需要更新粒子权重、判断有效粒子数、必要情况下进行重采样，同时还需要设置合适的阈值，过滤掉坏点。具体结果将在实验部分讨论。
2、轨迹清洗评估
汽车的行驶过程是一个变加速行驶过程。我们采用的变加速直线运动来设计状态方程。扩展的卡尔曼滤波整体结果如图3.5.2.1所示。X轴和Y轴构建了相对位置坐标系。该图中红点为原始位置，蓝点为估计位置。可以看到，蓝点与红点大致上是重合的。通过该图我们可以看到并预测值没有发散且已经很好地追踪到观察值。换句话说本文已经运动学公式构建的系统是合理的。
扩展的卡尔曼滤波局部结果如图3.5.2.2所示，X轴和Y轴构建了相对位置坐标系。该图中红点为原始位置，蓝点为估计位置。中部区域有一红点为漂移点，换句话说。而该点的预估位置在上方的蓝点处。扩展的卡尔曼滤波模型已经很好地将该漂移点丢去。
进一步地，为了衡量每个观察值与估计值的绝对位置，本文计算了GPS实际位置与估计位置之间的距离。结果如图3.5.2.3所示。X轴为GPS轨迹点的id，Y轴为该点与估计值之间的距离。其中最长的距离为81m，且该点为超速点，已经移除。最小的距离为0.1米。均值为6米。换句话说，扩展的卡尔曼滤波模型较好的进行了轨迹清洗。
本文依托运动学公式，建立了粒子滤波模型。粒子滤波整体结果如图3.5.2.1所示。X轴和Y轴构建了相对位置坐标系。该图中蓝色加号为原始位置，红色为估计位置。可以看到，蓝点与红点大致上是重合的。通过该图我们可以看到并预测值没有发散且已经很好地追踪到观察值。换句话说本文已经运动学公式构建的系统是合理的。
粒子滤波局部结果如图3.5.2.2所示，X轴和Y轴构建了相对位置坐标系。该图中蓝色加号为原始位置，红色为估计位置。中部区域有一蓝色加号为漂移点，换句话说。而该点的预估位置在上方的红点处。粒子滤波模型已经也很好地将该漂移点丢去。
同时，为了衡量每个观察值与估计值的绝对位置，本文计算了GPS实际位置与估计位置之间的距离。结果如图3.5.2.3所示。X轴为GPS轨迹点的id，Y轴为该点与估计值之间的距离。其中最长的距离为17.5m，且该点为超速点，已经移除。最小的距离为0.1米。均值为2.1米。
通过上述实验分析，我们可以看出，粒子滤波的效果要好于扩展的卡尔曼滤波。但是，粒子滤波的时间复杂度较高，不适合应用在应用中，因为本文采用扩折的卡尔曼滤波进行轨迹清洗，时间复杂度较低，且误差处于可接受的范围之内。