[发明专利]一种AUV地形匹配导航滤波方法

权利要求书

1.一种AUV地形匹配导航滤波方法，AUV在水下航行l_t|t距离之后到达地形匹配导航的修正点，推算导航给出了推算导航位置推算导航的误差并开启地形匹配导航程序，此时按照如下的步骤完成地形匹配导航的滤波；

步骤一、向地形匹配导航系统输入初始数据；

步骤二、计算地形匹配定位的搜索区间估计；

步骤三、地形匹配定位；

步骤四、地形匹配地位结果的有效性检验；

步骤五、地形匹配定位结果的定位误差估计；

步骤六、地形匹配定位与推算导航的融合滤波；

其特征是：

所述初始数据包括：推算导航位置推算导航误差AUV航程当前时刻测量得到的地形Z以及先验地形图h；

所述搜索区间由下式确定：

其中：λ_x，λ_y表示导航偏差占载体的运行里程的百分比，由参导航系统的统计结果事先给出；α≥1表示放大倍数，取(1,1.5]；

以为中心，和为x方向和y方向的长度构成的矩形区域就是搜索区间X_S。

2.根据权利要求1所述的AUV地形匹配导航滤波方法，其特征是所述地形匹配定位具体包括：

根据步骤二中计算得到的搜索范围X_S开始进行地形搜索匹配，假设潮差为t_ij，搜索点ij的先验地形插值序列与测量地形的高度偏差序列：

假设地形高度偏差序列满足白噪声分布而潮差是常量，得到搜索点ij的潮差和地形测量误差估计：

得到了潮差的估计值之后，计算测量地形和先验地形的真实相似程度：

其中：h(x_ij,y_ij)表示测量序列点(x_ij,y_ij)在先验地形图h中的插值结果；z_ij∈Z，Z表示测量地形，z_ij表示测量地形内的点；

查找似然函数L_ij值最大的位置得到地形匹配定位的位置得到定位位置后将定位位置的潮差估计和测量地形的误差估计作为当前定位位置的最优潮差估计和测量误差估计记为：

3.根据权利要求2所述的AUV地形匹配导航滤波方法，其特征是所述地形匹配地位结果的有效性检验具体包括：

根据地形匹配定位位置检验该定位点的有效性，建立如下的有效性检验模型：

其中C_t|t为有效区间，由下面的方法得到：

(1)以步骤三中得到的似然函数L_ij为输入，计算定位点似然函数的下界L_low；

其中:σ_p表示地形匹配地位点的测量误差估计；m,n表示地形匹配地位点的个数；Z_α/2表示以σ_p为正态分布的上分位点对应的值；

(2)以L_low为等高平面截取步骤三中的似然函数L_ij，得到的等高线其中i表示等高线索引号，假设共有n条等高线；

(3)查找包围地形匹配定位点的等高线在等高线所包围区域的内部，就是地形匹配定位的置信区间C_t|t。