CN115077530B - 一种基于强跟踪扩维eckf算法的多auv协同导航方法及系统 - Google Patents

Abstract

本发明提出一种基于强跟踪扩维ECKF算法的多AUV协同导航方法及系统，涉及多自主水下航行器协同导航技术领域，用以解决主从式多AUV协同导航系统先验信息不准确或系统状态发生突变时定位失效的问题。本发明的技术要点包括：根据强跟踪滤波器(StrongTracking Filter,STF)的原理，用STF对基于扩维ECKF的多AUV协同导航方法进行改进，提高多AUV协同导航系统的定位导航性能。本发明可应用于主从式多AUV协同导航之中。

Description

一种基于强跟踪扩维ECKF算法的多AUV协同导航方法及系统

技术领域

本发明涉及多自主水下航行器(Autonomous Underwater Vehicle,AUV)协同导航技术领域，具体涉及一种基于强跟踪扩维嵌入式容积卡尔曼滤波的多AUV协同导航方法及系统。

背景技术

多AUV系统中，跟随AUV结合相对观测获取的导航信息，能够通过协同导航算法对自身位置进行滤波估计，以此降低系统整体的导航误差。多AUV协同导航算法根据类型主要分为三种，分别为：基于优化理论的协同导航算法、基于图论的协同导航算法、基于贝叶斯估计的协同导航算法。

多AUV协同导航的过程中，由于系统初始状态的先验信息或是噪声统计特性不准确，在复杂恶劣的海洋环境中系统状态可能发生突变，从而引起协同导航算法的状态估计值误差增大，甚至有可能导致滤波发散。基于贝叶斯估计的扩维嵌入式容积卡尔曼滤波(Embedded Cubature Kalman Filter,ECKF)算法通过嵌入式容积准则增加了容积点的选取并改变其权值，从而抑制传统容积卡尔曼滤波随着状态空间维数增加而增大的定位估计误差；但其仍旧对系统状态突变以及初始状态的先验信息比较敏感，难以保证异常测量或是扰动情况下的稳定性。

发明内容

鉴于以上问题，本发明提出一种基于强跟踪扩维嵌入式容积卡尔曼滤波(StrongTracking Filter ECKF,STF-ECKF)算法的多AUV协同导航方法及系统，用以解决主从式多AUV协同导航系统先验信息不准确或系统状态发生突变时定位失效的问题。

根据本发明的一方面，提供一种基于强跟踪扩维ECKF算法的多AUV协同导航方法，该方法包括以下步骤：

步骤一、建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；

步骤二、采用扩维ECKF算法对多个AUV进行状态预测和量测预测；

步骤三、在状态预测和量测预测中引入强跟踪算法中的渐消因子，以更新跟随AUV的状态估计值，保证每个时刻滤波残差的正交性。

进一步地，步骤一中所述主从式多AUV协同导航系统的离散时间状态方程和量测方程表示为：

其中，表示k时刻跟随AUV的n维状态变量，x_k、y_k、分别表示跟随AUV在k时刻的x轴坐标、y轴坐标及航向角；表示k时刻跟随AUV的传感器输入量，V_k表示k时刻跟随AUV的航行方向上的前向速度；表示k时刻跟随AUV由前向速度噪声和航向角传感器噪声组成的n_w维过程噪声向量；v_k＝[v_k]^T表示k时刻相对距离测量噪声v_k所构成的n_v维量测噪声向量；

其中，状态函数表示为：

式中，Δt表示航位推算的系统周期；w_k表示k时刻AUV航行的角速度；

量测函数表示为：

式中，上标M代表领航AUV；上标S代表跟随AUV。

进一步地，步骤二中状态预测的具体过程包括：

假设k时刻后验概率密度函数中上一时刻状态估计值以及误差协方差P_k-1k-1已知，对P_k-1k-1进行Cholesky分解得到过程变量C_k-1k-1：

使用n_w维过程噪声对状态变量进行扩维，得到过程变量以及

