CN109900271B - 地形匹配定位中有效伪波峰估计与多点融合初始定位方法 - Google Patents
地形匹配定位中有效伪波峰估计与多点融合初始定位方法 Download PDFInfo
- Publication number
- CN109900271B CN109900271B CN201910136987.2A CN201910136987A CN109900271B CN 109900271 B CN109900271 B CN 109900271B CN 201910136987 A CN201910136987 A CN 201910136987A CN 109900271 B CN109900271 B CN 109900271B
- Authority
- CN
- China
- Prior art keywords
- positioning
- point
- terrain matching
- effective
- matching positioning
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了地形匹配定位中有效伪波峰估计与多点融合初始定位方法,包括以下步骤:步骤一:获得N个地形匹配定位点的似然函数;步骤二:计算每一个地形匹配定位点的置信区间;步骤三:查找地形匹配定位的有效峰值点以及有效峰值点的定位误差;步骤四:确定地形匹配融合定位的组合形式,计算每一个组合形式下的多点融合定位结果;步骤五:得到NU个地形匹配定位点融合定位结果;步骤六:计算每一个定位结果的代价函数,找到代价函数最小值对应的q就是最优的定位结果对应的索引序号,到此算法执行完毕,定位点确定。本发明的有益效果为:能够针对地形匹配定位结果连续的出现多伪波峰和误匹配的情况下提高地形匹配定位的精度。
Description
技术领域
本发明涉及一种地形匹配定位方法,具体涉及地形匹配定位中有效伪波峰估计与多点融合初始定位方法。
背景技术
地形匹配定位的初始阶段往往存在很大的初始定位误差,导致初始阶段的地形匹配定位需要很大的初始搜索区间,从而很容易导致初始阶段的地形匹配定位结果出现误匹配和多伪波峰现象,地形匹配定位的结果十分的不稳定,并且存在很强的跳变性。如何在地形匹配定位结果存在较多的伪波峰和误匹配现象时能够获得较稳定的定位结果是地形匹配导航领域的难点。
发明内容
针对现有技术上存在的不足,本发明的目的在于提出一种地形匹配定位的高精度与高稳定性定位方法,主要用于地形匹配定位结果连续的出现多伪波峰和误匹配的情况下提高地形匹配定位的精度。
为了实现上述目的,本发明是通过如下的技术方案来实现:
地形匹配定位中有效伪波峰估计与多点融合初始定位方法,包括以下步骤:
步骤一:获得N个地形匹配定位点的似然函数,并得到的每一个地形匹配定位点的参考导航定位点和参考导航的定位误差;
步骤二:根据步骤一得到的N个地形匹配定位点的似然函数计算其极大似然值的置信度下确界平面,并以此平面截取似然函数曲面获得得等值线即为置信区间边界;由此可得,每一个地形匹配定位点的置信区间,且在搜索区间内的搜索点被置信区间分割为两部分,其中搜索区间是由参考导航系统的误差得到,位于地形匹配定位置信区间内部的搜索点为有效的地形匹配定位点,即地形匹配定位有效定位点;
步骤三:根据步骤二得到的地形匹配定位置信区间和地形匹配定位有效定位点计算地形匹配定位的有效峰值点以及有效峰值点的定位误差;
步骤四:根据步骤三得到的地形匹配定位点的有效峰值点、有效峰值点的似然函数值以及有效峰值点的定位误差来确定地形匹配融合定位的组合形式,计算每一个组合形式下的多点融合定位结果;
步骤五:根据步骤一得到的每一个地形匹配定位点的参考导航定位点和参考导航的定位误差,步骤三得到的有效峰值点以及有效峰值点的定位误差,再利用步骤四得到的索引矩阵进行每一个有效峰值点组合的定位结果解算,一共能够得到NU个地形匹配定位点融合定位结果;
步骤六:根据步骤五中的地形匹配定位多点融合定位结果计算其中每一个定位结果的代价函数Jq,q=1,2,3...NU,q表示由所有的地形匹配定位点的有效峰值点所组成的多地形匹配定位点融合组合的索引号,设第q个组合得到的定位修正量为 中mtp表示该定位偏差是由多地形匹配定位算法获取的;找到Jq最小值对应的q就是最优的定位结果对应的索引序号,此时索引号为q的定位修正量即为最优的定位修正量,到此算法执行完毕,多地形匹配定位点融合定位确定。
上述的地形匹配定位中有效伪波峰估计与多点融合初始定位方法,所述步骤一中似然函数的具体获得方法如下:
假设在一个航迹段内规划了N个地形匹配定位点,并且已经获得了N个地形匹配定位结果,每一个地形匹配定位点对应一个参考导航定位点,同时得到每一个地形匹配定位点的似然函数,再同时记录下每一个地形匹配定位点对应的参考导航定位点Dk和参考导航的定位误差Rk,k表示规划的地形匹配定位点索引号。
上述的地形匹配定位中有效伪波峰估计与多点融合初始定位方法,所述步骤三中有效峰值点的计算方法如式(1)中所示,Si,j表示任意一个搜索点,C表示地形匹配定位的置信区间,式(1)中Li,j表示索引号为(i,j)的地形匹配定位有效定位点;式(1)表示有效定位点的似然函数值比他周围的其他点的似然值高,则表示该点为有效峰值点;找到有效峰值点之后,计算有效峰值点的地形匹配定位误差,假设第k个地形匹配定位点的索引号为有效峰值点,其地形匹配定位误差是按照这种方法能够计算所述步骤一得到的所有N个地形匹配定位点的有效峰值点、有效峰值点的似然函数值以及有效峰值点的定位误差;
上述的地形匹配定位中有效伪波峰估计与多点融合初始定位方法,所述步骤四中每一个组合形式下的多点融合定位结果的具体计算方法如下:具体计算方法如下:
假设一共有N个地形匹配定位点,索引号以k表示,每一个地形匹配定位点对应的有效峰值点的总数为mk,则组合模式的总数NU可以由下式得到:
然后是生成索引,根据式(2)的计算结果能够看到一共可以有NU种组合模式。
上述的地形匹配定位中有效伪波峰估计与多点融合初始定位方法,生成组合模式时对于每一个地形匹配定位点的有效峰值的具体索引方法如下:
根据步骤四中的假设一共有N个地形匹配定位点,每一个地形匹配定位点的有效峰值点为mk个,k表示地形匹配定位点的索引号,则多点融合地形匹配定位中对定位点的有效峰值点的索引矩阵A的大小是NU×N,第k个地形匹配定位点的索引顺序由A的第k列确定,索引矩阵A的任意一个列的形式可以有下述的方法确定:
对于k,1<k<N列向量的形式为:
其中,每一个编号复制IMk-1次,每一个序列复制Mk-1次;
对于k=1的情况,索引矩阵的列向量为:
其中,每一个编号复制IM2次;
对于k=N的情况,索引矩阵的列向量为:
其中,每一个序列复制MN-1次;
通过上述四步能够获得NU×N的有效峰值点索引矩阵。
上述的地形匹配定位中有效伪波峰估计与多点融合初始定位方法,所述步骤五中NU个地形匹配定位点融合定位结果的具体计算方法为:
设第q个组合得到的定位修正量为其计算方法如式(3),式(3)中表示第k个地形匹配定位点位置的第l号有效峰值点与第k个地形匹配定位点的参考导航定位点之间的距离,λk表示权重系数,它与参考导航的定位误差以及有效峰值点的定位误差有关,即为待求解的定位修正量;
本发明的有益效果为:本发明提出的地形匹配定位高精度与高稳定性定位方法,可针对地形匹配定位结果连续的出现多伪波峰和误匹配的情况下提高地形匹配定位的精度。
附图说明
下面结合附图和具体实施方式来详细说明本发明:
图1为本发明地形匹配导航的规划路径与规划匹配定位点;
图2为本发明地形匹配定位结果的示意图;
图3为本发明地形匹配定位点的置信区间和有效峰值点。
图中附图标记的含义:
1、一个航迹段,2、参考导航定位点,3、地形匹配定位点,4、搜索区间,5、置信区间,6、搜索点,7、有效峰值点。
具体实施方式
为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解,下面结合具体实施方式,进一步阐述本发明。
如图1至图3所示,本发明的地形匹配定位中有效伪波峰估计与多点融合初始定位方法,包括以下步骤:
步骤一:获得N个地形匹配定位点3的似然函数,并得到的每一个地形匹配定位点3的参考导航定位点2和参考导航的定位误差,具体获得方法如下:
如图1所示,假设在一个航迹段内规划了N个地形匹配定位点3,并且已经获得了N个地形匹配定位结果,每一个地形匹配定位点3对应一个参考导航定位点2,同时得到每一个地形匹配定位点3的似然函数,再同时记录下每一个地形匹配定位点3对应的参考导航定位点Dk和参考导航的定位误差Rk,k表示规划的地形匹配定位点索引号。
步骤二:根据步骤一得到的N个地形匹配定位点3的似然函数计算每一个地形匹配定位点3的置信区间5,计算其极大似然值的置信度下确界平面,并以此平面截取似然函数曲面获得等值线即为置信区间5的边界;由此可得,每一个地形匹配定位点的置信区间5,如图2所示,在搜索区间4内的搜索点6被置信区间5分割为两部分,位于置信区间5内部的搜索点6为有效的地形匹配定位点3,即地形匹配定位有效定位点;
步骤三:根据步骤二得到的地形匹配定位置信区间5和地形匹配定位有效定位点计算地形匹配定位的有效峰值点7以及有效峰值点7的定位误差,具体的计算方法如式(1)中所示,Si,j表示任意一个搜索点6,C表示地形匹配定位的置信区间5,式(1)中Li,j表示索引号为(i,j)的地形匹配定位有效定位点;式(1)表示有效定位点的似然函数值比他周围的其他点的似然值高,则表示该点为有效峰值点7;找到有效峰值点7之后,计算有效峰值点7的地形匹配定位误差,假设第k个地形匹配定位点3的索引号为有效峰值点7,其地形匹配定位误差是按照这种方法能够计算步骤一得到的所有N个地形匹配定位点3的有效峰值点7、有效峰值点7的似然函数值以及有效峰值点7的定位误差;
步骤四:根据步骤三得到的地形匹配定位点3的有效峰值点7、有效峰值点7的似然函数值以及有效峰值点7的定位误差来确定地形匹配融合定位的组合形式,计算每一个组合形式下的多点融合定位结果;
步骤四中每一个组合形式下的多点融合定位结果的具体计算方法如下:
假设一共有N个地形匹配定位点3,索引号以k表示,每一个地形匹配定位点3对应的有效峰值点7的总数为mk,则组合模式的总数NU可以由下式得到:
然后是生成索引,根据式(2)的计算结果能够看到一共可以有NU种组合模式;下面介绍生成组合模式时对于每一个地形匹配定位点3的有效峰值的索引方法:
根据步骤四中的假设一共有N个地形匹配定位点3,每一个地形匹配定位点3的有效峰值点7为mk个,k表示地形匹配定位点3的索引号,则多点融合地形匹配定位中对定位点的有效峰值点7的索引矩阵A的大小是NU×N,第k个地形匹配定位点3的索引顺序由A的第k列确定,索引矩阵A的任意一个列的形式可以有下述的方法确定:
对于k,1<k<N列向量的形式为:
其中,每一个编号复制IMk-1次,每一个序列复制Mk-1次;
对于k=1的情况,索引矩阵的列向量为:
其中,每一个编号复制IM2次;
对于k=N的情况,索引矩阵的列向量为:
其中,每一个序列复制MN-1次;
通过上述四步能够获得NU×N的有效峰值点7索引矩阵。
步骤五:根据步骤一得到的每一个地形匹配定位点3的参考导航定位点2和参考导航的定位误差,步骤三得到的有效峰值点7以及有效峰值点7的定位误差,再利用步骤四得到的索引矩阵进行每一个有效峰值点7组合的定位结果解算,一共能够得到NU个地形匹配定位点3融合定位结果,具体计算方法为:
设第q个组合得到的定位修正量为其计算方法如式(3),式(3)中表示第k个地形匹配定位点3位置的第l号有效峰值点7与第k个地形匹配定位点3的参考导航定位点2之间的距离,λk表示权重系数,它与参考导航的定位误差以及有效峰值点7的定位误差有关,即为待求解的定位修正量;
步骤六:根据步骤五中的地形匹配定位多点融合定位结果计算其中每一个定位结果的代价函数Jq,q=1,2,3...NU,q表示由所有的地形匹配定位点的有效峰值点所组成的多地形匹配定位点融合组合的索引号,设第q个组合得到的定位修正量为 中mtp表示该定位偏差是由多地形匹配定位算法获取的;找到Jq最小值对应的q就是最优的定位结果对应的索引序号,此时索引号为q的定位修正量即为最优的定位修正量,到此算法执行完毕,地形匹配定位点融合定位确定。
以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业描述的只是说明本发明的原理,在不脱离本发明精神和范围的前提下,本发明还会有各种变化和改进,这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。
Claims (6)
1.地形匹配定位中有效伪波峰估计与多点融合初始定位方法,其特征在于,包括以下步骤:
步骤一:获得N个地形匹配定位点的似然函数,并得到每一个地形匹配定位点的参考导航定位点和参考导航的定位误差;
步骤二:根据步骤一得到的N个地形匹配定位点的似然函数计算其极大似然值的置信度下确界平面,并以此平面截取似然函数曲面获得等值线即为置信区间边界;由此可得,每一个地形匹配定位点的置信区间,且在搜索区间内的搜索点被置信区间分割为两部分,其中搜索区间是由参考导航系统的误差得到,位于地形匹配定位置信区间内部的搜索点为有效的地形匹配定位点,即地形匹配定位有效定位点;
步骤三:根据步骤二得到的地形匹配定位置信区间和地形匹配定位有效定位点计算地形匹配定位的有效峰值点以及有效峰值点的定位误差;
步骤四:根据步骤三得到的地形匹配定位点的有效峰值点、有效峰值点的似然函数值以及有效峰值点的定位误差来确定地形匹配融合定位的组合形式,得到多点融合地形匹配定位中对定位点的有效峰值点的索引矩阵,计算每一个组合形式下的多点融合定位结果;
步骤五:根据步骤一得到的每一个地形匹配定位点的参考导航定位点和参考导航的定位误差,步骤三得到的有效峰值点以及有效峰值点的定位误差,再利用步骤四得到的索引矩阵进行每一个有效峰值点组合的定位结果解算,一共能够得到NU个地形匹配定位点融合定位结果;
2.根据权利要求1所述的地形匹配定位中有效伪波峰估计与多点融合初始定位方法,其特征在于:所述步骤一中似然函数的具体获得方法如下:
假设在一个航迹段内规划了N个地形匹配定位点,并且已经获得了N个地形匹配定位结果,每一个地形匹配定位点对应一个参考导航定位点,同时得到每一个地形匹配定位点的似然函数,再同时记录下每一个地形匹配定位点对应的参考导航定位点Dk和参考导航的定位误差Rk,k表示规划的地形匹配定位点索引号。
5.根据权利要求4所述的地形匹配定位中有效伪波峰估计与多点融合初始定位方法,其特征在于:生成组合模式时对于每一个地形匹配定位点的有效峰值的具体索引方法如下:
根据步骤四中的假设一共有N个地形匹配定位点,每一个地形匹配定位点的有效峰值点为mk个,k表示地形匹配定位点的索引号,则多点融合地形匹配定位中对定位点的有效峰值点的索引矩阵A的大小是NU×N,第k个地形匹配定位点的索引顺序由A的第k列确定,索引矩阵A的任意一个列的形式由下述的方法确定:
对于k,1<k<N;列向量的形式为:
其中,每一个编号复制IMk-1次,每一个序列复制Mk-1次;
对于k=1的情况,索引矩阵的列向量为:
其中,每一个编号复制IM2次;
对于k=N的情况,索引矩阵的列向量为:
其中,每一个序列复制MN-1次;
通过上述四步能够获得NU×N的有效峰值点索引矩阵。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910136987.2A CN109900271B (zh) | 2019-02-25 | 2019-02-25 | 地形匹配定位中有效伪波峰估计与多点融合初始定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910136987.2A CN109900271B (zh) | 2019-02-25 | 2019-02-25 | 地形匹配定位中有效伪波峰估计与多点融合初始定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109900271A CN109900271A (zh) | 2019-06-18 |
CN109900271B true CN109900271B (zh) | 2022-10-14 |
Family
ID=66945510
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910136987.2A Active CN109900271B (zh) | 2019-02-25 | 2019-02-25 | 地形匹配定位中有效伪波峰估计与多点融合初始定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109900271B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111121778B (zh) * | 2019-12-03 | 2023-06-16 | 河海大学 | 一种导航系统初始化方法 |
CN113932811B (zh) * | 2021-08-25 | 2023-10-27 | 河海大学 | 一种新的地形匹配导航系统、方法 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103047983B (zh) * | 2013-01-14 | 2016-03-09 | 哈尔滨工程大学 | 水下机器人的面地形匹配导航方法 |
US9404754B2 (en) * | 2013-03-25 | 2016-08-02 | Raytheon Company | Autonomous range-only terrain aided navigation |
CN106767836B (zh) * | 2017-02-17 | 2019-12-06 | 哈尔滨工程大学 | 一种auv地形匹配导航滤波方法 |
CN106885576B (zh) * | 2017-02-22 | 2020-02-14 | 哈尔滨工程大学 | 一种基于多点地形匹配定位的auv航迹偏差估计方法 |
-
2019
- 2019-02-25 CN CN201910136987.2A patent/CN109900271B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN109900271A (zh) | 2019-06-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US10323948B2 (en) | GPS data repair | |
CN108089148B (zh) | 一种基于时差信息的无源多站多目标测向交叉定位方法 | |
CN109900271B (zh) | 地形匹配定位中有效伪波峰估计与多点融合初始定位方法 | |
CN104833361B (zh) | 复杂路况下的基于多权重值的地图匹配方法 | |
CN103759732B (zh) | 一种角度信息辅助的集中式多传感器多假设跟踪方法 | |
CN106767836B (zh) | 一种auv地形匹配导航滤波方法 | |
CN107966718B (zh) | 一种改进的整周模糊度搜索方法 | |
CN108061877A (zh) | 一种基于角度信息的无源多站多目标测向交叉定位方法 | |
CN106525042A (zh) | 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法 | |
CN106772512B (zh) | 一种基于消电离层-噪声约束的三频模糊度解算方法 | |
CN113189541B (zh) | 一种定位方法、装置及设备 | |
CN110617819A (zh) | 一种基于蚁群算法路径规划的无人机地形辅助导航方法 | |
CN106324640A (zh) | 一种rtk定位中整周模糊度的动态确定方法 | |
CN104833373A (zh) | 一种基于Procrustes的地磁匹配导航方法 | |
CN110133702B (zh) | 一种基于正交变换的姿态测量方法和设备 | |
CN108427131B (zh) | 一种基线长约束下的整周模糊度快速搜索算法 | |
CN108267743B (zh) | 基于拟合插值的快速迭代水下定位方法 | |
US20140121986A1 (en) | System and method for aligning genome sequence | |
CN116958308A (zh) | 一种路口面边线的修正方法及相关装置 | |
KR100863228B1 (ko) | 도로 네트워크 데이터베이스를 위한 근사 인덱싱 방법 | |
CN113091751A (zh) | 网格空间内部分网格定向条件下的路径规划方法 | |
CN110045396B (zh) | 一种bd/gps双模接收机gdop最小值计算方法 | |
CN110412505A (zh) | 一种快速的时差定位网格搜索方法 | |
CN104994470A (zh) | 一种基于toa与rss融合的无线传感器网络协作跟踪方法 | |
CN107727096B (zh) | 基于有效节点筛选的auv地形匹配定位方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |