CN102426018A - 基于混合地形轮廓匹配tercom算法和粒子滤波的地形辅助导航方法 - Google Patents

Abstract

本发明一种基于混合地形轮廓匹配TERCOM算法和粒子滤波的地形辅助导航方法，目的是解决在大的初始位置误差，平坦地形区域以及飞行器高机动飞行时滤波器的发散问题。该导航方法包含搜索模式、跟踪模式以及丢失模式和模式逻辑装换，在搜索模式下，分别利用平均绝对差MAD算法、均方差MSD算法和交叉相关COR算法批处理测量信息获得位置估计信息，并根据一致性表决算法，在大的初始位置误差下快速、准确地实现初始定位；在跟踪模式下采用粒子滤波递推处理测量信息实现连续匹配定位；丢失模式和模式逻辑转换实现初始定位后由搜索模式转入跟踪模式，出现伪定位后由跟踪模式转换为搜索模式。本发明能够保证在大的初始位置误差稳定可靠地实现连续定位。

Description

基于混合地形轮廓匹配TERCOM算法和粒子滤波的地形辅助导航方法

技术领域

本发明涉及一种惯性地形辅助导航方法，特别是一种基于混合地形轮廓匹配TERCOM算法和粒子滤波的地形辅助导航方法，适用于无人飞行器和有人飞行器地形辅助导航定位系统。

背景技术

在现代军民用领域中应用的无人飞行器(无人机和巡航导弹)对自主导航系统精度和可靠性要求越来越高，要求实时、可靠地提供无人飞行器的位置、速度和姿态信息。依靠单一的导航系统很难满足要求。将惯性导航系统与卫星定位系统信息进行融合来提供载体的运动信息已被广泛应用。但由于卫星定位信号易受干扰或欺骗，甚至信号转发源被摧毁，不能够长期提供稳定可靠的定位信息，在军民用领域中尚不能完全依赖于卫星定位来辅助惯性导航实现飞行器的精确导航定位。因此，发展具有自主性强和较高精度的地形辅助导航技术已成为提高飞机、巡航导弹等武器系统战斗力的有效手段之一。

经过近60年的发展，已经发展成熟的地形辅助导航方法有两类，一种是基于相关分析原理的地形轮廓匹配算法的导航方法，例如TERCOM算法；另一类是基于扩展Kalman滤波的地形辅助惯性导航算法的导航方法，例如SITAN算法。两种导航方法已被成功应用在多种型号的精确制导武器和战术飞机上。然而，当飞行器在平坦区域、地形梯度变化剧烈的区域或者飞行器有大的机动时，两种导航方法的定位精度下降，甚至产生伪定位。为了克服传统地形匹配算法的不足，提出了一些改进的导航方法，例如基于多模自适应估计技术的HELI/SITAN算法、AFTI/SITAN算法、北航惯性地形辅助导航(BUAA Inertial Terrain AidedNavigation，BITAN，BITAN)算法和北航惯性地形辅助导航算法(BITAN)II的导航方法，但这些导航方法实质都是将非线性的系统状态方程和观测方程进行线性化处理后，利用扩展卡尔曼滤波方法对地形高度匹配问题进行处理，因此只适用于滤波误差和预测误差很小的情况。然而由于地形特征是非线性的，当初始位置误差较大时，在地形线性化时会造成拟合区域线性化误差增大，无法满足卡尔曼滤波对局部线性化的要求，使得滤波发散，而且当地形标高变化过于剧烈、不能满足线性化假设时，定位精度将下降，甚至不能进行定位。

本发明中提到的一种基于混合地形轮廓匹配TERCOM算法和粒子滤波的地形辅助导航方法，利用基于相关分析原理的TERCOM算法实现跟踪模式，能够快速、准确地完成初始定位，在跟踪模式下利用能充分反映地形非线性特性的粒子滤波实现连续快速的跟踪，再辅以模式逻辑转换，保证了算法的定位精度和可靠性。