计算容积点X_i,k-1k-1：

其中，i＝1,2,…,N,ξ_i表示容积点集；Q_k-1表示k-1时刻的过程噪声协方差矩阵；

根据ECKF算法中的嵌入式容积准则，替换容积点集ξ_i以及权值

式中，[0]_i为维数为n的零向量；[δ]＝[s₁σ²,s₁σ²,…,s₁σ²]^T，σ＝1且s₁＝±1；

根据系统函数传播容积点X_i,k-1k-1的状态变量分量以及过程噪声分量得到传播后的容积点

根据系统函数传播后的容积点计算k时刻的状态预测值

计算k时刻的状态误差协方差预测值P_kk-1：

进一步地，步骤二中量测预测的具体过程包括：

通过量测函数传播的状态变量分量

根据量测函数传播后的容积点计算k时刻的观测预测值

式中，r_k＝E(v_k)为v_k的均值；

计算k时刻的自相关协方差矩阵P_ZZ,kk-1：

式中，表示量测噪声协方差矩阵；

计算k时刻互相关协方差矩阵P_XZ,kk-1：

进一步地，步骤三的具体步骤包括：

通过k时刻实际相对距离量测值Z_k以及观测预测值计算残差γ_k：

计算滤波残差γ_k的协方差阵

式中，ρ表示遗忘因子；

计算中间过程变量N_k和M_k：

式中，β表示弱化因子；

计算自适应渐消因子λ_k：

在自相关协方差矩阵P_ZZ,kk-1中引入渐消因子λ_k：

计算k时刻的卡尔曼滤波增益K_k：

计算k时刻状态估计值

计算k时刻的状态误差协方差估计值P_k|k：

根据本发明的另一方面，提供一种基于强跟踪扩维ECKF算法的多AUV协同导航系统，该系统包括：

系统方程构建模块，其配置成建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；

协同定位模块，其配置成采用扩维ECKF算法对跟随AUV进行位置估计，实现多AUV协同定位导航；包括预测子模块和修正子模块，所述预测子模块配置成采用扩维ECKF算法对跟随AUV进行状态预测和量测预测；所述修正子模块配置成在状态预测和量测预测中引入强跟踪算法中的渐消因子，以更新跟随AUV的状态估计值，保证每个时刻滤波残差的正交性。

进一步地，所述系统方程构建模块中所述主从式多AUV协同导航系统的离散时间状态方程和量测方程表示为：

其中，表示k时刻跟随AUV的n维状态变量，x_k、y_k、分别表示跟随AUV在k时刻的x轴坐标、y轴坐标及航向角；表示k时刻跟随AUV的传感器输入量，V_k表示k时刻跟随AUV的航行方向上的前向速度；表示k时刻跟随AUV由前向速度噪声和航向角传感器噪声组成的n_w维过程噪声向量；v_k＝[v_k]^T表示k时刻相对距离测量噪声v_k所构成的n_v维量测噪声向量；

其中，状态函数表示为：

式中，Δt表示航位推算的系统周期；w_k表示k时刻AUV航行的角速度；

量测函数表示为：

式中，上标M代表领航AUV；上标S代表跟随AUV。

进一步地，所述预测子模块中状态预测的具体过程包括：

假设k时刻后验概率密度函数中上一时刻状态估计值以及误差协方差P_k-1|k-1已知，对P_k-1|k-1进行Cholesky分解得到过程变量C_k-1|k-1：

使用n_w维过程噪声对状态变量进行扩维，得到过程变量以及

计算容积点X_i,k-1|k-1：

其中，i＝1,2,…,N,ξ_i表示容积点集；Q_k-1表示k-1时刻的过程噪声协方差矩阵；

根据ECKF算法中的嵌入式容积准则，替换容积点集ξ_i以及权值

式中，[0]_i为维数为n的零向量；[δ]＝[s₁σ²,s₁σ²,…,s₁σ²]^T，σ＝1且s₁＝±1；

