航位推算和全球定位系统的组合定位方法
【专利摘要】本发明公开了一种航位推算和全球定位系统的组合定位方法，包括初始化组合定位参量以待用于DR系统、GPS定位以判断是否启用DR系统以及DR和GPS组合定位。在DR和GPS组合定位中，DR系统输出的车辆的位置和速度与GPS接收机输出的位置和速度的差值被输入卡尔曼滤波器以获取校正值，通过该校正值和DR系统输出的位置和速度获取组合定位的位置和速度。本发明可以有效解决城市道路复杂环境下单GPS系统定位不连续不可靠的问题，本发明只需要使用两个里程计而不需要使用陀螺仪加速度计等惯性传感器辅助，就能实时校正DR系统的输出，使其不累积误差，本发明的实现方法简便可靠。
【专利说明】航位推算和全球定位系统的组合定位方法
【技术领域】
[0001] 本发明涉及车辆定位技术，尤其涉及一种航位推算和全球定位系统的组合定位方 法。
【背景技术】
[0002] 车辆定位技术是实现行车路线引导的关键技术，GPS(全球定位系统）凭借其定 位迅速、准确，以及在无遮挡情况下可实现全天候定位的特点，成为当前首选的车辆定位技 术。但是，在当今的城市道路环境下，单独使用GPS导航定位系统的车辆信号经常会受到高 楼、树木、高架桥等物体的遮挡，导致其定位不连续以及定位精度低等问题。因此GPS导航 定位系统需要和其他导航系统进行组合，以获得较高的导航定位质量。
[0003] 在常与GPS导航定位系统组合的导航系统中，惯性导航系统由于惯性测量单元 价格较高，实现方法复杂，因此被较少应用。经常和GPS组合应用在车载组合导航中的是 DR(航道推算）导航定位方式，其通常的做法有三种：1、一个里程计加一个单轴陀螺仪；2、 一个单轴陀螺仪加一个单轴加速度计；3、两个里程计差分。由于前两种方案都需要用到惯 性传感器，而目前可用于车辆导航的惯性传感器价格昂贵，且相应数据处理方法复杂，不适 用于车载DR导航定位。另外，由于DR导航系统是基于位置推测的，其误差会随时间累积。
[0004] 因此，本领域的技术人员致力于开发一种航位推算和全球定位系统的组合定位方 法，无需使用惯性传感器，且能消除其中DR导航的随时间累积的误差。
【发明内容】
[0005] 有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是提供一种航位推算和 全球定位系统的组合定位方法，消除其中DR导航的随时间累积的误差。
[0006] 为实现上述目的，本发明提供了一种航位推算和全球定位系统的组合定位方法， 用于对车辆导航定位，其特征在于，包括：
[0007] 步骤100,初始化组合定位参量并将所述参量输入微处理器,所述参量包括所述车 辆的车轮的半径和后轮间距；
[0008] 步骤200，GPS定位，包括：
[0009] 启动GPS接收机；当所述GPS接收机的GPS定位成功，且当所述车辆的速度大于设 定值时，进入步骤300 ;
[0010] 步骤300, DR和GPS组合定位，包括：
[0011] 步骤310,所述微处理器将所述步骤100中获取的所述参量输入所述DR系统，所述 微处理器将完成所述步骤200后的所述GPS接收机输出的所述车辆的位置和速度输入所述 DR系统；
[0012] 步骤320,所述微处理器将所述DR系统接收的所述车辆的位置和速度作为所述DR 系统采用的初始位置和初始速度，对所述DR系统进行初始化；所述微处理器启动DR系统；
[0013] 步骤330,所述微处理器指令所述DR系统工作，所述DR系统输出所述车辆的位置 和速度；
[0014] 步骤340,所述微处理器将所述DR系统输出的所述车辆的位置和速度以及所述 GPS接收机输出的所述车辆的速度和位置输入卡尔曼滤波器以对所述DR系统输出的所述 位置和速度进行校正；
[0015] 步骤350,所述微处理器根据所述卡尔曼滤波器输出的所述车辆的位置和速度的 校正量以及所述DR系统输出的所述车辆的位置和速度，计算获得所述车辆的组合定位的 位置和速度，并将所述的组合定位的位置和速度输出；
[0016] 步骤360,所述微处理器将所述组合定位的位置和速度输入所述DR系统，进入步 骤 320。
[0017] 进一步地，所述DR系统和所述卡尔曼滤波器在所述微处理器中，所述GPS接收机 通过串口连接到所述微处理器，所述车辆包括左轮里程计和右轮里程计分别连接到所述微 处理器，所述左轮里程计与所述微处理器间的通信通过外部中断1触发，所述右轮里程计 与所述微处理器间的通信通过外部中断2触发。
[0018] 进一步地，所述左轮是左后轮，所述右轮是右后轮。
[0019] 进一步地，所述步骤200包括：
[0020] 步骤210, GPS接收机接收卫星信号；
[0021] 步骤220,所述微处理器判断GPS接收机是否定位成功，如果判断为是，则进入步 骤230,如判断为否，则进入步骤210 ;
[0022] 步骤230,所述GPS接收机输出所述车辆的位置和速度，所述微处理器根据所述 GPS输出的所述车辆的速度以及所述左轮里程计和所述右轮里程计输出的脉冲频率判断所 述车辆的速度是否大于所述设定值，如果判断为是，则进入所述步骤310,如果判断为否，则 进入步骤210。
[0023] 进一步地，所述GPS接收机输出的所述车辆的位置包括经度、纬度和高度，所述DR 系统输出的所述车辆的位置包括经度、纬度和高度，所述GPS接收机输出的所述车辆的速 度包括东向速度、北向速度和偏航角，所述DR系统输出的所述车辆的速度包括东向速度、 北向速度和偏航角。
[0024] 进一步地，所述DR系统基于DR算法工作，所述DR算法根据所述车辆的车轮的半 径和后轮间距、初始位置和初始速度以及行驶中的所述车辆的所述左轮和所述右轮在一个 计算周期内通过的距离来获得所述车辆在所述一个计算周期末的位置和速度。
[0025] 进一步地，所述车辆的所述左轮在一个计算周期内通过的距离由所述左轮里程计 输出，所述车辆的所述右轮在一个计算周期内通过的距离由所述右轮里程计输出。
[0026] 进一步地，所述步骤340包括：
[0027] 步骤341，所述微处理器判断GPS接收机是否定位成功，如果判断为是，则进入步 骤342,如判断为否，则进入步骤343 ;
[0028] 步骤342,所述微处理器将所述DR系统输出的所述车辆的位置和速度与所述GPS 输出的所述车辆的位置和速度相减以获得差值，并将所述差值作为所述卡尔曼滤波器的量 测值；进入步骤344 ;
[0029] 步骤343,所述微处理器将一个零向量作为所述卡尔曼滤波器的量测值；进入步 骤 344 ;
[0030] 步骤344,所述卡尔曼滤波器依次使用卡尔曼滤波的状态方程和卡尔曼滤波的量 测方程以对所述DR系统输出的所述位置和速度进行校正；
[0031] 所述卡尔曼滤波的状态方程为：X⑴=F〇〇A^/)+W⑴：其中，t为时间，系统状态 转移矩阵F(t)是一个单位矩阵，系统过程噪声序列W(t)是一个均值为0、方差为Q(t)、呈 正态分布的函数，状态向量X(t)为所述量测值；当所述步骤341的判断为是的时候，状态向 量X= [Alon Alat Ahed AVn AVe]T中的Z Ion为所述GPS接收机输出的所述车辆的 经度和所述DR系统输出的所述车辆的经度之间的差值，」Iat为所述GPS接收机输出的所 述车辆的纬度和所述DR系统输出的所述车辆的纬度之间的差值，」hed为所述GPS接收机 输出的所述车辆的偏航角和所述DR系统输出的所述车辆的偏航角之间的差值，Z Vn为所 述GPS接收机输出的所述车辆的北向速度和所述DR系统输出的所述车辆的北向速度之间 的差值，」Ve为所述GPS接收机输出的所述车辆的东向速度和所述DR系统输出的所述车 辆的东向速度之间的差值；当所述步骤341的判断为否的时候，状态向量X= [0 0 0 0 0] T
[0032] 所述卡尔曼滤波的观测方程为：F(?)=X(〇=C(dYV)+V(t);其中，观测矩阵C(t) 为一个单位矩阵，测量噪声序列V(t)为一个均值为0、方差为R(t)、呈正态分布的函数；当 所述步骤341的判断为是的时候，观测向量Y (t)取为：
[0033] yk= [ A Ion A Iat Ahed AVn AVe]T ;当所述步骤341的判断为否的时候，观测 向量 Y(t)取为：yk = [0 0 0 0 0]T ;
[0034] 所述为中间量，所述Y(t)为所述车辆的位置和速度的校正量。
[0035] 进一步地，」Ion等于所述DR系统输出的所述车辆的经度减去所述GPS接收机输 出的所述车辆的经度，」Iat等于所述DR系统输出的所述车辆的纬度减去所述GPS接收机 输出的所述车辆的纬度，」hed等于所述DR系统输出的所述车辆的偏航角减去所述GPS接 收机输出的所述车辆的偏航角，」Vn等于所述DR系统输出的所述车辆的北向速度减去所 述GPS接收机输出的所述车辆的北向速度，」Ve等于所述DR系统输出的所述车辆的东向 速度减去所述GPS接收机输出的所述车辆的东向速度。
[0036] 进一步地，所述车辆的组合定位的位置和速度分别地等于所述DR系统输出的所 述车辆的位置和速度分别地减去所述卡尔曼滤波器输出的所述车辆的位置和速度的校正 量。
[0037] 在本发明的较佳实施方式中，应用了上述的组合定位方法，其在GPS定位无效的 时候，依然能较好的体现运行中的车辆的真实位置。
[0038] 本发明的航位推算和全球定位系统的组合定位方法具有以下优点：
[0039] 1)相比于单独的GPS导航定位系统，本方法只额外使用了两个里程计（左轮里程 计和右轮里程计），用来作为DR系统的信号输入；相比于常与GPS导航定位系统组合的导 航系统，本发明无需用到陀螺仪加速度计等惯性器件，处理方法简洁高效可靠；
[0040] 2)本发明中启动DR系统的方式能确保DR系统初始化对准快速、正确；
[0041] 3)本发明的工作机制清晰，便于维护。在GPS定位不正常的情况下，其仍然可以在 较长时间内保持有效定位；
[0042] 4)本发明能实时校正DR系统，使其随时间产生的误差不被累积；
[0043] 5)本发明成本较低。
[0044] 以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以 充分地了解本发明的目的、特征和效果。
【专利附图】
【附图说明】
[0045] 图1是本发明的航位推算和全球定位系统的组合定位方法的流程图。
[0046] 图2是本发明中的卡尔曼滤波器的工作原理框图。
[0047] 图3是在一个较佳实施例中，实现本发明的航位推算和全球定位系统的组合定位 方法的系统结构框图。
[0048] 图4显示了图3所示的系统实施本发明的航位推算和全球定位系统的组合定位方 法的仿真结果。
【具体实施方式】
[0049] 图1示出了本发明的航位推算和全球定位系统的组合定位方法的流程图，在本实 施例中，其应用于如图3所示的系统。该系统包括微处理器、GPS接收机和两个里程计（左 轮里程计及右轮里程计），其中微处理器中有DR系统和所述卡尔曼滤波器，GPS接收机通 过串口连接到微处理器，左轮里程计和右轮里程计分别连接到微处理器，左轮里程计与微 处理器间的通信通过外部中断1触发，右轮里程计与微处理器间的通信通过外部中断2触 发。其中左轮是车辆的左后轮，右轮是车辆的右后轮，左轮里程计及右轮里程计分别计量左 轮和右轮经过的里程。
[0050] 本发明的航位推算和全球定位系统的组合定位方法具体地为：
[0051] 步骤100,初始化组合定位参量并将这些参量输入微处理器。这些参量是与DR系 统的工作相关的常数，包括车辆的车轮的半径和后轮间距。
[0052] 本实施例中，车辆的车轮的半径为0. 33米，后轮间距为1. 55米。
[0053] 步骤 200, GPS 定位。
[0054] 首先不启动DR系统，而进行GPS独立的定位，具体地包括步骤210、220和230 :
[0055] 步骤210, GPS接收机接收卫星信号。这是GPS接收机的工作方式，在此不赘述。
[0056] 步骤220,微处理器判断GPS接收机是否定位成功，如果判断为是，则进入步骤 230,如判断为否，则进入步骤210 ;
[0057] 步骤230, GPS接收机输出车辆的位置和速度，微处理器根据GPS输出的车辆的速 度以及左轮里程计和右轮里程计输出的脉冲频率判断该车辆的速度是否大于设定值，如果 判断为是，则进入所述步骤310,如果判断为否，则进入步骤210。
[0058] 这里的设定值，是启动DR系统的最小车速，即只有当车辆的速度到达一定值时， 才启动DR系统，否则还是保持GPS的独立定位。一般地，该设定值取为10Km/h。
[0059] 步骤300, DR和GPS组合定位，其具体地包括步骤310、320、330、340、350和360。
[0060] 步骤310,微处理器将步骤100中获取的那些参量输入DR系统，微处理器将完成步 骤200后的GPS接收机输出的车辆的位置和速度输入DR系统。
[0061] GPS接收机输出的车辆的位置包括经度、纬度和高度，GPS接收机输出的车辆的速 度包括东向速度、北向速度和偏航角。
[0062] 步骤320,微处理器将DR系统接收的车辆的位置和速度作为DR系统采用的初始位 置和初始速度，对DR系统进行初始化；微处理器启动DR系统。
[0063] 步骤330,微处理器指令DR系统工作，DR系统输出车辆的位置和速度。
[0064] 本步骤中，DR系统基于DR算法工作，其根据车辆的车轮的半径和后轮间距、初始 位置和初始速度以及行驶中的车辆的左轮和右轮在一个计算周期内通过的距离来获得所 述车辆在这个计算周期末的位置和速度并将该位置和速度输出。其中，车辆的左轮在一个 计算周期内通过的距离由左轮里程计输出，车辆的右轮在一个计算周期内通过的距离由右 轮里程计输出。
[0065] 计算周期可以设定，本实施例中选用的计算周期为1秒。
[0066] DR系统输出的车辆的位置包括经度、纬度和高度，DR系统输出的车辆的速度包括 东向速度、北向速度和偏航角。
[0067] 步骤340,微处理器将DR系统输出的车辆的位置和速度以及GPS接收机输出的车 辆的速度和位置输入卡尔曼滤波器以对DR系统输出的位置和速度进行校正，具体地包括 步骤 341、342、343 和 344。
[0068] 步骤341，微处理器判断GPS接收机是否定位成功，如果判断为是，则进入步骤 342,如判断为否，则进入步骤343。
[0069] 步骤342,微处理器将DR系统输出的车辆的位置和速度与GPS输出的车辆的位置 和速度相减以获得差值，并将该差值作为卡尔曼滤波器的量测值；进入步骤344。
[0070] 参见图2所示的卡尔曼滤波器的工作原理框图，本实施例中是用DR系统输出的车 辆的位置和速度分别地减去GPS输出的车辆的位置和速度，由此获得差值」Ion、」lat、 」hed、」Vn和」Ve，其中，」Ion等于DR系统输出的车辆的经度减去GPS接收机输出的车 辆的经度，」Iat等于DR系统输出的车辆的纬度减去GPS接收机输出的车辆的纬度，」hed 等于DR系统输出的车辆的偏航角减去GPS接收机输出的车辆的偏航角，」Vn等于DR系统 输出的车辆的北向速度减去GPS接收机输出的车辆的北向速度，」Ve等于DR系统输出的 车辆的东向速度减去GPS接收机输出的车辆的东向速度。由此，卡尔曼滤波器的量测值为 状态向量 X= [Alon Alat Ahed AVn AVe]T。
[0071] 步骤343,微处理器将一个零向量作为卡尔曼滤波器的量测值；进入步骤344。即 认为GPS接收机与DR系统具有相同的输出，由此卡尔曼滤波器的量测值为状态向量X = [Alon Alat Ahed AVn AVe]1= [0 0 0 0 0]T。
[0072] 步骤344,卡尔曼滤波器依次使用卡尔曼滤波的状态方程和卡尔曼滤波的量测方 程以对所述DR系统输出的所述位置和速度进行校正。
[0073] 首先使用卡尔曼滤波的状态方程：X⑴⑴+W⑴，式中，t为时间，系统状 态转移矩阵F(t)是一个单位矩阵，系统过程噪声序列W(t)是一个均值为0、方差为Q(t)、 呈正态分布的函数。这里；⑴为中间量，可以认为它是对X(t)的估值。本实施例中，方差 Q(t)被设置为0.2乘以单位矩阵。
[0074] 然后使用卡尔曼滤波的观测方程为：yy'j=；n力⑴，式中，观测向量 Y(t)取为：yk=[Al0n Alat Ahed AVn AVe]T，观测矩阵C(t)为一个单位矩阵，测量 噪声序列v(t)为一个均值为0、方差为R(t)、呈正态分布的函数。本实施例中，方差R(t) 被设置为0.4乘以单位矩阵。
[0075] 卡尔曼滤波器输出的Y(t)为车辆的位置和速度的校正量。
[0076] 步骤350,微处理器根据卡尔曼滤波器输出的车辆的位置和速度的校正量以及DR 系统输出的车辆的位置和速度，计算获得车辆的组合定位的位置和速度，并将该组合定位 的位置和速度输出。
[0077] 如图2所示，本实施例中车辆的组合定位的位置和速度分别地等于DR系统输出的 车辆的位置和速度分别地减去卡尔曼滤波器输出的车辆的位置和速度的校正量。
[0078] 步骤360,微处理器将上述组合定位的位置和速度输入DR系统，进入步骤320。由 此DR系统开始对下一个计算周期末的车辆的位置和速度进行推算。
[0079] 本实施例通过使用Spirent公司生产的模拟器并编辑一段车辆行驶轨迹来模拟 车辆运行，模拟器在运行编辑好的轨迹时可以给出每一时刻标准的经、纬度，同时输出相对 应的卫星信号以及车辆左右后轮的里程计信号。在运行场景时，人为地在一些时间段切断 GPS信号，使用本发明的航位推算和全球定位系统的组合定位方法进行组合导航计算，同时 将标准位置信息、GPS接收机输出的位置信息和本发明组合导航计算得到的位置信息绘制 于图4。由图4可以看出在GPS定位无效的时候，本发明依然能较好的体现车辆的真实位 置。
[0080] 以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无 需创造性劳动就可以根据本发明的构思做出诸多修改和变化。因此，凡本【技术领域】的技术 人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的 技术方案，皆应在由权利要求书所确定的保护范围内。
【权利要求】
1. 一种航位推算和全球定位系统的组合定位方法，用于对车辆导航定位，其特征在于， 包括： 步骤100,初始化组合定位参量并将所述参量输入微处理器，所述参量包括所述车辆的 车轮的半径和后轮间距； 步骤200, GPS定位，包括： 启动GPS接收机；当所述GPS接收机的GPS定位成功，且当所述车辆的速度大于设定值 时，进入步骤300 ; 步骤300, DR和GPS组合定位，包括： 步骤310,所述微处理器将所述步骤100中获取的所述参量输入所述DR系统，所述微 处理器将完成所述步骤200后的所述GPS接收机输出的所述车辆的位置和速度输入所述DR 系统； 步骤320,所述微处理器将所述DR系统接收的所述车辆的位置和速度作为所述DR系统 采用的初始位置和初始速度，对所述DR系统进行初始化；所述微处理器启动DR系统； 步骤330,所述微处理器指令所述DR系统工作，所述DR系统输出所述车辆的位置和速 度； 步骤340,所述微处理器将所述DR系统输出的所述车辆的位置和速度以及所述GPS接 收机输出的所述车辆的速度和位置输入卡尔曼滤波器以对所述DR系统输出的所述位置和 速度进行校正； 步骤350,所述微处理器根据所述卡尔曼滤波器输出的所述车辆的位置和速度的校正 量以及所述DR系统输出的所述车辆的位置和速度，计算获得所述车辆的组合定位的位置 和速度，并将所述的组合定位的位置和速度输出； 步骤360,所述微处理器将所述组合定位的位置和速度输入所述DR系统，进入步骤 320。
2. 如权利要求1所述的航位推算和全球定位系统的组合定位方法，其中所述DR系统和 所述卡尔曼滤波器在所述微处理器中，所述GPS接收机通过串口连接到所述微处理器，所 述车辆包括左轮里程计和右轮里程计分别连接到所述微处理器，所述左轮里程计与所述微 处理器间的通信通过外部中断1触发，所述右轮里程计与所述微处理器间的通信通过外部 中断2触发。
3. 如权利要求2所述的航位推算和全球定位系统的组合定位方法，其中所述左轮是左 后轮，所述右轮是右后轮。
4. 如权利要求3所述的航位推算和全球定位系统的组合定位方法，其中所述步骤200 包括： 步骤210, GPS接收机接收卫星信号； 步骤220,所述微处理器判断GPS接收机是否定位成功，如果判断为是，则进入步骤 230,如判断为否，则进入步骤210 ; 步骤230,所述GPS接收机输出所述车辆的位置和速度，所述微处理器根据所述GPS输 出的所述车辆的速度以及所述左轮里程计和所述右轮里程计输出的脉冲频率判断所述车 辆的速度是否大于所述设定值，如果判断为是，则进入所述步骤310,如果判断为否，则进入 步骤210。
5. 如权利要求3或4所述的航位推算和全球定位系统的组合定位方法，其中所述GPS 接收机输出的所述车辆的位置包括经度、纬度和高度，所述DR系统输出的所述车辆的位置 包括经度、纬度和高度，所述GPS接收机输出的所述车辆的速度包括东向速度、北向速度和 偏航角，所述DR系统输出的所述车辆的速度包括东向速度、北向速度和偏航角。
6. 如权利要求5所述的航位推算和全球定位系统的组合定位方法，其中所述DR系统基 于DR算法工作，所述DR算法根据所述车辆的车轮的半径和后轮间距、初始位置和初始速度 以及行驶中的所述车辆的所述左轮和所述右轮在一个计算周期内通过的距离来获得所述 车辆在所述一个计算周期末的位置和速度。
7. 如权利要求6所述的航位推算和全球定位系统的组合定位方法，其中所述车辆的所 述左轮在一个计算周期内通过的距离由所述左轮里程计输出，所述车辆的所述右轮在一个 计算周期内通过的距离由所述右轮里程计输出。
8. 如权利要求6或7所述的航位推算和全球定位系统的组合定位方法，其中所述步骤 340包括： 步骤341，所述微处理器判断GPS接收机是否定位成功，如果判断为是，则进入步骤 342,如判断为否，则进入步骤343 ; 步骤342,所述微处理器将所述DR系统输出的所述车辆的位置和速度与所述GPS输 出的所述车辆的位置和速度相减以获得差值，并将所述差值作为所述卡尔曼滤波器的量测 值；进入步骤344 ; 步骤343,所述微处理器将一个零向量作为所述卡尔曼滤波器的量测值；进入步骤 344 ； 步骤344,所述卡尔曼滤波器依次使用卡尔曼滤波的状态方程和卡尔曼滤波的量测方 程以对所述DR系统输出的所述位置和速度进行校正； 所述卡尔曼滤波的状态方程为：i(t)=F(t)；T(/)+W(t);其中，t为时间，系统状态转移 矩阵F(t)是一个单位矩阵，系统过程噪声序列W(t)是一个均值为0、方差为Q(t)、呈正态 分布的函数，状态向量X(t)为所述量测值；当所述步骤341的判断为是的时候，状态向量X =[Alon Alat Ahed AVn AVe]1中的Zl Ion为所述GPS接收机输出的所述车辆的经 度和所述DR系统输出的所述车辆的经度之间的差值，」Iat为所述GPS接收机输出的所述 车辆的纬度和所述DR系统输出的所述车辆的纬度之间的差值，」hed为所述GPS接收机输 出的所述车辆的偏航角和所述DR系统输出的所述车辆的偏航角之间的差值，」Vn为所述 GPS接收机输出的所述车辆的北向速度和所述DR系统输出的所述车辆的北向速度之间的 差值，」Ve为所述GPS接收机输出的所述车辆的东向速度和所述DR系统输出的所述车辆 的东向速度之间的差值；当所述步骤341的判断为否的时候，状态向量X= [0 0 0 0 0]T; 所述卡尔曼滤波的观测方程为：其中，观测矩阵C(t)为一 个单位矩阵，测量噪声序列V(t)为一个均值为0、方差为R(t)、呈正态分布的函数；当所述 步骤341的判断为是的时候，观测向量Y(t)取为：yk= [Alon Alat Ahed AVn AVe]T ;当所述步骤341的判断为否的时候，观测向量Y(t)取为：yk= [0 0 0 0 0]T; 所述为中间量，所述Y(t)为所述车辆的位置和速度的校正量。
9. 如权利要求8所述的航位推算和全球定位系统的组合定位方法，其中Z Ion等于所 述DR系统输出的所述车辆的经度减去所述GPS接收机输出的所述车辆的经度，」Iat等 于所述DR系统输出的所述车辆的纬度减去所述GPS接收机输出的所述车辆的纬度，」hed 等于所述DR系统输出的所述车辆的偏航角减去所述GPS接收机输出的所述车辆的偏航角， 」Vn等于所述DR系统输出的所述车辆的北向速度减去所述GPS接收机输出的所述车辆的 北向速度，」Ve等于所述DR系统输出的所述车辆的东向速度减去所述GPS接收机输出的 所述车辆的东向速度。
10. 如权利要求9所述的航位推算和全球定位系统的组合定位方法，其中所述车辆的 组合定位的位置和速度分别地等于所述DR系统输出的所述车辆的位置和速度分别地减去 所述卡尔曼滤波器输出的所述车辆的位置和速度的校正量。
【文档编号】G01S19/48GK104360366SQ201410618043
【公开日】2015年2月18日 申请日期:2014年11月5日 优先权日:2014年11月5日
【发明者】王励扬, 何文涛, 马成炎, 蔺晓龙, 翟昆朋, 胡晓峰, 王浩 申请人:中国科学院嘉兴微电子与系统工程中心