6.1 研究思路
上一章介绍了道路约束条件的想法。实验证明，将这一想法应用到GPS 定位导航当中可以起到提高定位精度的结果。这种带道路约束条件的GPS 定位滤波方法之所以有效，是因为它将人们对道路条件的知识引入了GPS 定位导航系统模型当中来，在卡尔曼滤波算法的过程中，利用了更多的先验信息。
在飞行器、船舶和车辆导航等一大类导航应用当中，载体行进路线（具体到车辆导航当中就是道路）的信息都是预先可以获得的。例如车辆沿着道路行驶，道路信息可以通过导航系统的矢量地图数据库获得，这一信息通常都是非常准确的。；而在有些情况下，这些路线甚至是预先设定好的，载体必须要沿着它行进。例如导弹沿着预先制定的弹道运行直至击中目标。所以，带道路约束条件的GPS定位导航滤波算法是有一定的应用价值的。
但是，必须注意到，在许多导航应用情形下，恰恰相反，载体的行进路线信息却很难获得，或者很不精确，不足以支持带道路约束条件的GPS 定位导航算法的运行。这种情况同样是很常见的。比如车辆在陌生的路段行驶，矢量地图库中没有此路段的任何信息。在这种情况下，上一章介绍的带约束条件的GPS 定位导航算法就无能为力了。
那么，有没有其他办法，来获得更多的关于载体运行的实时信息呢？这就是本章将要回答的问题。
6.2 速度方向约束条件
还是以车辆定位导航系统为例进行分析。考虑无法预先确切获得载体运动路线的情况。
其实在国内外的一些文献中，已经把速度观测量引入了观测方程，以期充分利用GPS 接收机输出的有用信息。
文献[22]指出，把速度观测量引入观测方程，增加了观测矩阵得秩，提高了状态的可观测度；同时，也意味着分别在载体位置、速度、加速度传递函数种增加了一个零点，因此增加了滤波器的带宽，因此可以明显减小滤波器动态误差。大量的仿真实验表明，在系统观测方程中引入速度观测量在一定程度上提高了滤波精度。但是当载体运动速度较低，或者载体运动状态发生突变，如大角度急转弯的时候，滤波器常常出现抖动、滞后甚至发散的情况。
分析表明，出现这种情况的原因，正是由于引入速度观测量的缘故。GPS 接收机接受到的是每一时刻载体的瞬时速度，而文献[22]提出的状态方程中的速度量应该是在两个连续观测时刻的平均速度。由于GPS 的采样周期较长（1 秒），所以瞬时速度与平均速度有时会相差较大，有时甚至完全失真。这样一来，引入
速度观测量非但没有起到降低动态误差的作用，反而增加了模型误差，降低了滤波精度，甚至出现抖动、滞后和发散的情况。
引入速度观测量增加了可以获得的信息，但是简单的将速度观测量加入到观测方程中去，有时并不能起到良好的效果。怎样利用好速度观测量，使之发挥更好的作用呢？本文试图换一个角度来分析。
由于GPS 定位和测速原理的不同，GPS 接收机输出的速度数据较位置数据更为准确一些。载体（即汽车）在行驶的过程中，每一时刻总是在沿着一定的方向运动，不过这个运动方向可能是在不断变化的，而且无法在事先准确得到，能够得到的只有GPS 接收机输出的载体运动信息。GPS 接收机在输出载体位置信息的同时，也输出载体速度信息，包括速度的大小和速度的方向。速度的方向和载体运动的方向必然有着密切的联系。事实上，某一时刻速度的方向，就是载体在该时刻的运动方向。虽然GPS 输出的载体运动信息是离散的，不能完全描述载体的连续运动状态，但是在缺乏载体运动方向的精确信息的情况下，可以认为载体的速度方向即为该时刻载体运动方向。
不但考虑速度数值，而且利用速度的方向，可以避免因瞬时速度与平均速度不同而引入的误差，同时又利用了速度观测量的信息。怎样来实现这一想法呢？上一章引入的约束条件的概念，在这里也可以继续应用。不过，约束条件从已知的载体运动的路线变成了载体运动的实时方向，也就是GPS 接收机输出的速度方向条件。
6.3 带速度约束条件的GPS 导航系统模型
6.3.1 状态方程的建立
状态方程的建立思路和过程与5.1.2 节基本相同，这里就不再赘述。为方便叙述，在这里只是将其列出。
6.3.2 观测方程的建立
与5.1.3 节不同，这里将只把GPS 接收机输出的位置定位数据作为观测量，而不把速度数据直接作为观测量列在观测方程里。
将GPS 接收机的输出的位置数据作为观测量
Zxe 为接收机输出的载体在东向的位置分量；Z xe 为载体在北向的位置分量。它们的值包括真实的状态值和量测误差，即：
可以看出来，本节在建立观测方程时，将观测向量从四维降为两维向量，即在观测量中只有载体位置信息，而没有直接利用到速度值的信息。
6.3.3 速度方向约束条件方程
而GPS 接收机输出的k 时刻的瞬时速度方向为d ．从日常生活经验可知，载体在运动的过程中，由于自身的机动性能的限制，其速度大小和速度方向必定是逐渐连续变化的。特别对汽车而言，由于行驶速度有限，而且在行驶中又受到道路的宽度和方向以及交通规则的限制，其速度大小和方向的变化就更加平缓一些。所以，在一定程度的近似条件下，可以用GPS接收机输出的载体瞬时速度方向d 来代替平均速度方向d 。这样，就得到了载体瞬时速度方向和载体运动轨迹之间的近似关系式
也就是说，速度方向对载体的运动轨迹有着一种约束的作用。
写成矩阵的形式为：
这样，(6.1)、(6.5)、(6.8)式在一起就构成了带速度方向约束条件的系统方程。
6.3.4 带速度方向约束条件的卡尔曼滤波算法的推导
同样，由于在模型中引入了约束方程，使得状态参数间要满足新的条件关系。原来的卡尔曼滤波方程就无法使用了。而且，由于速度方向约束条件和道路约束条件的表达形式不一致，也不能直接套用5.2 节推导出的带约束条件的卡尔曼滤波递推。
但是，和5.2 节一样，采用最小方差估计原则，在保留原有的状态参数的基础上，可以推导出带速度约束条件的卡尔曼滤波递推方程。
已知对于状态方程存在约束条件：
同5.2 节给出的带道路约束条件的卡尔曼滤波算法一样，该递推算法和一般卡尔曼滤波方程的不同之处也是在于其状态一步预测值后加了一个和约束条件有关的修正项。这个修正项的物理意义也很明显：载体运动轨迹估计值要不断做出修正，以尽量符合速度方向约束条件的要求。
6.4 实测轨迹滤波实例分析
带有速度方向约束条件的卡尔曼滤波算法，在GPS 定位信息得基础上，又利用了速度方向信息，作为一个约束条件，不断的修正GPS 定位滤波数据。虽然同样是利用了速度观测量，但是，该算法不仅利用了速度的大小值，而且还利用的是速度的方向信息作为约束条件。所以，该算法又不同于一般的带速度观测量的GPS 定位滤波算法。
因此，直观的分析，带速度方向约束条件的卡尔曼滤波算法应用于GPS 定位导航系统，处理GPS 定位数据，应该可以取得以下效果：
第一， 当载体做急转弯运动时，采用带速度观测量的卡尔曼滤波算法进行滤波处理后，滤波曲线有可能无法快速的跟踪载体的运动轨迹，出现滞后现象；而采用带约束条件的卡尔曼滤波算法后，滤波曲线应该能够较好的跟踪载体的运动轨迹。在速度方向条件的不断修正作用下，GPS 定位滤波结果应该能够较好的跟踪载体的运动轨迹。
第二， 当载体在直路或相对较直的道路上平稳运动时，由于速度方向变化不明显，使用带约束条件的卡尔曼滤波算法得到的滤波效果应该至少和一般卡尔曼滤波算法的效果一样好。
第三， 总体上看来，使用带约束条件的卡尔曼滤波算法得到的滤波效果应该比一般卡尔曼滤波算法的效果更好一些，滤波误差应该更小，滤波误差方差也应该更小。
为了检验上述想法的正确与否，本文进行了实测数据滤波分析。实验所用的接收机为美国ROCKWELL 公司的JUPITER GPS-OEM 接收板，实验资料取自2003年12 月15 日在合肥市实际跑车所纪录的数据，共2800 个历元。同时，也采用文献[22]中提出的带速度观测量的卡尔曼滤波算法同时对数据进行处理，以作为比较。
图6.1 为实地跑车测量记录的载体运动轨迹图。图中横坐标为东向，纵坐标为北向。载体从a 出发经过b、c 运动到d 处。之所以选择这样一条轨迹来进行滤波实验，是因为在这条轨迹的运动过程中，既有启动时低速行驶阶段，又有直路高速行驶阶段，还有急转弯和缓转弯行驶阶段，可以充分检验各个滤波算法的动态特性。
注意图中b、c 处为两处急转弯。下面的实验中，将会专门对几种滤波算法在这两处的滤波效果作出比较和分析。
图6.1 实测轨迹
图6.2 实测轨迹滤波误差比较
图6.2 是实测轨迹滤波误差比较图。其中，图a 是使用带速度约束条件的卡尔曼滤波算法的滤波误差曲线；图b 是使用基本卡尔曼滤波算法的滤波误差曲线。可以看出来，使用带速度约束条件的卡尔曼滤波算法的滤波误差确实比使用基本卡尔曼滤波算法的滤波误差小一些，而且更加平稳一些。图b 中多次出现的毛刺现象在图a 中也减少了许多。这确实说明，从整体上看来，使用带速度约束条件的卡尔曼滤波算法的滤波精度要更好一些。
如表6.1 可以所示，从滤波误差均值来看，使用基本卡尔曼滤波算法的滤波误差均值为0.00240,而使用带速度约束条件的卡尔曼滤波算法的滤波误差均值为0.00214,误差均值减小了11.2%。可见，使用带速度约束条件的卡尔曼滤波算法后，滤波精度的提高效果还是相当明显的。
图6.3 实测轨迹急转弯b 处的滤波曲线
图6.4 实测轨迹急转弯c 处的滤波曲线
图6.3 是实测轨迹急转弯b 处的滤波曲线。其中，“*”为真实位置点，“o”为观测位置点，虚线为基本卡尔曼滤波算法的滤波结果曲线，实线为带速度约束条件的卡尔曼滤波算法的滤波结果曲线。可以看出，带速度约束条件的卡尔曼滤波算法的滤波结果曲线较好的跟踪了真实位置点。
图6.4 是实测轨迹急转弯c 处的滤波曲线。其中，“*”为真实位置点，“o”为观测位置点，虚线为基本卡尔曼滤波算法的滤波结果曲线，实线为带速度约束条件的卡尔曼滤波算法的滤波结果曲线。同样可以看出，带速度约束条件的卡尔曼滤波算法的滤波结果曲线较好的跟踪了真实位置点。这说明，采用带速度方向约束条件的卡尔曼滤波算法后，滤波曲线能够较好的跟踪载体的运动轨迹。
6.5 增加速度观测量对速度方向约束条件算法的影响
从前面的论述中可以看到，带速度方向约束条件的算法之所以能够取得比基本卡尔曼滤波算法更好的滤波效果，就在于利用了速度方向这一约束条件，来不断地修正位置滤波结果。可是，速度方向约束条件仅仅是利用了速度的方向信息，而没有利用到速度值的大小。下面将讨论在观测方程中增加速度值观测量之后，带速度方向约束条件的滤波算法的滤波效果会有什么变化。
6.5.1 系统模型
状态方程的建立思路和过程与5.1.2 节完全相同，这里就不再赘述。为方便叙述，在这里只是将其列出。
观测方程的建立同5.1.3 相同，下面只给出观测方程，而不再讨论建立过程。
观测量选取GPS 接收机的输出信息，即载体的位置、速度信息：
其中， Z xe, Z ve 分别为接收机输出的载体在东向的位置和速度； Zxe , Z ve 为载体在北向的位置和速度。
观测方程的为：
其中，
约束条件方程以及带速度方向约束条件的卡尔曼滤波递推算法的推导和6.3 节叙述的完全相同，这里就不再赘述了。
6.5.2 实验数据分析
为了检验上述新的滤波算法，本文进行了实测数据滤波分析。实验所用的接收机为美国ROCKWELL 公司的JUPITER GPS-OEM 接收板，实验资料取自2003 年12 月15 日在合肥市实际跑车所纪录的数据，共2800 个历元。同时采用了基本卡尔曼滤波算法、带速度观测量的带速度约束条件的卡尔曼滤波算法、和上一节的不带速度观测量的带速度约束条件的卡尔曼滤波算法进行滤波处理，以作为比较。
图6.5 是实测轨迹滤波误差比较图。其中，图a 是使用基本卡尔曼滤波算法的滤波误差曲线；图b 是使用带速度观测量的带速度约束条件的卡尔曼滤波算法的滤波误差曲线；图c 是使用上一节的不带速度观测量的带速度约束条件的卡尔曼滤波算法的滤波误差曲线。
图6.5 三种滤波算法滤波误差曲线
滤波方法 滤波误差均值（单位：分）
表6.2 三种滤波算法滤波误差比较
可以看出来，图b 中使用带速度观测量的带速度约束条件的卡尔曼滤波算法的滤波误差确实比图a 中使用基本卡尔曼滤波算法的滤波误差小一些，但是和图c 中不带速度观测量的带速度约束条件的卡尔曼滤波算法的滤波误差曲线相比，效果就差一些了。
如表6.2 所示，从滤波误差均值来看，使用基本卡尔曼滤波算法的滤波误差均值为0.00240；使用带速度观测量的带速度约束条件的卡尔曼滤波算法的滤波误差均值为0.00235,误差均值减小了2.7%。使用不带速度观测量的带速度约束条件的卡尔曼滤波算法的滤波误差均值为214,误差均值减小了11.2%。
可见，使用带速度观测量的带速度约束条件的卡尔曼滤波算法后，滤波精度确实提高了，但是和不带速度观测量的带速度约束条件的卡尔曼滤波算法的提高幅度相比还是差一些的。
为了证明上述的结果是具有普遍意义的，本文又进行了大量的实验。如果把系统噪声改为正态白噪声，结果又如何呢？
图6.6 三种滤波算法滤波误差曲线
表6.3 三种滤波算法滤波误差比较
图6.6 是轨迹滤波误差比较图。其中，图a 是使用基本卡尔曼滤波算法的滤波误差曲线；图b 是使用带速度观测量的带速度约束条件的卡尔曼滤波算法的滤波误差曲线；图c 是使用上一节的不带速度观测量的带速度约束条件的卡尔曼滤波算法的滤波误差曲线。
可以看出来，图b 中使用带速度观测量的带速度约束条件的卡尔曼滤波算法的滤波误差确实比图a 中使用基本卡尔曼滤波算法的滤波误差小一些，但是和图c 中不带速度观测量的带速度约束条件的卡尔曼滤波算法的滤波误差曲线相比，效果就差一些了。
如表6.3 所示，从滤波误差均值来看，使用基本卡尔曼滤波算法的滤波误差均值为0.0007678；使用带速度观测量的带速度约束条件的卡尔曼滤波算法的滤波误差均值为0.0007446,误差均值减小了3.0%。使用不带速度观测量的带速度约束条件的卡尔曼滤波算法的滤波误差均值为0.0007110,误差均值减小了7.4%。
可见，使用带速度观测量的带速度约束条件的卡尔曼滤波算法后，滤波精度确实提高了，但是和不带速度观测量的带速度约束条件的卡尔曼滤波算法的提高幅度相比要差一些。这和使用实测数据进行滤波所得到的结果是一致的。
6.5.3 分析
为什么增加了速度观测量之后，滤波效果反而不如不加速度观测量的滤波算法呢？本文认为是这样的：
出现这种情况的原因，正是由于引入速度观测量的缘故。GPS 接收机接受到的是每一时刻载体的瞬时速度，而状态方程中的速度量应该是在两个连续观测时刻的平均速度。由于GPS 的采样周期较长（1 秒），所以瞬时速度与平均速度有时会相差较大，有时甚至完全失真。这样一来，引入速度观测量非但没有起到降低动态误差的作用，反而增加了模型误差，降低了滤波精度。
而不考虑速度数值，只利用速度的方向，可以避免因瞬时速度与平均速度不同而引入的误差，同时又利用了速度观测量的信息，所以可以相应的提高滤波精度。
6.6 速度方向可靠性的影响研究
上述带速度方向约束条件的算法之所以能够取得比基本卡尔曼滤波算法更好的滤波效果，就是因为利用了速度方向这一约束条件，不断地修正位置滤波结果。显然，该算法的效果是深受速度方向这一条件的影响的。速度方向数据的可靠性应该起了至关重要的作用。
为了验证速度方向的准确程度带速度方向约束条件的滤波精度的影响，下面人为的给速度方向加上不同程度的随机干扰因素，再进行滤波实验。实验所用的接收机为美国ROCKWELL 公司的JUPITER GPS-OEM 接收板，实验资料取自2003年12 月15 日在合肥市实际跑车所纪录的数据，共2000 个历元。
对速度方向的干扰为符号随机，大小固定的干扰。下面依次列出了当速度方向随机干扰的幅度为45 度、30 度、20 度、10 度时，三种滤波方法的滤波结果图像和数据。其中，图a 是基本卡尔曼滤波器的滤波结果，图b 是对速度方向加了随机干扰后的带速度方向约束条件的滤波结果，图c 是前述的带速度方向约束
条件的滤波结果。
可以明显看出，速度方向的准确程度对滤波结果的精确性有着显著的影响。当随即干扰大于45 度的时候，速度方向约束条件已经完全没有效果；而当速度方向的随即干扰小于20 度以后，干扰的影响就已经大大减少了；进一步，当干扰值达到10 度时，干扰的影响就已经很小了。
所以可以得出结论，带速度方向约束条件的算法之所以能够取得比基本卡尔曼滤波算法更好的滤波效果，就是因为利用了速度方向这一约束条件，不断地修正位置滤波结果。速度方向数据的可靠性对于滤波结果的精确程度有着重要的作用。
图6.7 速度方向干扰为45 度时滤波误差比较
表6.4 速度方向干扰为45 度时滤波误差比较
图6.8 速度方向干扰为30 度时滤波误差比较
表6.5 速度方向干扰为30 度时滤波误差比较
图6.9 速度方向干扰为20 度时滤波误差比较
图6.10 速度方向干扰为10 度时滤波误差比较
表6.7 速度方向干扰为10 度时滤波误差比较
6.7 小结
本章建立了带有速度方向约束条件的卡尔曼滤波算法。该算法在GPS 定位信息得基础上，又利用了速度方向信息，作为一个约束条件，不断的修正GPS 定位滤波数据。通过实测轨迹滤波实验分析，可以得出以下结论：
第一， 当载体做急转弯运动时，采用带速度观测量的卡尔曼滤波算法进行滤波处理后，滤波曲线有可能无法快速的跟踪载体的运动轨迹，出现滞后现象；而采用带约束条件的卡尔曼滤波算法后，滤波曲线能够较好的跟踪载体的运动轨迹。
第二， 总体上看来，使用带约束条件的卡尔曼滤波算法得到的滤波效果比一般卡尔曼滤波算法的效果更好一些，滤波误差更小，滤波误差方差也更小。
由于速度方向约束条件仅仅是利用了速度的方向信息，而没有利用到速度值的大小。所以，本章接着又讨论了带速度值观测量的带速度方向约束条件的滤波算法。
实验证明，在观测方程中增加速度值观测量之后，带速度方向约束条件的滤波算法的滤波精度和基本卡尔曼滤波算法的滤波精度相比确实提高了，但是和不带速度观测量的带速度约束条件的卡尔曼滤波算法的提高幅度相比却要差一些。本文认为出现这种情况的原因，正是由于引入速度观测量的缘故。GPS 接收机接受到的每一时刻载体的瞬时速度，和状态方程中的两个连续观测时刻的平均速度的差别增加了模型误差，降低了滤波精度。