根据系统函数传播容积点X_i,k-1|k-1的状态变量分量以及过程噪声分量得到传播后的容积点

根据系统函数传播后的容积点计算k时刻的状态预测值

计算k时刻的状态误差协方差预测值P_k|k-1：

进一步地，所述预测子模块中量测预测的具体过程包括：

通过量测函数传播的状态变量分量

根据量测函数传播后的容积点计算k时刻的观测预测值

式中，r_k＝E(v_k)为v_k的均值；

计算k时刻的自相关协方差矩阵P_ZZ,k|k-1：

式中，表示量测噪声协方差矩阵；

计算k时刻互相关协方差矩阵P_XZ,k|k-1：

进一步地，所述修正子模块中修正多个AUV的状态估计值和状态误差协方差估计值的具体步骤包括：

通过k时刻实际相对距离量测值Z_k以及观测预测值计算残差γ_k：

计算滤波残差γ_k的协方差阵

式中，ρ表示遗忘因子；

计算中间过程变量N_k和M_k：

式中，β表示弱化因子；

计算自适应渐消因子λ_k：

在自相关协方差矩阵P_ZZ,k|k-1中引入渐消因子λ_k：

计算k时刻的卡尔曼滤波增益K_k：

计算k时刻状态估计值

计算k时刻的状态误差协方差估计值P_k|k：

本发明的有益技术效果是：

针对系统模型不确定或系统发生突变时引起的滤波发散、数值不稳定问题，本发明根据强跟踪滤波器(Strong Tracking Filter,STF)的原理，用STF对基于扩维ECKF的多AUV协同导航算法进行改进，以解决协同导航系统模型存在一定的不确定性或是系统发生突变时协同导航精度下降的问题，从而提高协同导航系统的定位导航性能。

附图说明

本发明可以通过参考下文中结合附图所给出的描述而得到更好的理解，所述附图连同下面的详细说明一起包含在本说明书中并且形成本说明书的一部分，而且用来进一步举例说明本发明的优选实施例和解释本发明的原理和优点。

图1是本发明实施例一种基于强跟踪扩维ECKF算法的多AUV协同导航方法的流程图；

图2是本发明实施例中实验仿真过程的领航艇和跟随艇真实航迹示例图；

图3是本发明实施例中实验仿真过程中本发明方法与另外两种方法的导航结果对比图；

图4是本发明实施例中实验仿真过程中本发明方法与另外两种方法的导航结果误差曲线对比图；其中，(a)对应x轴与y轴的位置误差曲线比较；(b)对应航向角的误差曲线比较；

图5是本发明实施例中实验仿真过程中本发明方法与另外两种方法的导航结果-定位误差曲线对比图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，在下文中将结合附图对本发明的示范性实施方式或实施例进行描述。显然，所描述的实施方式或实施例仅仅是本发明一部分的实施方式或实施例，而不是全部的。基于本发明中的实施方式或实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式或实施例，都应当属于本发明保护的范围。

本发明实施例提供一种基于强跟踪扩维ECKF算法的多AUV协同导航方法，如图1所示，该方法包括以下步骤：

步骤一：建立AUV协同定位系统的离散时间状态方程和量测方程，表示为：

其中，表示跟随AUV在k时刻的n维状态变量，且x_k、y_k及分别代表跟随AUV在k时刻的x轴位置，y轴位置及航向角；表示看k时刻跟随AUV的传感器输入量，且V_k表示跟随AUV在k时刻航行方向上的前向速度；是k时刻跟随AUV获取前向速度噪声以及航向角传感器噪声所构成的n_w维过程噪声向量；v_k＝[v_k]^T是k时刻相对距离测量噪声v_k所构成的n_v维量测噪声向量。

考虑过程噪声协方差矩阵与量测噪声协方差矩阵是正定对称阵且存在非线性系统互不相关的系统过程噪声w_k和观测噪声v_k，w_k的均值为0且v_k的均值为r_k，假设系统具有如式(2)所示的时变统计特性：

