[发明专利]一种多无人艇协同导航条件下陀螺漂移的滤波方法

权利要求书

1.一种多无人艇协同导航条件下陀螺漂移的滤波方法，其特征在于，该多无人艇协同导航条件下陀螺漂移的滤波方法包括以下步骤：

步骤一，装有惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号；

步骤二，从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离；

步骤三，估计初始航向偏差：

在采用卡尔曼滤波方法估计MEMS陀螺初始航向偏差的这段时间里，陀螺漂移的影响小到可以忽略不计的地步，所以陀螺漂移先不作为系统状态，设系统状态X_k=[x_k y_k X(3)]^T,x_k、y_k表示从艇在k时刻的位置，先在t₀秒内准确估计出初始航向偏差t₀秒后转而估计陀螺漂移ε，即

步骤四，补偿初始航向偏差：

在MEMS陀螺测得的航向中完全消除估计出的初始航向偏差，即

式中表示MEMS陀螺测得的航向，在以后的航向测量值中不再包含初始航向偏差，只有陀螺漂移，在t₀后，今X(3)=0，并重新设置系统噪声方差阵，为估计陀螺漂移做好准备；

步骤五，估计并补偿陀螺漂移：

式中v_k为从艇速度，t表示推位时间间隔，ε表示MEMS陀螺漂移，k表示算法执行次数，注意到项表明算法在状态一步预测中用陀螺漂移的估计值补偿了航向测量值的误差，表示成一般形式得：

X_k+1=f(X_k，u_k，t)+w_k

线性化后得，

X_k+1=F_kX_k+B_ku_k+w_k

式中，X_k=[x_k y_k ε]^T，系统噪声w_k～N(0，Q_k)，

步骤六，建立量测方程：

Zk=r=h(Xk)=(xa-xb)2+(ya-yb)2+Vk=HX+Vk]]>

式中，观测量Z_k表示主从艇的距离r，x_a、y_a表示主艇的位置，x_b、y_b表示从艇位置，H=∂h∂X=(xb-xa)/r(yb-ya)/r0,]]>V_k～N(0，R_k)为测量噪声；

步骤七，用扩展卡尔曼滤波修正从艇推位误差：

X^k/k-1=FkX^k-1]]>

Pk/k-1=FkPk-1FkT+Bk-1Qk-1Bk-1T]]>

Kk=Pk/k-1HkT(HkPk/k-1HkT+Rk)-1]]>

X^k=X^k/k-1+Kk(Zk-HkX^k/k-1)]]>

P_k=(I-K_kH_k)P_k/k-1

式中，Pk=E[(Xk-X^k)(Xk-X^k)T],]]>为滤波输出的状态估计，为状态和方差一步预测，K_k为滤波增益。