本发明与其它惯性地形辅助导航不同在于：在当前已公开发表的资料中，惯性地形辅助导航集中在对传统算法的改进，在搜索模式和跟踪模式都采用卡尔曼滤波，或是在搜索模式采用TERCOM算法，跟踪模式采用扩展卡尔曼滤波等，但这些方法无一例外均无法解决在大的初始位置误差，平坦地形区域以及飞行器高机动飞行时滤波器的发散问题。

发明内容

本发明要解决的技术问题：克服现有的惯性地形辅助导航系统在大的初始位置误差，平坦地形区域以及飞行器高机动飞行时无法定位，甚至滤波器发散问题，提出一种基于混合地形轮廓匹配TERCOM算法和粒子滤波的地形辅助导航方法。

本发明采用的技术方案为：一种基于混合地形轮廓匹配TERCOM算法和粒子滤波的地形辅助导航方法，该导航方法包含搜索模式、跟踪模式以及丢失模式和模式逻辑装换，在搜索模式下，分别利用平均绝对差MAD算法、均方差MSD算法和交叉相关COR算法批处理测量信息获得位置估计信息，并根据一致性表决算法，在大的初始位置误差下快速、准确地实现初始定位；在跟踪模式下采用粒子滤波递推处理测量信息实现连续匹配定位；丢失模式和模式逻辑转换实现初始定位后由搜索模式转入跟踪模式，出现伪定位后由跟踪模式转换为搜索模式。保证在大的初始位置误差稳定可靠地实现连续定位。具体实现步骤为：

(1)进入地形匹配区域判断

根据惯导系统的指示位置和数字高程基准图左上角和右下角的坐标进行判断，飞行器是否进入地形匹配区域；

利用坐标变换公式将每一个匹配区左上角和右下角的高斯直角坐标转换为地理坐标，并分别记为(start_i_λ，start_i_L)和(end_i_λ，end_i_L)，i＝1，2，…为匹配区编号；

根据惯导系统(Inertial Navigation System，INS)指示的位置(λ，L)，判断飞行器是否到达或进入第i个匹配区

start_i_λ≤λ＜end_i _λ    (1)

start_i_L≤L＜end_i_L        (2)

式中，λ为经度，L为纬度，上述两个公式同时成立且等号不成立时表明飞行器已进入匹配区，当等号刚好成立时，表明飞行器到达匹配区的边缘；

(2)搜索模式

进入匹配区后，利用高度表连续获取高度测量信息，分别利用基于相关分析原理的交叉相关COR算法、平均绝对差MAD算法和均方差MSD算法进行批处理运算，获得飞行器的位置的估计值

和

根据位置的估计值构造位置一致性判断准则为：

${\hat{P}}_{\mathrm{\δ}}=\frac{\sqrt{3}}{3}\sqrt{{({\hat{P}}_{\mathrm{COR}}-{\hat{P}}_{\mathrm{MAD}})}^2+{({\hat{P}}_{\mathrm{COR}}-{\hat{P}}_{\mathrm{MSD}})}^2+{({\hat{P}}_{\mathrm{MSD}}-{\hat{P}}_{\mathrm{MAD}})}^2}---\left(3\right)$

当

(δ为数字高程图的分辨率)时，则获得飞行器位置估计值是正确的，完成一次位置一致性的判别；

当连续三次一致性判别一致时，则转入跟踪模式，此时通过相关分析算法获得飞行器的位置估计值是实际位置的概率为99.7％；

为保证每次一致性判别的独立性，每次匹配定位的间隔必须大于连续匹配时的匹配间隔，保证每次匹配时地形轮廓是无关的；

(3)跟踪模式

针对地形辅助导航的非线性问题，建立一个能够描述地形辅助导航的概率模型，通过对该模型的随机采样得到大量样本点，并通过计算样本点的统计特征估计飞行器的位置信息，即利用状态空间的一组加权随机粒子样本逼近状态变量的后验概率密度；

基于粒子滤波的地形辅助导航系统的状态取飞行器在水平面上的位置信息，即在东北天坐标系下，系统状态取为东向位置和北向位置，其状态方程和量测方程为：