状态函数为：

式中，Δt表示航位推算的系统周期；w_k为k时刻AUV航行的角速度。

量测函数为：

式中，上标M代表领航AUV；上标S代表跟随AUV。

对于多AUV协同导航非线性系统，采用扩维ECKF滤波对状态变量和过程噪声进行容积变换，同时引入强跟踪滤波，从而提出扩维STF-ECKF协同导航算法。

步骤二：状态预测

假设k时刻后验概率密度函数中上一时刻状态估计值以及误差协方差P_k-1|k-1已知，对P_k-1|k-1进行Cholesky分解可以得到过程变量C_k-1|k-1：

使用n_w维过程噪声对状态变量进行扩维可得过程变量以及

计算容积点X_i,k-1|k-1

根据ECKF算法中的嵌入式容积准则，替换容积点集ξ_i以及权值

式中，[0]_i为维数为n的零向量；[δ]＝[s₁σ²,s₁σ²,…,s₁σ²]^T为容积点集且下标i表示的是在点集中此基本点的序号。令σ＝1，且s₁＝±1，对n维容积点集[s₁σ²,s₁σ²,…,s₁σ²]^T中的元素进行全排列可得：

通过系统函数传播容积点X_i,k-1|k-1的状态变量分量以及过程噪声分量可得传播后的容积点

通过系统函数传播后的容积点计算k时刻的状态预测值

计算k时刻的状态误差协方差预测值P_k|k-1：

步骤三：量测预测

通过量测函数传播的状态变量分量可得传播后的容积点Z_i,k|k-1：

通过量测函数传播后的容积点计算k时刻的观测预测值

计算k时刻的自相关协方差矩阵P_ZZ,k|k-1：

计算k时刻互相关协方差矩阵P_XZ,k|k-1：

步骤四：渐消因子的计算与引入

通过k时刻实际相对距离量测值Z_k以及观测预测值计算残差γ_k：

计算滤波残差γ_k的协方差阵

式中，ρ为遗忘因子，一般取0.95≤ρ≤0.995。

计算中间过程变量N_k和M_k：

式中，β为弱化因子，可以取β_k＝1-(1-ρ)/(1-ρ^k-1)。

计算自适应渐消因子λ_k：

在状态预测协方差矩阵P_ZZ,k|k-1中引入渐消因子λ_k：

步骤五：状态更新

计算k时刻的卡尔曼滤波增益K_k：

计算k时刻状态估计值

计算k时刻的状态误差协方差估计值P_k|k：

跟随AUV通过更新的状态估计值可以完成自身位置的估计与修正，同时将更新的状态估计值以及状态误差协方差估计值P_k|k作为下一次迭代中后验概率密度函数的初始值，并不断重复步骤二至步骤五实现多AUV系统的协同导航。

进一步通过实验验证本发明的技术效果。

在二维平面上，建立一个基于单领航艇的多AUV协同导航系统，系统中领航艇和跟随艇仅设置了一艘。假设领航艇通过自主导航的定位误差足够小，而跟随艇通过船位推算自主导航技术对自身进行定位，与此同时领航艇和跟随艇间只进行相对距离量测以获取相对距离信息。

领航艇沿某一方向进行匀速直线航行，初始位置坐标设置为(0,500)，初始航向角设置为45°；跟随艇进行曲线圆周运动，初始位置坐标设置为(500,500)，初始航向角同样设置为45°，这样能够尽量使领航AUV和跟随AUV相邻采样时刻相对方位变化更大，从而提高系统的可观测性；同时领航艇和跟随艇的运动速度在正常情况下保持为2m/s。系统状态更新周期Δt＝10s，运载体航行时间为T＝2500s。可得领航艇和跟随艇真实航迹，如图2所示。

