[发明专利]罗经失效下的多潜航器协同定位自适应调节鲁棒滤波方法

说明书

本发明提供一种罗经失效下的多潜航器协同定位自适应调节鲁棒滤波方法，包括以下步骤：步骤一：建立航向角未知的多潜航器协同定位模型；步骤二：计算航向角输入未知的待校正状态估计值；步骤三：通过自适应更新量测噪声方差矩阵削弱野值噪声对状态估计的影响；步骤四：构建核带宽度自适应调节因子；步骤五：计算航向估计值与校正后的位置向量估计值。本发明在保证不扩展状态维数的基础上，实现航向角未知伴随非高斯噪声条件下的准确定位。本发明可用于非理想条件下的多潜航器协同定位领域。

技术领域

本发明属于非理想条件下的多潜航器协同定位领域，涉及一种罗经失效下的多潜航器协同定位自适应调节鲁棒滤波方法。

背景技术

协同定位是目前中间层区域多潜航器最有效的导航方法之一。然而，协同系统的定位性能经常受多种因素的制约，在协同定位过程中，一个重要的问题是如何在非理想条件下对潜航器的位置进行估计。多潜航器协同定位系统是一个非线性系统，当其航向信息缺失伴随噪声非高斯分布时，将对定位精度产生极大的影响。罗经的电路结构复杂，低成本罗经的使用也会造成故障出现概率的增大。一般情况下，当罗经失效时，通常采用扩展状态维数的方法来估计航向角，然而，计算量将随之增加，特别是当未知故障向量维数与系统状态维数相当时，计算代价更大。针对维数问题，二阶扩展卡尔曼滤波、二阶无迹卡尔曼滤波相继被提出，但其受限于故障向量的统计模型未知且不适用于噪声呈非高斯分布的环境。

因此，探讨罗经失效与非高斯噪声对协同定位系统定位精度的影响机理，考虑如何在不扩展状态维数且克服非高斯噪声干扰的前提下对实时变化的航向角进行估计，是协同定位方向需要解决的技术问题。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种在不扩展状态维数且航向噪声序列未知的前提下、在克服非高斯噪声干扰的同时对变化的航向角进行实时估计的罗经失效下的多潜航器协同定位自适应调节鲁棒滤波方法，以提高协同定位精度。

为解决上述技术问题，本发明的一种罗经失效下的多潜航器协同定位自适应调节鲁棒滤波方法，包括以下步骤：

步骤一：建立航向角未知的多潜航器协同定位模型；

步骤二：计算航向角输入未知的待校正状态估计值；

步骤三：通过自适应更新量测噪声方差矩阵削弱野值噪声对状态估计的影响；

步骤四：构建核带宽度自适应调节因子；

步骤五：计算航向估计值与校正后的位置向量估计值。

本发明还包括：

1.步骤一中建立航向角未知的多潜航器协同定位模型具体为：

状态方程与量测方程如下：

式中a_k和b_k分别为随从潜航器k时刻的东向与北向位置，Δt为采样周期，v_a,k,v_b,k为随从潜航器k时刻的右向与前向速度，θ_k为k时刻前向与北向的夹角，w_a,k,w_b,k为零均值高斯白噪声，Z_k为领航潜航器与随从潜航器间的相对距离量测信息，为领航潜航器k时刻的位置，ε_k为量测噪声；

将θ_k作为估计变量，满足

式中为零均值白噪声；

离散时间状态空间模型建立具体为：