X_k＝X_k-1+V_k-1T+Q_k-1    (4)

Z_k＝h(X_k)+r_k           (5)

式中，X＝[x y]^T，其中x表示东向位置，y表示北向位置；V＝[v_x v_y]^T，其中v_x表示东向速度，v_y表示北向速度；Q_k-1为系统噪声；h(X_k)是地形高程值关于位置(x，y)的非线性函数；r_k为量测误差，为高斯噪声，包括气压高度表误差，雷达高度噪声以及数字地图制作噪声；

(4)丢失模式和模式逻辑转换

丢失模式是一种特殊的处理模式，包括真丢失和假丢；

真丢失是指惯导系统的位置误差很大，导致其指示位置与真实位置间距超过了搜索范围，主要原因是系惯导系统初始对准问题和系统硬件故障；

假丢失实际上是由于算法判据不完善或是地形平坦或重复造成的伪定位。如果不及时处理，会导致搜索模式下获得位置估值与真实位置有很大的误差，导致飞行器定位精度下降，甚至出现定位错误；

在丢失模式下，根据模式转换算法使其转入丢失模式，重新进行搜索，获得正确的飞行器位置估计值后再转入跟踪模式；

模式转换包括两种形式，一种是系统完成初始定位后由搜索模式转入跟踪模式；另一种是在跟踪模式下出现伪定位，由跟踪模式转入搜索模式。

本发明的优点在于：采用TERCOM算法实现跟踪模式，能够快速、准确地实现飞行器初始定位，而且初始位置是飞行器实际位置的概率为99.7％；采用粒子滤波实现飞行器连续定位，充分利用了地形的非线性信息，可实现在平坦区域以及飞行器高机动飞行时进行连续定位，提高了系统的可靠性和定位精度，并拓展了其适用范围；设置丢失模式和模式逻辑转换进一步保证了算法的可靠性。可以实现在大的初始位置误差、平坦地形区域以及飞行器高机动飞行时实现连续可靠地定位。

附图说明

图1为本发明的一种基于混合地形轮廓匹配TERCOM算法和粒子滤波的地形辅助导航方法的原理流程图；

图2为本发明的测量量间的关系图；

图3为本发明的地形匹配区域判断示意图；

图4为本发明的粒子滤波算法及重采样原理图；

图5(a)为本发明与基于BITANII算法的导航方法的东向位置误差比较结果；

图5(b)为本发明与BITANII算法的导航方法的北向位置误差比较结果。

具体实施方式

下面结合附图和具体实施方式对本发明进一步说明，本发明的具体实现步骤如下：

(1)算法初始化

算法初始化包括匹配区搜索大小、初始粒子分布、惯导系统的初始位置、速度和姿态以及数字高程基础图的原点坐标以及地图大小等信息。

(2)进入匹配区域判断

如图2所示，惯性导航系统作为主导航系统提供全部导航信息，雷达高度表输出的高度h_r是飞行器到地面的瞬时距离，气压高度表或大气数据计算机单独或与惯性导航系统组合输出的高度h是飞行器到海平面的瞬时距离，根据惯导系统的指示位置和数字高程基准图左上角和右下角的坐标进行判断，飞行器是否进入地形匹配区域；

利用坐标变换公式将每一个匹配区左上角和右下角的高斯直角坐标转换为地理坐标，并分别记为(start_i_λ，start_i_L)和(end_i_λ，end_i_L)，i＝1，2，…为匹配区编号；

根据惯导系统(Inertial Navigation System，INS)指示的位置(λ，L)，判断飞行器是否到达或进入第i个匹配区

start_i_λ≤λ＜end_i_λ     (1)

start_i_L≤L＜end_i_L        (2)