跟随艇通过自身携带的速度传感器获取自身的速度信息，通过自身携带的方位传感器获取自身的航向角，显然在复杂的海洋环境下测量过程中的量测噪声是无法避免的，假设上述两种传感器的量测噪声满足高斯白噪声的条件，互不相关的同时其均值为零，设置这两种噪声的方差分别为和除此之外，领航艇通过声学测距的方法对艇间相对距离进行测量，测量时的量测噪声依旧满足高斯白噪声条件，设置其方差为

取扩维的状态估计初始值为：

P₀₀＝diag[1 1 0.01 0 0] (29)

为了验证本发明方法对系统发生突变时依旧能够在较短时间内跟踪上从AUV，从而保证定位的精度，在系统时间为800s以及1400s时对从AUV设置了两次强机动，速度提高到3m/s并保持10s，以此来验证本发明方法对于多AUV协同导航的有效性。

图3给出了跟随AUV的真实运动轨迹以及分别采用CKF，扩维ECKF与本发明方法—扩维STF-ECKF协同导航算法得到的目标运动轨迹。根据对图3进行分析可知，跟随AUV做匀速圆周运动时基于扩维STF-ECKF与其他协同导航方法在系统发生突变时的定位精度的优劣对比。显而易见，根据本发明方法对跟随AUV进行协同导航定位的航迹几乎与跟随AUV的真实航迹相重合，在系统发生突变时也能快速恢复较高的定位精度，而根据基于其他滤波的协同导航方法所得的航迹与真实航迹存在不同程度的偏差。

图4(a)为三种算法得到的x轴与y轴位置误差曲线比较，图4(b)为三种算法的航向角的误差曲线比较。由图4(a)与图4(b)可知，在跟随AUV突然加速的情况下，三种算法的x轴与y轴位置误差以及航向角误差波形均出现不同大小的尖峰，本发明方法的尖峰最小，能有效地减小系统发生突变的情况下扩维ECKF算法协同定位的x轴与y轴位置以及航向角误差上升的幅度，而CKF算法的误差曲线收敛速度缓慢，难以保持较好的鲁棒性并存在发散的趋势。

图5为三种算法得到的定位误差曲线比较。通过分析图5可知，在系统时间800s以及1400s系统发生突变时，本发明方法相较于扩维ECKF算法的定位误差具有更快的收敛速度，且误差曲线波动幅度更小，说明其具有较好的准确性以及鲁棒性。而CKF算法的最大误差距离以及误差曲线波动程度出现了大幅度波动，具有发散的趋势，相较于本发明方法难以保持较高的准确性以及稳定性。表1是仿真场景中跟随AUV的x轴与y轴位置和航向角以及定位误差的均方根统计数据。

表1三种算法协同导航性能比较

结合图5以及表1可知，在两种场景下，通过本发明方法得到的位置、航向角和定位误差均小于采用CKF和扩维ECKF算法协同导航得到的误差。相较于CKF和扩维ECKF算法，本发明方法在仿真场景中协同导航性能提升了18.62％。由此可以看出特别是在系统出现扰动或是与状态发生突变的情况下，本发明方法能够有效抑制跟随AUV定位误差发散，同时减小协同定位误差，提高协同导航精度和稳定性。

表2列出了仿真场景下3种不同算法的总运行时间以及平均每次迭代的时间。3种算法都具有较高的运算效率，由于采用了嵌入式容积准则的扩维ECKF算法增加了容积点，使得运行时间相较于CKF有所增加。考虑到运算量，本发明方法将量测噪声从状态变量中取出并引入渐消因子，使状态变量的维度降低，减少了容积点个数，降低了计算的复杂度，虽然采用强跟踪算法会增加一定运算时间，但是总运行时间有所减少。相较于扩维ECKF算法，本发明方法在场景中协同导航效率提升了39.13％。

表2仿真场景中3种算法的运行时间

本发明另一实施例还提供一种基于强跟踪扩维ECKF算法的多AUV协同导航系统，该系统包括：

系统方程构建模块，其配置成建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；

