[发明专利]一种估计初始航向偏差滤波的方法

摘要

本发明公开了一种估计初始航向偏差滤波的方法，该估计初始航向偏差滤波的方法包括以下步骤：准备装有高精度惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号；从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离；使用卡尔曼滤波算法对从艇的位置进行更新和修正，估计并补偿陀螺的初始航向偏差。本发明通过主艇无线电广播位置信息，从艇被动接受主艇发射的无线电信号，提高了扩展性，用两主艇和一从艇可以模拟多从艇的情形；从艇安装低精度惯导设备进行推位，并通过测量和主艇的距离利用主艇的高精度惯导设备，提高了定位精度，大大降低了设备的成本。

主权项

1.一种估计初始航向偏差滤波的方法，其特征在于，该估计初始航向偏差滤波的方法包括以下步骤：步骤一、将所有相关设备安装在从艇上，读取MEMS陀螺输出航向作为静态数据并保存，一小时后，主艇设备安装完毕，三艇在水面上行驶时保持三角队形，两主艇将水声通信信号和发送时间戳交替发送给从艇，间隔10秒，从艇上的水声通信模块接收主艇发来的测距信息，通过水声信号发送和接收的时间差乘以声速计算主从艇的距离，利用测量的速度和航向进行航迹推算，并利用主从艇的距离作为观测量修正位置信息并估计初始航向偏差；步骤二、建立系统方程：式中，x_k、y_k表示从艇在k时刻的位置，v_k为从艇速度，t表示推位时间间隔，表示MEMS陀螺测得的航向，表示初始航向偏差，在方程中估计值修正了航向测量值的偏差，表示成：X_k+1＝f(X_k，u_k，t)+w_k线性化后得，X_k+1＝F_kX_k+B_ku_k+w_k式中，系统噪声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)为测量噪声；步骤四、用扩展卡尔曼滤波修正从艇推位误差；(1)时间更新：X^k/k-1=FkX^k-1Pk/k-1=FkPk-1FkT+Bk-1Qk-1Bk-1T]]>(2)量测更新：Kk=Pk/k-1HkT(HkPk/k-1HkT+Rk)-1X^k=X^k/k-1+Kk(Zk-HkX^k/k-1)Pk=(I-KkHk)Pk/k-1]]>式中，Pk=E[(XK-X^k)(Xk-X^k)T],]]>为滤波输出的状态估计，P_k/k-1为状态和及方差一步预测，K_k为滤波增益。