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

权利要求书

1.一种罗经失效下的多潜航器协同定位自适应调节鲁棒滤波方法，其特征在于，包括以下步骤：

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

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

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

量测噪声方差矩阵的自适应更新，即：

且有其中T_P,k|k-1与T_r,k分别为与量测噪声方差阵R_k经过三角分解后的下三角矩阵；在协同定位模型中，假设系统噪声为高斯噪声，因此有：

式中σ为核带宽度，且有：

e_k＝ζ_k-G_kx_k

式中m为量测值的维数，ε_k为量测噪声；

步骤四：构建核带宽度自适应调节因子，具体为：

新息向量定义为通过新息矩阵和量测误差协方差矩阵构建自适应调节因子：

式中当新息矩阵的迹小于等于量测误差方差矩阵的迹时，自适应因子取值为1，否则，利用构造的自适应因子对带宽σ进行实时校正，即σ_t＝λ_tσ_t-1，t为迭代次数；

步骤五：计算航向估计值与校正后的位置向量估计值，具体为：

航向角的误差协方差矩阵为：

式中

位置与航向估计的增益矩阵分别为：

航向估计值为：

由航向角估计值校正后的位置估计与位置估计误差协方差矩阵分别为：

式中

2.根据权利要求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作为估计变量，满足

式中为零均值白噪声；

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

式中x_k＝[a_k b_k]^T为k时刻随从潜航器的位置，n^a＝n+1，n为x_k的维数，和分别为状态扩展后k时刻的非线性状态函数和量测函数，假设为高斯白噪声序列，y_k为真实量测信息，且有：

式中，u_k＝[v_a,k v_b,k]^T为DVL测得的前向与右向速度，为k-1时刻航向角的估计值；

式中

式中，为k时刻的非线性状态函数。

3.根据权利要求1或2所述的一种罗经失效下的多潜航器协同定位自适应调节鲁棒滤波方法，其特征在于：步骤二所述计算航向角输入未知的待校正状态估计值具体为：

状态估计值的误差协方差可以被分解为：

式中S_k-1/k-1为经过三角分解后的下三角矩阵；

容积点计算具体为：

式中[I_n]_i表示n维单位矩阵I_n的第i列；

容积点传播具体为：

y_k/k-1,i＝h_k(χ_k/k-1,i)

式中为k-1时刻非线性状态函数f_k-1(·)展开的采样点，为状态预测值，为的误差协方差矩阵，χ_k/k-1,i为二次采样点，y_k/k-1,i为非线性量测函数h_k(·)展开的采样点，为量测预测值，Q_k为过程噪声协方差矩阵，ξ_i与ρ_i的取值方式相同；

量测更新为：

式中为互协方差矩阵，为量测自协方差矩阵，R_k为量测噪声协方差矩阵，K_k为增益矩阵，y_k为真实量测信息，分别为无航向输入时的状态估计值与误差协方差矩阵。