式中，λ为经度，L为纬度，上述两个公式同时成立且等号不成立时表明飞行器已进入匹配区，当等号刚好成立时，表明飞行器到达匹配区的边缘，如图3所示，其中大方框代表地形匹配区，细实线代表惯导指示航线，粗实线代表飞行器真实航线；飞行器在A点时，系统判断刚进入匹配区，此时系统开始加载数字高程基准图，当飞行器飞到B点时，小方框代表根据误差带估计设定的搜索区，当搜索区全部位于地形匹配区，开始进入搜索模式。

(3)搜索模式

进入匹配区后，利用高度表连续获取高度测量信息，分别利用基于相关分析原理的交叉相关COR算法、平均绝对差MAD算法和均方差MSD算法进行批处理运算，获得飞行器的位置的估计值

和

其计算公式为：

交叉相关(Cross Correlation，COR)算法

$J_{\mathrm{COR}}=\frac{1}{L}\underset{i=1}{\overset{L}{\mathrm{\Σ}}}{T}_A\left(i\right)\·T_S(x+{\mathrm{i\τ}}_x,y+{\mathrm{i\τ}}_y)---\left(3\right)$

平均绝对差(Mean Absolute Difference，MAD)算法

$J_{\mathrm{MAD}}=\frac{1}{L}\underset{i=1}{\overset{L}{\mathrm{\Σ}}}|T_A\left(i\right)-T_S(x+{\mathrm{i\τ}}_x,y+{\mathrm{i\τ}}_y)|---\left(4\right)$

均方差(Mean SquareDifference，MSD)算法

$J_{\mathrm{MSD}}=\frac{1}{L}\underset{i=1}{\overset{L}{\mathrm{\Σ}}}[T_A\left(i\right)-T_S(x+{\mathrm{i\τ}}_x,y+{\mathrm{i\τ}}_y){]}^2---\left(5\right)$

式中，L为采样地形轮廓长度，T_A(i)表示计算获得的第i个地形高度值(i＝1，2…L)；T_S(x+iτ_x，y+iτ_y)表示对应的第i个数字地图高程值；D(T_A)表示地形高度序列T_A(i)的方差；x、y为所选进行匹配的网格中心坐标；τ_x、τ_y为飞行器在相邻两次采样之间飞行器在两坐标轴上飞行经过的距离，若飞行器匀速飞行，则二者为常值。最佳定位位置是使J_COR最大，J_MAD和J_MSD最小；

根据位置的估计值构造位置一致性判断准则为：

${\hat{P}}_{\mathrm{\δ}}=\frac{\sqrt{3}}{3}\sqrt{{({\hat{P}}_{\mathrm{COR}}-{\hat{P}}_{\mathrm{MAD}})}^2+{({\hat{P}}_{\mathrm{COR}}-{\hat{P}}_{\mathrm{MSD}})}^2+{({\hat{P}}_{\mathrm{MSD}}-{\hat{P}}_{\mathrm{MAD}})}^2}---\left(6\right)$

当

(δ为数字高程图的分辨率)时，则获得飞行器位置估计值是可靠的，完成一次位置一致性的判别；

当连续三次一致性判别都满足要求时，则转入跟踪模式，此时通过相关分析算法获得飞行器的位置估计值是实际位置的概率为99.7％；

为保证每次一致性判别的独立性，每次匹配定位的间隔必须大于连续匹配时的匹配间隔，保证每次匹配时地形轮廓是无关的。

(4)跟踪模式

针对地形辅助导航的非线性问题，建立一个能够描述地形辅助导航的概率模型，通过对该模型的随机采样得到大量样本点，并通过计算样本点的统计特征估计飞行器的位置信息，即利用状态空间的一组加权随机样本(粒子)逼近状态变量的后验概率密度；

基于粒子滤波的地形辅助导航系统的状态取飞行器在水平面上的位置信息，即在东北天坐标系下，系统状态取为东向位置和北向位置，其状态方程和量测方程为：

X_k＝X_k-1+V_k-1T+Q_k-1    (7)

Z_k＝h(X_k)+r_k           (8)

