[发明专利]一种基于粒子滤波嵌套粒子滤波算法的重力匹配定位方法

说明书

本发明公开了一种基于粒子滤波嵌套粒子滤波算法的重力匹配定位方法，能够抑制矢量匹配算法中的误差。包括：通过惯性导航系统获取载体的曲线航迹，根据所述航迹的判断第i时刻的真实位置与第i‑1时刻的真实位置的相对位置，记为向量其中O_i‑1、O_i分别表示惯性导航系统在第i‑1时刻和第i时刻指示的所述载体所在的位置；i为时刻；第i‑1时刻的粒子群为其中表示第i‑1时刻的第j个粒子，j为整数；对于每个加上相对距离得到即在附近进行撒点得到粒子群求解每一个粒子的归一化权值，经过滤波得到求出与粒子群中每一个对应的组成粒子群对粒子群进行粒子滤波得到第i时刻的匹配点。

技术领域

本发明涉及导航、制导与控制技术领域，具体涉及一种基于粒子滤波嵌套粒子滤波算法的重力匹配定位方法。

背景技术

重力匹配算法直接决定重力辅助惯导系统的精度，是水下重力无源辅助导航的核心技术。其本质是利用各种合适的信息处理融合方法，将实际测量的水下重力数据和存储好的重力基准图中的相应的重力数据进行相关性分析，根据一定的准则判断这两类数据的相似程度，最终得到对惯导位置的最佳估计结果。

传统的重力匹配算法按照采样方式可以分为序列匹配算法和单点匹配算法。序列匹配算法主要有ICCP和TERCOM算法。但是序列匹配算法的实时性比较差。单点匹配算法实时性较好，但是需要较高精度的初始位置，线性化方法选用不当或者线性化的误差较大的话，会出现滤波发散的情况。最近出现了矢量匹配算法。矢量匹配算法是一种单点粒子滤波匹配算法基础上的二次匹配算法，是在单点粒子滤波匹配结果的基础上加入惯导相邻点之间的位置相关性，对匹配结果进行二次估计，并借鉴贪心算法对多个校正结果利用加权最小二乘法得到最终匹配结果。矢量匹配算法相比较传统的重力匹配算法来说具有较好的精度和鲁棒性。但是该方法的精度经过理论的分析是可以进一步提高的。也就是说，该方法的误差是可以继续缩小的。

而目前国内外尚未有能够抑制矢量匹配误差的方案，因此如何进一步提高矢量匹配算法的精度，是亟待解决的问题。

发明内容

有鉴于此，本发明提供了一种基于粒子滤波嵌套粒子滤波算法的重力匹配定位方法，能够抑制矢量匹配算法中的误差，使得匹配的轨迹更加逼近于真实的轨迹。

为达到上述目的，本发明所提供的基于粒子滤波嵌套粒子滤波算法的重力匹配定位方法，包括：

步骤一、通过惯性导航系统获取载体的曲线航迹，根据所述航迹的判断第i时刻的真实位置与第i-1时刻的真实位置的相对位置，记为向量其中O_i-1、O_i分别表示惯性导航系统在第i-1时刻和第i时刻指示的所述载体所在的位置；i为时刻；

步骤二、第i-1时刻的粒子群为其中表示第i-1时刻的第j个粒子，j为整数；对于每个加上相对距离得到即在附近进行撒点得到粒子群求解每一个粒子的归一化权值，经过滤波得到

求出与粒子群中每一个对应的组成粒子群

步骤三、对粒子群进行粒子滤波得到第i时刻的匹配点。

有益效果：

1、本发明所提供的基于粒子滤波嵌套粒子滤波算法的重力匹配定位方法，利用概率论的知识，采用粒子滤波嵌套粒子滤波对重力匹配的方法进行误差抑制，大大减小了经度误差和纬度误差，使得最终匹配的轨迹更加逼近于真实的轨迹。

2、本发明采用大量的粒子进行滤波得到匹配点，同时每个粒子的位置都是通过粒子滤波的得到，所以本发明的鲁棒性很好，在复杂环境中也能有很好的效果。

附图说明

图1为本发明所提供的一种基于粒子滤波嵌套粒子滤波算法的重力匹配定位方法的流程图；

图2为粒子滤波嵌套粒子滤波算法误差分析图。

具体实施方式