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

权利要求书

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为滤波增益。

2.如权利要求1所述的估计初始航向偏差滤波的方法，其特征在于，在步骤四中，由于GPS容易受到干扰，当收不到GPS信号或GPS信号出现错误时，只计算时间更新部分，即航迹推算，不计算量测更新部分，当收到正确的GPS信号后，再进行时间更新和量测更新，用主从艇的距离对从艇的位置进行更新修正。