[發(fā)明專(zhuān)利]一種多模型水下航行器組合導(dǎo)航濾波方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201410030165.3
• 申請(qǐng)日: 2014-01-22
• 公開(kāi)（公告）號(hào): CN103776453A
• 公開(kāi)（公告）日: 2014-05-07
• 發(fā)明（設(shè)計(jì)）人: 程向紅;王磊;冉昌艷;陳紅梅;周玲;朱倚嫻
• 申請(qǐng)（專(zhuān)利權(quán)）人: 東南大學(xué)
• 主分類(lèi)號(hào): G01C21/20
• 分類(lèi)號(hào): G01C21/20
• 代理公司: 江蘇永衡昭輝律師事務(wù)所 32250
• 代理人: 王斌

1.一種多模型水下航行器組合導(dǎo)航濾波方法，包括下列步驟：

(1)建立SINS/DVL/TAN/MCP組合導(dǎo)航系統(tǒng)的狀態(tài)方程、觀(guān)測(cè)方程及噪聲模型；

(2)根據(jù)步驟(1)獲取的狀態(tài)方程、觀(guān)測(cè)方程和噪聲模型，建立由n個(gè)模型構(gòu)成的模型集；

(3)根據(jù)步驟(2)所獲取的模型集，構(gòu)建交互式多模型濾波結(jié)構(gòu)，采用n個(gè)并行的模型Kalman濾波器進(jìn)行濾波，獲取模型子濾波器的狀態(tài)估計(jì)、協(xié)方差陣以及殘差和殘差協(xié)方差陣，具體包括如下步驟：

3.1)模型轉(zhuǎn)移概率預(yù)測(cè)：將k-1時(shí)刻的模型轉(zhuǎn)移概率μ_j(k-1)和Markov轉(zhuǎn)移概率π_ji進(jìn)行交互，得到k時(shí)刻的預(yù)測(cè)模型轉(zhuǎn)移概率下標(biāo)i，j分別表示第i，j個(gè)模型；

3.2)重新初始化：根據(jù)k-1時(shí)刻模型子濾波器的狀態(tài)估計(jì)k-1時(shí)刻模型子濾波器的協(xié)方差陣估計(jì)P_j(k-1)，預(yù)測(cè)的模型轉(zhuǎn)移概率通過(guò)如下公式計(jì)算模型子濾波器的狀態(tài)與協(xié)方差陣P_oi(k-1)：

X^oi(k-1)=Σj=1nX^j(k-1)μ^j|i(k-1)]]>

Poi(k-1)=Σj=1nμ^j|i(k-1){Pj(k-1)+[X^j(k-1)-X^oi(k-1)][X^j(k-1)-X^oi(k-1)]T};]]>

3.3)模型濾波：根據(jù)步驟32)獲取的重初始化狀態(tài)和協(xié)方差陣P_oi(k-1)，在獲得新的量測(cè)z_k之后，利用Kalman濾波器進(jìn)行狀態(tài)估計(jì)更新，得到模型子濾波狀態(tài)估計(jì)協(xié)方差陣P_i(k)、殘差估值ε_i(k)和殘差方差S_i(k)；

(4)根據(jù)步驟(1)所述組合導(dǎo)航系統(tǒng)，選擇特征變量建立貝葉斯網(wǎng)絡(luò)，通過(guò)貝葉斯網(wǎng)絡(luò)參數(shù)對(duì)模型切換概率進(jìn)行修正；

(5)根據(jù)步驟(3)得到模型子濾波狀態(tài)估計(jì)協(xié)方差陣P_i(k)和步驟(4)得到的模式切換概率μ_i(k)，進(jìn)行概率加權(quán)融合得到系統(tǒng)狀態(tài)估計(jì)和協(xié)方差陣P(k)：

X^(k)=Σi=1nX^i(k)μi(k)]]>

P(k)=Σi=1nμi(k){Pi(k)+[X^i(k-1)-X^(k-1)][X^i(k-1)-X^(k-1)]T};]]>

(6)返回步驟(3)，從第(3)步重復(fù)以上步驟；

(7)完成組合導(dǎo)航的數(shù)據(jù)處理與解算工作。