第五章中介绍的算法有效的校正了GPS信号和地图误差中主要的慢漂移误差，并据此克服了沿道路方向和垂直道路方向的主要误差（系统误差或者说慢漂移误差），达到准确匹配的目标。在本章中介绍的改进卡尔曼滤波地图匹配算法将进一步利用卡尔曼滤波器，使新设计的地图匹配算法不仅能够校正慢变化的系统误差，还能同时对随机误差进行处理。卡尔曼滤波由于自身在处理随机干扰方面的优越性，被广泛应用于含有随机噪声的信号滤波。在论文的研究中，根据GPS误差的特性，设计实现了利用卡尔曼滤波器能够对 GPS系统误差和随机误差进行有效校正的地图匹配算法。并且根据第三章对地图误差的分析，可以知道地图误差是连续慢变化的，也可以与GPS系统误差同样的方法来处理。在这部分新算法介绍中我们不再单独说明GPS误差和地图误差，而是把地图误差看作与GPS慢漂移同性质的。
6.1卡尔曼滤波器及其在车辆导航系统中的应用
卡尔曼滤波器是一种高效率的递归滤波器（自回归滤波器）[8]，用于估计离散时间过程的状态变量x
Rn ，模型假设处理的离散时间过程由以下离散随机差分方程描述。
其中F是作用在xk1上的状态变迁模型。B是作用在控制器向量uk上的输入- 控制模型。
定义观测变量zRn ，得到量测方程
H是观测模型，它被真实状态空间映射到观测空间。随机信号w，v分别表示过程激励噪声和观测噪声。假设他们为相互独立，正态分布的白色噪声：
初始状态以及每一时刻的噪声{x0,w1,… , wk, v1 … vk}都认为是互相独立的。实际上，很多真实世界的动态系统都不确切的符合这个模型；但是由于卡尔曼滤波器被设计在有噪声的情况下工作，近似的符合已经可以使这个滤波器非常有用了。当然如果系统模型能够更好的符合这个模型，就可以获得更好的效果。
卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一时刻状态的估计，做出对当前状态的预测。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。
预测
更新
卡尔曼滤波器最初就是用来设计空间导航系统的[1]，当前被广泛应用于各种系统实现信号滤波。在车辆导航系统中，也是目前使用的主要信号滤波手段和多传感器融合方法。然而在独立GPS车载导航系统的地图匹配算法中，卡尔曼滤波并没有引起足够的重视，一个重要原因是对于独立GPS信号的传统卡尔曼模型不能有效实现地图匹配目标。融合交通矢量地图上的道路网信息，是应用于地图匹配算法与单纯的GPS接收器信号滤波最大不同之处。例如论文[9]中，将地图上路段的几何数学表达式作为卡尔曼滤波的一个约束条件该地图匹配算法能够部分校正沿道路方向的误差。但是算法中对于随机噪声的模型没有进行仔细的研究与分析，而直接把GPS轨迹中的位置信息的误差模型作为均值为0的高斯白噪声来处理，所以匹配的稳定性比较差，当车辆行驶到新道路上时，需要较长时间的调整，而且对于沿道路方向误差没有进行处理。论文[10] 中介绍的算法虽然在状态空间中包含了互相垂直的两个方向上的状态，但是没有给出其随机噪声的准确模型，并且其观测方程中对应的观测量只来源于垂直道路方向误差的观测。以上两种算法都提出了利用卡尔曼滤波处理地图匹配问题的新思路，但是都没有对于信号的噪声模型给予足够的重视，对于当前地图匹配问题中最需要解决的沿道路方向上的误差也没有给出有效的方法。
在独立GPS导航系统中，使用卡尔曼滤波器实现车辆位置的地图匹配方法，大部分都是将GPS信息中的位置、速度、加速度作为状态空间量[11]。实际上对于车辆定位最重要的位置信息，其观测值在车辆运行过程中并非零均值的白色噪声，不符合卡尔曼滤波器模型中的0均值的高斯白噪声假设，如图3.2，图3.3所示。图3.2中车辆位置观测值的误差期望是随时间渐变的，当车辆在同一路段或者相邻路段上行驶时（时间<300s），误差的期望值在定位的精度范围内基本保持不变（<1m）。图3.3中，位置观测误差的自相关函数并不是冲激函数d (k)，这说明从GPS车载接收器获得的车辆位置信息并不是白噪声，当前时刻的位置概率分布与前面时刻有关。同时由于地图道路网连续慢变化误差的存在，会导致两种误差的合成在以上所说的两方面表现更为明显。以上情况决定了单纯利用从GPS信息中获得的位置信息作为观测值，卡尔曼滤波的状态估计无法达到最优。所以调整卡尔曼滤波器的状态空间模型使之符合卡尔曼滤波器的基本假设是必要的。
6.2 算法模型
两种分解方式如下所示：
并且
(k)v = q(k ) -p(k) (6.9) ev 与DSe(k ) DSn(k) vv 分别为x和
获得e(k)是求得q(k)的前提，对于垂直于道路方向上的误差分量ev(k) 比较容易，只需要求取轨迹点 g(k)到其在弧段上的垂足q(k)距离即可，于是如何求取沿道路方向上的误差分量eh(k)就成了要解决的主要问题。实际上eh(k)的求取是现有算法都没有有效处理的问题，这是引起路口导航提醒不准确，导航失败的一个主 要问题。
图3 GPS误差的观测噪声的期望值随时间的变化函数
图6.2 GPS误差观测噪声的自相关函数
6.3转弯后状态变量与估计误差协方差的更新
在车辆导航过程中，每当经过一个转弯车辆在新路段上的匹配点个数达到阈值Ceff 时，就用这种方法对滤波器进行更新（或者说重新初始化），可以保证地图匹配算法在导航过程中能够有效校正垂直道路方向误差ev(k) 和沿道路方向误差eh(k) 。
图6.3 车辆转弯处进行轨迹沿道路方向的误差求解
弧段纵向误差eh(k) 的求取与匹配点p(k)直接相关，是地图道路匹配算法最重要又最难精确求解的预测变量。实现车辆转向行驶至每一条新弧段Si时自动获得eh(k) 的准确更新，是本文的重要工作之一。
6.4完整算法流程
Step1 ：从车载GPS接收机获得最新的轨迹信息。
Step2 ：用6.2中设计的卡尔曼滤波器预测方程预测当前的位置、速度、位置偏差量。
Step3：将新的车辆位置、速度信息、位置偏差信息作为对应状态量的观测值，用卡尔曼滤波器的更新方程更新Step2中的预测值。
Step4 ：将更新后的车辆位置按照最短距离原则确定匹配道路并垂直匹配到路段上。更新位置偏差信息。
Step5 ：判断匹配到当前路段上的匹配点数是否为Ceff ，如果是，利用论文6.4部分介绍的方法更新卡尔曼滤波器状态空间以及状态估计方差。并转到Step1，否则直接转到Step1.
6.5算法效果
将改进的卡尔曼滤波地图算法，应用于车辆导航系统的仿真平台进行验证。
在实验中设置Q和R为常量
图6.4显示的是一个交叉路口附近从车载GPS接收仪获得的车辆GPS原始轨迹数据。从图中的轨迹可以看出在论文第二部分分析的GPS轨迹误差的均值非0特性，车辆轨迹有相似的偏移。图6.5是经过新地图匹配算法中的卡尔曼滤波器滤波过后的车辆轨迹。通过图6.4与图6.5的对比，可以看出新地图匹配算法中的卡
尔曼滤波器不仅能有效校正GPS轨迹垂直于道路方向的误差，而且对于轨迹沿道路方向的误差能够有效校正，尤其在交叉路口和转弯处，这种沿道路方向的准确性的提高能高有效提高GPS车载导航系统导航的准确性、及时性和稳定性。图6.6是地图匹配算法，将卡尔曼滤波校正后的轨迹点垂直匹配到地图道路上，最后完成轨迹到地图道路匹配的结果。完成了将车辆轨迹匹配到地图道路上的最终结果。
图6.4交叉路口附近GPS原始轨迹
图 6.5交叉路口附近经过卡尔曼滤波后的轨迹
图 6.6交叉路口附近通过垂直匹配完成最终匹配后的轨迹
图6.7和图6.8分别是实验中一段道路上原始轨迹沿道路方向误差与经过卡尔曼滤波后轨迹的沿道路方向误差，从两幅图的对比可以更清楚的看出新地图匹配算在校正GPS轨迹沿道路方向误差方面的作用。滤波前误差均值为22.73m，滤波之后是0.95m。这里的误差曲线是车辆开始运行一段时间之后的，在地图匹配的初始阶段，由于历史信息有限，无法达到初始化之后的校正效果。
图6.7不同时刻GPS轨迹位置沿道路方向误差
图6.8经过新设计的卡尔曼滤波处理后的不同时刻GPS 轨迹位置沿道路方向误差
6.6算法评论与总结
论文在这一部分根据GPS 误差及其相关噪声的特性，在新地图匹配算法的卡尔曼滤波器的状态空间中加入GPS轨迹在两个垂直方向上的误差，并且通过利用车辆转弯位置和GPS误差统计特性来获得GPS轨迹的准确对应位置（校正了垂直道路方向误差和基本校正了沿道路方向误差），通过上述两种方法的结合，新地图匹配算法能够有效克服GPS信号中的慢漂移误差和随机误差。
慢漂移误差的跟踪与克服主要是依靠加入的GPS误差作为状态量和转弯位置处的误差准确校正和系统状态更新，随机误差主要通过卡尔曼滤波器本身的特点进行最优估计来克服。这两个误差的同时处理使新算法能够有效校正垂直道路方向的误差以及现在急需解决的沿道路方向误差。并且通过导航仿真平台实验，改进卡尔曼滤波地图匹配算法被很好的证明了其对于轨迹误差处理的有效性，以及对于定位准确性可靠性的提升。
本论文研究的主要目标是将卡尔曼滤波器更有效的用于车辆导航系统的地图匹配问题处理中以及如何有效处理沿道路方向的GPS轨迹误差。通过对GPS误差统计特性的研究，以及车辆转弯处状态更新方法的设计，成功达到了研究目标。在车辆导航的实际应用中，可以将本文的算法与现有的各种比较成熟的车辆匹配道路选取算法[3]相结合使其具有更好的稳定性和兼容性。
在论文的研究中，主要分析GPS轨迹误差。但是在实验过程中发现，一定范围内的地图同样可以得到校正。其原因在于新算法中处理GPS慢漂移误差的方法同样可以有效处理连续慢变化的地图误差。