协同定位模块，其配置成采用扩维ECKF算法对跟随AUV进行位置估计，实现多AUV协同定位导航；包括预测子模块和修正子模块，所述预测子模块配置成采用扩维ECKF算法对跟随AUV进行状态预测和量测预测；所述修正子模块配置成在状态预测和量测预测中引入强跟踪算法中的渐消因子，以更新跟随AUV的状态估计值，保证每个时刻滤波残差的正交性。

本发明实施例一种基于强跟踪扩维ECKF算法的多AUV协同导航系统的功能可以由前述一种基于强跟踪扩维ECKF算法的多AUV协同导航方法说明，因此系统实施例未详述部分，可参见以上方法实施例，在此不再赘述。

尽管根据有限数量的实施例描述了本发明，但是受益于上面的描述，本技术领域内的技术人员明白，在由此描述的本发明的范围内，可以设想其它实施例。对于本发明的范围，对本发明所做的公开是说明性的，而非限制性的，本发明的范围由所附权利要求书限定。

Claims (4)

1.一种基于强跟踪扩维ECKF算法的多AUV协同导航方法，其特征在于，包括以下步骤：

步骤一、建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；

步骤二、采用扩维ECKF算法对跟随AUV进行状态预测和量测预测；其中，

状态预测的具体过程包括：

假设k时刻后验概率密度函数中上一时刻状态估计值以及误差协方差P_k-1|k-1已知，对P_k-1|k-1进行Cholesky分解得到过程变量C_k-1|k-1：

使用n_w维过程噪声对状态变量进行扩维，得到过程变量以及

计算容积点X_i,k-1|k-1：

其中，i＝1,2,…,N,ξ_i表示容积点集；Q_k-1表示k-1时刻的过程噪声协方差矩阵；

根据ECKF算法中的嵌入式容积准则，替换容积点集ξ_i以及权值

式中，[0]_i为维数为n的零向量；[δ]＝[s₁σ²,s₁σ²,…,s₁σ²]^T，σ＝1且s₁＝±1；

根据系统函数传播容积点X_i,k-1|k-1的状态变量分量以及过程噪声分量得到传播后的容积点

根据系统函数传播后的容积点计算k时刻的状态预测值

计算k时刻的状态误差协方差预测值P_k|k-1：

量测预测的具体过程包括：

通过量测函数传播的状态变量分量

根据量测函数传播后的容积点计算k时刻的观测预测值

式中，r_k＝E(v_k)为v_k的均值；

计算k时刻的自相关协方差矩阵P_ZZ,k|k-1：

式中，表示量测噪声协方差矩阵；

计算k时刻互相关协方差矩阵P_XZ,k|k-1：

步骤三、在状态预测和量测预测中引入强跟踪算法中的渐消因子，以更新跟随AUV的状态估计值，保证每个时刻滤波残差的正交性；具体步骤包括：

通过k时刻实际相对距离量测值Z_k以及观测预测值计算残差γ_k：

计算滤波残差γ_k的协方差阵

式中，ρ表示遗忘因子；

计算中间过程变量N_k和M_k：

式中，β表示弱化因子；

计算自适应渐消因子λ_k：

在自相关协方差矩阵P_ZZ,k|k-1中引入渐消因子λ_k：

计算k时刻的卡尔曼滤波增益K_k：

计算k时刻状态估计值

计算k时刻的状态误差协方差估计值P_k|k：

2.根据权利要求1所述的一种基于强跟踪扩维ECKF算法的多AUV协同导航方法，其特征在于，步骤一中所述主从式多AUV协同导航系统的离散时间状态方程和量测方程表示为：

其中，表示k时刻跟随AUV的n维状态变量，x_k、y_k、分别表示跟随AUV在k时刻的x轴坐标、y轴坐标及航向角；表示k时刻跟随AUV的传感器输入量，V_k表示k时刻跟随AUV的航行方向上的前向速度；表示k时刻跟随AUV由前向速度噪声和航向角传感器噪声组成的n_w维过程噪声向量；v_k＝[v_k]^T表示k时刻相对距离测量噪声v_k所构成的n_v维量测噪声向量；

