技術(shù)特征：

1.一種具有自適應(yīng)的協(xié)同導(dǎo)航濾波方法，其特征在于，包括如下步驟：

(1)輸入交互過(guò)程：

通過(guò)輸入交互過(guò)程、模型濾波、模型概率更新以及輸出交互過(guò)程四個(gè)過(guò)程完成狀態(tài)估計(jì)；

在輸入交互過(guò)程中，首次通過(guò)預(yù)驗(yàn)?zāi)Ｐ透怕诗@取混合狀態(tài)，下次循環(huán)則通過(guò)計(jì)算得到的模型概率替換；

協(xié)同導(dǎo)航從AUV交互混合狀態(tài)估計(jì)及協(xié)方差為：

${\hat{x}}_{0i}(k-1)=\underset{j=1}{\overset{2}{\Σ}}{\hat{x}}_j(k-1){\mathrm{\μ}}_{j|i}(k-1)$

$P_{0i}(k-1)=\underset{j=1}{\overset{2}{\Σ}}{\mathrm{\μ}}_{j|i}(k-1)\{P_j(k-1)+\[{\hat{x}}_j(k-1)-{\hat{x}}_{0i}(k-1)\]\·$

$\[{\hat{x}}_i(k-1)-{\hat{x}}_{0i}{(k-1)}^T\]\}$

式中為k-1時(shí)刻第j各濾波器的狀態(tài)估計(jì)；P_j(k-1)為對(duì)應(yīng)的協(xié)方差矩陣；P_0i(k-1)為狀態(tài)估計(jì)所對(duì)應(yīng)的狀態(tài)誤差協(xié)方差矩陣；

首次濾波中，用初始模型預(yù)測(cè)概率代替模型條件轉(zhuǎn)移概率進(jìn)行運(yùn)算，下個(gè)濾波周期中通過(guò)利用上一個(gè)周期各個(gè)濾波器的狀態(tài)估計(jì)和模型條件轉(zhuǎn)移概率進(jìn)行運(yùn)算；

(2)模型濾波

基于EKF的協(xié)同導(dǎo)航濾波器1工作：

系統(tǒng)狀態(tài)一步預(yù)測(cè)：其中表示從AUV在時(shí)刻k+1的一步估計(jì)狀態(tài)，f為從AUV運(yùn)動(dòng)學(xué)方程，為從AUV交互混合估計(jì)狀態(tài)1，u_k量測(cè)輸入；

一步預(yù)測(cè)協(xié)方差矩陣為：

其中F_k為狀態(tài)轉(zhuǎn)移矩陣，P_k為上一時(shí)刻的的協(xié)方差矩陣，G_k為過(guò)程噪聲分布矩陣，Q_k表示噪聲協(xié)方差矩陣；

量測(cè)量預(yù)測(cè)為：

其中h表示量測(cè)方程，分別為領(lǐng)航者AUV1，AUV2的狀態(tài)。

殘差信息和其協(xié)方差：

其中Z_k+1為傳感器接收到的量測(cè)值，H_k+1為量測(cè)矩陣的雅可比矩陣，為濾波器1中的量測(cè)噪聲協(xié)方差矩陣；

濾波增益：

狀態(tài)估計(jì)及誤差協(xié)方差矩陣：

P_1,k+1＝(I-K_k+1H_k+1)P_k+1,k，其中I為單位矩陣。

基于EKF的協(xié)同導(dǎo)航濾波器2中不同于濾波器1的是量測(cè)噪聲協(xié)方差矩陣選取了另外一組其他步驟相同，并得到濾波器2下殘差信息v_2,k+1和其協(xié)方差S_2,k+1，狀態(tài)估計(jì)及其誤差協(xié)方差矩陣P_2,k+1；

(3)模型概率更新

通過(guò)獲取各個(gè)濾波器的殘差信息并且結(jié)合貝葉斯假設(shè)檢驗(yàn)方法，得到各模型的似然函數(shù)：

$f_i\left(k\right)={\[{\left(2\mathrm{\π}\right)}^m\left|S_i\left(k\right)\right|\]}^{1/2}\·\exp (-\frac{1}{2}{v}_i^T\left(k\right)S_i^{-1}\left(k\right)v_i\left(k\right))$

m＝3表示協(xié)同導(dǎo)航中量測(cè)向量的維數(shù)；v_i(k)為協(xié)同導(dǎo)航模型在k時(shí)刻時(shí)通過(guò)卡爾曼濾波獲得的殘差向量；S_i(k)為對(duì)應(yīng)殘差協(xié)方差矩陣；

結(jié)合上一時(shí)刻的模型概率以及模型先驗(yàn)信息，得到k時(shí)刻的模型概率為

${\mathrm{\μ}}_i\left(k\right)=\frac{f_i\left(k\right)\underset{j=1}{\overset{2}{\Σ}}{\mathrm{\π}}_{ji}{\mathrm{\μ}}_i\left(k-1\right)}{\underset{i=1}{\overset{2}{\Σ}}{f}_i\left(k\right)\underset{j=1}{\overset{2}{\Σ}}{\mathrm{\π}}_{ji}{\mathrm{\μ}}_i\left(k-1\right)};$

(4)交互輸出

由前面各個(gè)基于不同模型濾波器的狀態(tài)估計(jì)值及其模型概率加權(quán)融合得到估計(jì)結(jié)果，結(jié)合各模型的從AUV聯(lián)合狀態(tài)估計(jì)和協(xié)方差矩陣P(k)計(jì)算如下：

至此完成一次輸出，進(jìn)入下一時(shí)刻的狀態(tài)估計(jì)。