[發明專利]一種估計初始航向偏差濾波的方法

在審

• 申請號: 201310713137.7
• 申請日: 2013-12-23
• 公開（公告）號: CN103712625A
• 公開（公告）日: 2014-04-09
• 發明（設計）人: 徐博;金辰;劉楊;單為;邱立民;董海波;白金磊;陶冠時;易楚偉
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

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信號后，再進行時間更新和量測更新，用主從艇的距離對從艇的位置進行更新修正。