其中，状态函数表示为：

式中，Δt表示航位推算的系统周期；ω_k表示k时刻AUV航行的角速度；

量测函数表示为：

式中，上标M代表领航AUV；上标S代表跟随AUV。

3.一种基于强跟踪扩维ECKF算法的多AUV协同导航系统，其特征在于，包括：

系统方程构建模块，其配置成建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；

协同定位模块，其配置成采用扩维ECKF算法对跟随AUV进行位置估计，实现多AUV协同定位导航；包括预测子模块和修正子模块，所述预测子模块配置成采用扩维ECKF算法对跟随AUV进行状态预测和量测预测；所述修正子模块配置成在状态预测和量测预测中引入强跟踪算法中的渐消因子，以更新跟随AUV的状态估计值，保证每个时刻滤波残差的正交性；其中所述预测子模块中状态预测的具体过程包括：

假设k时刻后验概率密度函数中上一时刻状态估计值以及误差协方差P_k-1|k-1已知，对P_k-1|k-1进行Cholesky分解得到过程变量C_k-1|k-1：

使用n_w维过程噪声对状态变量进行扩维，得到过程变量以及

计算容积点X_i,k-1|k-1：

其中，i＝1,2,…,N,ξ_i表示容积点集；Q_k-1表示k-1时刻的过程噪声协方差矩阵；

根据ECKF算法中的嵌入式容积准则，替换容积点集ξ_i以及权值

式中，[0]_i为维数为n的零向量；[δ]＝[s₁σ²,s₁σ²,…,s₁σ²]^T，σ＝1且s₁＝±1；

根据系统函数传播容积点X_i,k-1|k-1的状态变量分量以及过程噪声分量得到传播后的容积点

根据系统函数传播后的容积点计算k时刻的状态预测值

计算k时刻的状态误差协方差预测值P_k|k-1：

量测预测的具体过程包括：

通过量测函数传播的状态变量分量

根据量测函数传播后的容积点计算k时刻的观测预测值

式中，r_k＝E(v_k)为v_k的均值；

计算k时刻的自相关协方差矩阵P_ZZ,k|k-1：

式中，表示量测噪声协方差矩阵；

计算k时刻互相关协方差矩阵P_XZ,k|k-1：

所述修正子模块中修正多个AUV的状态估计值和状态误差协方差估计值的具体步骤包括：

通过k时刻实际相对距离量测值Z_k以及观测预测值计算残差γ_k：

计算滤波残差γ_k的协方差阵

式中，ρ表示遗忘因子；

计算中间过程变量N_k和M_k：

式中，β表示弱化因子；

计算自适应渐消因子λ_k：

在自相关协方差矩阵P_ZZ,k|k-1中引入渐消因子λ_k：

计算k时刻的卡尔曼滤波增益K_k：

计算k时刻状态估计值

计算k时刻的状态误差协方差估计值P_k|k：

4.根据权利要求3所述的一种基于强跟踪扩维ECKF算法的多AUV协同导航系统，其特征在于，所述系统方程构建模块中所述主从式多AUV协同导航系统的离散时间状态方程和量测方程表示为：

其中，表示k时刻跟随AUV的n维状态变量，x_k、y_k、分别表示跟随AUV在k时刻的x轴坐标、y轴坐标及航向角；表示k时刻跟随AUV的传感器输入量，V_k表示k时刻跟随AUV的航行方向上的前向速度；表示k时刻跟随AUV由前向速度噪声和航向角传感器噪声组成的n_w维过程噪声向量；v_k＝[v_k]^T表示k时刻相对距离测量噪声v_k所构成的n_v维量测噪声向量；

其中，状态函数表示为：

式中，Δt表示航位推算的系统周期；ω_k表示k时刻AUV航行的角速度；

量测函数表示为：

式中，上标M代表领航AUV；上标S代表跟随AUV。