式中，X＝[x y]^T，其中x表示东向位置，y表示北向位置；V＝[v_x v_y]^T，其中v_x表示东向速度，v_y表示北向速度；Q_k-1为系统噪声；h(X_k)是地形高程值关于位置(x，y)的非线性函数；r_k为量测误差，为高斯噪声，包括气压高度表误差，雷达高度噪声以及数字地图制作噪声；

如图4所示，跟踪模式的具体实现步骤为：

其具体方法为：

①初始化  设置适当的p(x₀)分布，生成N个服从p(x₀)分布的随机样本x₀(i)，其中i＝1，…N，即对p(x₀)进行抽样。一般情况下，可令p(x₀)服从正态分布；

②时间更新  生成N个服从p_Q(Q_k-1)的随机数并根据状态方程对粒子进行一步预测：

$X_k^{*}\left(i\right)=X_{k-1}+V_{k-1}T+Q_{k-1}^i---\left(9\right)$

③测量更新  在得到当前测量值Z_k后，计算各粒子的似然比，从而获得归一化的权值和更新粒子后的状态估计值为：

$w_i=\frac{p_{r_k}(Z_k-h\left(X_k^{*}(i\right))}{\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{p}_{r_k}(Z_k-h\left(X_k^{*}(j\right))}$ i＝1，…，N    (10)

${\hat{X}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_i{X}_k^{*}\left(i\right)---\left(11\right)$

④重采样  对

的离散分布粒子进行重采样，获得一组新的粒子X_k(i)，i＝1，…，N，使各粒子的权值均为1/N；

⑤迭代从②到④循环递推。

(5)丢失模式和模式逻辑转换

丢失模式是一种特殊的处理模式，包括真丢失和假丢；

真丢失是指惯导系统的位置误差很大，导致其指示位置与真实位置间距超过了搜索范围，主要原因是系惯导系统初始对准问题和系统硬件故障；

假丢失实际上是由于算法判据不完善或是地形平坦或重复造成的伪定位。如果不及时处理，会导致搜索模式下获得位置估值与真实位置有很大的误差，导致飞行器定位精度下降，可靠性降低，甚至出现定位错误；

在丢失模式下，根据模式转换算法使其转入丢失模式，重新进行搜索，获得正确的飞行器位置估计值后再转入跟踪模式；

模式转换包括两种形式，一种是系统完成初始定位后由搜索模式转入跟踪模式；另一种是在跟踪模式下出现伪定位，由跟踪模式转入搜索模式，具体实现步骤为：

a、记录搜索模式下连续三次通过表决的匹配结果和时间，并以第三次匹配的匹配结果作为系统跟踪算法的初始位置；

b、依据系统跟踪算法计算出当前时刻的匹配位置，经度λ(k)和纬度L(k)；

c、根据前5次匹配定位结果，推算出5个当前时刻的位置估计值，经度

和纬度

(n＝1，2，…，5)，其计算公式为：

${\widehat{\mathrm{\λ}}}_{(k-n)}\left(k\right)=\mathrm{\λ}(k-n)+\frac{{\∫}_{k-n}^k{\stackrel{\‾}{V}}_E\mathrm{dt}}{R_N\cos \left(L_{(k-n)}\left(k\right)\right)}---\left(12\right)$

${\hat{L}}_{(k-n)}\left(k\right)=L(k-n)+\frac{{\∫}_{k-n}^k{\stackrel{\‾}{V}}_N\mathrm{dt}}{R_M}---\left(13\right)$

式中：V_E、V_N分别为惯导东和北方向速度在k-n至k时刻内的平均值；R_M、R_N分别是子午圈和卯酉圈半径；n＝1，2，…，5；

d、将系统跟踪算法解算出来的当前时刻匹配位置与由式(9)和(10)推算出来的5个当前时刻估计位置分别相减，当误差小于阈值的次数大于3次则认为系统跟踪算法解算出来的当前时刻匹配位置λ(k)和L(k)是正确的；否则，视当前匹配结果无效，放弃此次匹配结果；

e、如若连续5次匹配结果无效，则重新搜索；如若连续5次匹配有效，则终止检测，进入连续跟踪；未完成连续5次匹配，则返回第b步；

f、在跟踪模式下，在跟踪一段时间后，惯导累积了较大的误差，为了防止系统跟踪算法滤波发散，强制匹配系统重新进入搜索模式，使得搜索与跟踪交替进行。

Claims (3)

1.一种基于混合地形轮廓匹配TERCOM算法和粒子滤波的地形辅助导航方法，其特征在于，该导航方法包括搜索模式、跟踪模式以及丢失模式和模式逻辑转换，在飞行器进入地形匹配区域后，该导航方法开始工作，其具体实现步骤：

(1)进入地形匹配区域判断：

根据惯导系统的指示位置和数字高程基准图左上角和右下角的坐标进行判断，飞行器是否进入地形匹配区域；

利用坐标变换公式将每一个地形匹配区左下上角和右下角的高斯直角坐标转换为地理坐标，并分别记为(start_i_λ，start_i_L)和(end_i_λ，end_i_L)，i＝1，2，…为匹配区编号；

根据惯导系统指示的位置(λ，L)，判断飞行器是否到达或进入第i个地形匹配区：

start_i_λ≤λ＜end_i_λ (1)

start_i_L≤L＜end_i_L    (2)

式中，λ为经度，L为纬度，上述两个公式同时成立且等号不成立时表明飞行器已进入匹配区，当等号刚好成立时，表明飞行器到达匹配区的边缘；

(2)搜索模式：

进入匹配区后，利用高度表连续获取高度测量信息，分别利用基于相关分析原理的交叉相关COR算法、平均绝对差MAD算法和均方差MSD算法进行批处理运算，获得飞行器的位置的估计值

和

根据位置的估计值构造位置一致性判断准则为：

${\hat{P}}_{\mathrm{\δ}}=\frac{\sqrt{3}}{3}\sqrt{{({\hat{P}}_{\mathrm{COR}}-{\hat{P}}_{\mathrm{MAD}})}^2+{({\hat{P}}_{\mathrm{COR}}-{\hat{P}}_{\mathrm{MSD}})}^2+{({\hat{P}}_{\mathrm{MSD}}-{\hat{P}}_{\mathrm{MAD}})}^2}---\left(3\right)$

当

时，其中δ为数字高程图的分辨率，则获得飞行器位置估计值是正确的，完成一次位置一致性的判别；

当连续三次一致性判别一致时，则转入跟踪模式，此时通过相关分析算法获得飞行器的位置估计值是实际位置的概率为99.7％；

为保证每次一致性判别的独立性，每次匹配定位的间隔必须大于连续匹配时的匹配间隔，保证每次匹配时地形轮廓是无关的；

(3)跟踪模式：

针对地形辅助导航的非线性问题，建立一个能够描述地形辅助导航的概率模型，通过对该模型的随机采样得到大量样本点，并通过计算样本点的统计特征估计飞行器的位置信息，即利用状态空间的一组加权随机粒子样本逼近状态变量的后验概率密度；

基于粒子滤波的地形辅助导航系统的状态取飞行器在水平面上的位置信息，即在东北天坐标系下，系统状态取为东向位置和北向位置，其状态方程和量测方程为：

X_k＝X_k-1+V_k-1T+Q_k-1    (4)

Z_k＝h(X_k)+r_k           (5)

式中，X＝[x y]^T，其中x表示东向位置，y表示北向位置；V＝[v_x v_y]^T，其中v_x表示东向速度，v_y表示北向速度；Q_k-1为系统噪声；h(X_k)是地形高程值关于位置(x，y)的非线性函数；r_k为量测误差，为高斯噪声，包括气压高度表误差，雷达高度噪声以及数字地图制作噪声；

(4)丢失模式和模式逻辑转换：

丢失模式是一种特殊的处理模式，包括真丢失和假丢；

真丢失是指惯导系统的位置误差很大，导致其指示位置与真实位置间距超过了搜索范围，主要原因是系惯导系统初始对准问题和系统硬件故障；

假丢失实际上是由于算法判据不完善或是地形平坦或重复造成的伪定位，如果不及时处理，会导致搜索模式下获得位置估值与真实位置有很大的误差，导致飞行器定位精度下降，甚至出现定位错误；

在丢失模式下，根据模式转换算法使其转入丢失模式，重新进行搜索，获得正确的飞行器位置估计值后再转入跟踪模式；

模式转换包括两种形式，一种是系统完成初始定位后由搜索模式转入跟踪模式；另一种是在跟踪模式下出现伪定位，由跟踪模式转入搜索模式。

2.根据权利要求1所述的一种基于混合地形轮廓匹配TERCOM算法和粒子滤波的地形辅助导航方法，其特征在于：所述步骤(3)跟踪模式，其具体方法为：

①初始化：

设置适当的p(x₀)分布，生成N个服从p(x₀)分布的随机样本x₀(i)，其中i＝1，…N，即对p(x₀)进行抽样，令p(x₀)服从正态分布；

②时间更新：

生成N个服从p_Q(Q_k-1)的随机数

并根据状态方程对粒子进行一步预测：

$X_k^{*}\left(i\right)=X_{k-1}+V_{k-1}T+Q_{k-1}^i---\left(6\right)$

③测量更新：

在得到当前测量值Z_k后，计算各粒子的似然比，从而获得归一化的权值和更新粒子后的状态估计值为：

$w_i=\frac{p_{r_k}(Z_k-h\left(X_k^{*}(i\right))}{\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{p}_{r_k}(Z_k-h\left(X_k^{*}(j\right))}$ i＝1，…，N    (7)

${\hat{X}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_i{X}_k^{*}\left(i\right)---\left(8\right)$

④重采样：

对

的离散分布粒子进行重采样，获得一组新的粒子X_k(i)，i＝1，…，N，使各粒子的权值均为1/N；

⑤迭代：从②到④循环递推。

3.根据权利要求1所述的一种基于混合地形轮廓匹配TERCOM算法和粒子滤波的地形辅助导航方法，其特征在于：所述步骤(4)丢失模式和模式逻辑转换，其具体方法为：

a、记录搜索模式下连续三次通过表决的匹配结果和时间，并以第三次匹配的匹配结果作为系统跟踪算法的初始位置；

b、依据系统跟踪算法计算出当前时刻的匹配位置，经度λ(k)和纬度L(k)；

c、根据前5次匹配定位结果，推算出5个当前时刻的位置估计值，经度和纬度

(n＝1，2，…，5)，其计算公式为：

${\widehat{\mathrm{\λ}}}_{(k-n)}\left(k\right)=\mathrm{\λ}(k-n)+\frac{{\∫}_{k-n}^k{\stackrel{\‾}{V}}_E\mathrm{dt}}{R_N\cos \left(L_{(k-n)}\left(k\right)\right)}---\left(9\right)$

${\hat{L}}_{(k-n)}\left(k\right)=L(k-n)+\frac{{\∫}_{k-n}^k{\stackrel{\‾}{V}}_N\mathrm{dt}}{R_M}---\left(10\right)$

式中：V_E、V_N分别为惯导东和北方向速度在k-n至k时刻内的平均值；R_M、R_N分别是子午圈和卯酉圈半径；n＝1，2，…，5；

d、将系统跟踪算法解算出来的当前时刻匹配位置与由式(9)和(10)推算出来的5个当前时刻估计位置分别相减，当误差小于阈值的次数大于3次则认为系统跟踪算法解算出来的当前时刻匹配位置λ(k)和L(k)是正确的；否则，视当前匹配结果无效，放弃此次匹配结果；

e、如若连续5次匹配结果无效，则重新搜索；如若连续5次匹配有效，则终止检测，进入连续跟踪；未完成连续5次匹配，则返回第b步；

f、在跟踪模式下，在跟踪一段时间后，惯导累积了较大的误差，为了防止系统跟踪算法滤波发散，强制匹配系统重新进入搜索模式，使得搜索与跟踪交替进行。

Images