聯(lián)邦濾波器在船舶導(dǎo)航中的應(yīng)用.doc

聯(lián)邦濾波器在船舶導(dǎo)航中的應(yīng)用1 準(zhǔn)備知識(shí)So federal filter with its design flexibility and small computation, fault tolerance is good wait for a characteristic to achieve high precision navigation provides an effective way. 聯(lián)邦濾波器由多個(gè)子濾波器和一個(gè)主濾波器組成，是一種具有兩級(jí)數(shù)據(jù)處理的分散化濾波方法。子濾波器的工作狀態(tài)與其所受干擾情況密切相關(guān)。當(dāng)干擾大到足以影響濾波器正常工作時(shí)，一般就認(rèn)為子濾波器發(fā)生了故障。為了描述子濾波器的工作狀態(tài)，定義如下新息序列：Ui,k=Zi,k-Hi,kXi,k/(k-1) 當(dāng)子濾波器工作存在干擾時(shí)，觀測(cè)方程為：Zi,k=Hi,kXi,k+Vi,k 新息序列也隨之變?yōu)椋簎i,k=Hi,kXi,k-Xi,kk-1 +Vi,k 2 工作原理2.1 集中式卡爾曼濾波概述卡爾曼濾波是解決狀態(tài)最優(yōu)估計(jì)問題的一種常用方法。設(shè)隨機(jī)動(dòng)態(tài)系統(tǒng)的數(shù)學(xué)模型和有關(guān)隨機(jī)向量的統(tǒng)計(jì)性質(zhì)如下： 式中。是系統(tǒng)狀態(tài)向量；是系統(tǒng)觀測(cè)向量；是系統(tǒng)噪聲向量；是觀測(cè)噪聲向量；是系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；是系統(tǒng)噪聲矩陣；是系統(tǒng)觀測(cè)矩陣。假定系統(tǒng)噪聲和觀測(cè)噪聲是不相關(guān)的零均值高斯白噪聲，初始狀態(tài)向量是m維高斯隨機(jī)向量，且： 隨機(jī)系統(tǒng)的狀態(tài)估計(jì)問題，就是根據(jù)選定的估計(jì)準(zhǔn)則和獲得的量測(cè)信息，對(duì)系統(tǒng)狀態(tài)進(jìn)行估計(jì)。其中狀態(tài)估計(jì)確定了被估計(jì)的隨機(jī)狀態(tài)的轉(zhuǎn)移過程，估計(jì)準(zhǔn)則確定了狀態(tài)估計(jì)最優(yōu)性的含義，卡爾曼濾波的估計(jì)準(zhǔn)則是：估計(jì)量是的無(wú)偏估計(jì)，即；同時(shí)也是的最小方差估計(jì)，即：。根據(jù)這兩個(gè)準(zhǔn)則推導(dǎo)出系統(tǒng)的完整的濾波算法。下面給出最終的結(jié)果：濾波方程： 預(yù)測(cè)方程： 增益矩陣： 預(yù)測(cè)誤差協(xié)方差陣： 波誤差協(xié)方差陣： 初值為 ,。卡爾曼濾波算法是一組遞推算法。計(jì)算最優(yōu)濾波值，只需要即時(shí)的觀測(cè)值，無(wú)需存儲(chǔ)以前的觀測(cè)數(shù)據(jù)。利用上一時(shí)刻的最優(yōu)估計(jì)值，由預(yù)測(cè)方程求得下一時(shí)刻的一步預(yù)測(cè)值，；當(dāng)前測(cè)量值和之差，即新息，與相乘對(duì)進(jìn)行修正，得出k+1時(shí)刻的最優(yōu)估計(jì)值。如此反復(fù)遞推運(yùn)算，但這個(gè)計(jì)算順序不是唯一的。在該計(jì)算中，我們把預(yù)測(cè)下一時(shí)刻的和稱為濾波時(shí)間的更新，把得到和的過程稱為濾波的測(cè)量更新。2.2 聯(lián)邦濾波基本理論卡爾曼濾波及聯(lián)邦濾波一個(gè)重要的應(yīng)用是高精度船舶導(dǎo)航系統(tǒng)?，F(xiàn)在可供運(yùn)載體使用的導(dǎo)航裝備很多，選擇余地很大。由于采用各種原理實(shí)現(xiàn)導(dǎo)航設(shè)備的增加使量測(cè)信息增多，這對(duì)提高船舶導(dǎo)航系統(tǒng)精度提出了很高的要求2.2.1 聯(lián)邦濾波器結(jié)構(gòu)聯(lián)邦濾波器是一種信息融合技術(shù)，其結(jié)構(gòu)可通過圖1來(lái)說(shuō)明，它由若干個(gè)子濾波器和一個(gè)主濾波器組成，是一種具有兩級(jí)數(shù)據(jù)處理的特殊的分散化濾波方法，其特殊性在于聯(lián)邦濾波器采用的是信息分配原理。圖中公共參考系統(tǒng)的輸出一方面直接給主濾波器，另一方面給各子濾波器作為公共狀態(tài)變量。各子濾波器的局部估計(jì)值（公共狀態(tài)）及其協(xié)方差陣送入主濾波器和主濾波器的估計(jì)值一起進(jìn)行融合以得到全局最優(yōu)估計(jì)和相應(yīng)的協(xié)方差陣。參考系統(tǒng)子系統(tǒng)1子系統(tǒng)2子系統(tǒng)N局部濾波器1局部濾波器2局部濾波器N主濾波器時(shí)間更新最優(yōu)組合圖1 聯(lián)邦濾波器的一般結(jié)2.2.2 聯(lián)邦濾波器算法設(shè)、表示聯(lián)邦濾波器的最優(yōu)估計(jì)值和方差，Xi、Pi表示第i個(gè)子濾波器的估計(jì)值和方差（i=1，2.n），、表示主濾波器的估計(jì)值和方差。全局狀態(tài)方程為： 其中：是系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，是系統(tǒng)的噪聲矩陣。設(shè)是零均值的白噪聲序列，其協(xié)方差陣為。第i個(gè)子系統(tǒng)的量測(cè)方程為： 其中：Zi(k)是第i個(gè)量測(cè)傳感器的量測(cè)值，Hi（k）是第i個(gè)量測(cè)傳感器的量測(cè)陣，Vi（k）是傳感器的觀測(cè)噪聲陣，是獨(dú)立于W(k)的零均值白噪聲序列，其協(xié)方差陣為R（k）。信息分配過程：信息分配就是在各子濾波器和主濾波器之間分配系統(tǒng)的信息。系統(tǒng)的過程信息和按如下的信息分配原則在各子濾波器和主濾波器之間進(jìn)行分配：， 其中是信息分配因子，并滿足信息分配原理： 在系統(tǒng)噪聲分配過程中，采用了方程上界技術(shù)，得到的子濾波器的濾波結(jié)果是次優(yōu)的。物理意義上是由于過程噪聲方差被上界取代，則每個(gè)濾波器更少的依靠時(shí)間更新后的狀態(tài)值，更多的依靠測(cè)量更新。信息的時(shí)間更新：時(shí)間更新過程在各子濾波器和主濾波器之間獨(dú)立進(jìn)行，各子濾波器和主濾波器的濾波算法為： （13）信息的測(cè)量，由于主濾波器沒有測(cè)量值，所以主濾波器沒有測(cè)量更新。測(cè)量更新只在各個(gè)局部子濾波器中進(jìn)行，測(cè)量更新通過下式起作用： 信息融合：聯(lián)邦濾波器的核心算法是將各個(gè)局部濾波器的局部估計(jì)信息按下式進(jìn)行融合，以得到全局的最優(yōu)估計(jì)。 通過以上的信息分配、添加（時(shí)間更新和測(cè)量更新）和信息融合過程，在局部濾波器中由于方差上界技術(shù)引起的信息丟失，在融合過程中這種非最優(yōu)性被重新合成，得到全局最優(yōu)解。3 船舶航行姿態(tài)系統(tǒng)中聯(lián)邦濾波器設(shè)計(jì)3.1 船舶航行系統(tǒng)中聯(lián)邦濾波器結(jié)構(gòu)在多傳感器組合導(dǎo)航系統(tǒng)中，聯(lián)邦濾波器能夠利用信息分配原理實(shí)現(xiàn)多傳感器信息的最優(yōu)估計(jì)，同時(shí)使整個(gè)系統(tǒng)具有一定的容錯(cuò)能力?？紤]到各個(gè)子系統(tǒng)的導(dǎo)航精度都較高，在船舶航行系統(tǒng)中，各子系統(tǒng)容易出現(xiàn)故障，為確保組合系統(tǒng)具有高容錯(cuò)性能，本文采用的是如圖3-1所示的聯(lián)邦濾波器無(wú)復(fù)位結(jié)構(gòu)。子濾波器選擇GPS（全球定位系統(tǒng)）、SINS（慣性導(dǎo)航系統(tǒng)）、TAN（地形輔助性導(dǎo)航系統(tǒng)）和北斗雙星子系統(tǒng)。GPS子系統(tǒng)量測(cè)信息與SINS系統(tǒng)組成子濾波器1；北斗雙星子系統(tǒng)量測(cè)信息與SINS 系統(tǒng)組成子濾波器2 ，TAN子系統(tǒng)量測(cè)信息與SINS 系統(tǒng)組成子濾波器3 ；主濾波器的輸出是所有子系統(tǒng)與SINS 系統(tǒng)的量測(cè)所確定的全局最優(yōu)估計(jì)。GPSBDTAN子濾波器1子濾波器2子濾波器3主濾波器時(shí)間更新信息融合日?qǐng)D2 聯(lián)邦濾波器結(jié)構(gòu)圖中,分別為三個(gè)子濾波器的狀態(tài)變量和狀態(tài)協(xié)方差陣；為主濾波器狀態(tài)變量和主濾波器狀態(tài)協(xié)方差陣。系統(tǒng)選取的全局狀態(tài)變量為 式中：為緯度弧長(zhǎng)；為經(jīng)度弧長(zhǎng)；為海流東向速度分量；為海流北向速度分量；為船舶相對(duì)水的速度；為船舶航向。動(dòng)態(tài)方程為：其中：為獨(dú)立的高斯白噪聲；海流用一階馬爾可夫過程來(lái)表示；為相關(guān)時(shí)間常數(shù)。全局狀態(tài)方程狀態(tài)轉(zhuǎn)移矩陣 圖3 聯(lián)邦濾波器流程圖設(shè)動(dòng)態(tài)系統(tǒng)為: 觀測(cè)方程為：3.2無(wú)重置估計(jì)仿真結(jié)果： 圖4 無(wú)重置仿真狀態(tài)X1仿真曲線 圖5 無(wú)重置仿真狀態(tài)X2仿真曲線圖6 無(wú)重置仿真狀態(tài)X3仿真曲線有重置估計(jì)仿真上圖為400次濾波的狀態(tài)誤差。藍(lán)色為濾波前X曲線，紅色為濾波后X曲線，綠色為主濾波器估計(jì)值曲線。無(wú)重置結(jié)構(gòu)的濾波器受噪聲的影響大，系統(tǒng)容錯(cuò)性能差。3.3有重置估計(jì)仿真仿真的信息分配系數(shù)為 圖7 有重置仿真狀態(tài)X1仿真曲線 圖8 有重置仿真狀態(tài)X1仿真曲線圖9 有重置仿真狀態(tài)X3仿真曲線上圖為400次濾波的狀態(tài)誤差。藍(lán)色為濾波前X曲線，紅色為濾波后X曲線，綠色為主濾波器估計(jì)值曲線。 很明顯看出有重置的聯(lián)邦濾波器能夠更好地濾除噪聲干擾，子濾波器的得到的估計(jì)值相似性非常高，可以更好地降低某一個(gè)子濾波器因?yàn)楣收匣蛘呖垢蓴_能力差對(duì)系統(tǒng)造成的影響，極大提高了系統(tǒng)的容錯(cuò)性，極大地提高了系統(tǒng)的穩(wěn)定性。附件無(wú)重置濾波仿真程序：clearN=400;w(1)=0;w=randn(1,N)x1(1)=0;x2(1)=0;x3(1)=0;a=1;for k=2:N;x1(k)=x1(k-1)+w(k-1);x2(k)=0.4*x2(k-1)+w(k-1);x3(k)=0.9*x3(k-1)+w(k-1);end V=randn(1,N); q1=std(V); Rvv=q1.2; q3=std(w); Rww=q3.2; a=1;b=0.4;c=0.9;d=0.2; Y1=d*x1+V; Y2=d*x2+V;Y3=d*x3+V;a1(1)=0; a2(1)=0;a3(1)=0;s1(1)= 0; s2(1)= 0; s3(1)= 0; e(1)=1f(1)=1 for t=2:N; p1(t)=a.2*a1(t-1)+Rww; b1(t)=d*p1(t)/(d.2*p1(t)+Rvv); s1(t)=a*s1(t-1)+b1 (t)*(Y1(t)-a*d*s1(t-1); a1(t)=p1(t)-d*b1(t)*p1(t); p2(t)=b.2*a2(t-1)+Rww; b2(t)=d*p1(t)/(d.2*p2(t)+Rvv); s2(t)=b*s2(t-1)+b2(t)*(Y2(t)-b*d*s2(t-1); a2(t)=p2(t)-d*b2(t)*p1(t); p3(t)=c.2*a3(t-1)+Rww; b3(t)=d*p3(t)/(d.2*p1(t)+Rvv); s3(t)=c*s3(t-1)+b3(t)*(Y3(t)-c*d*s3(t-1); a3(t)=p3(t)-d*b3(t)*p1(t); e(t)=1/(1/a1(t)+1/a2(t)+1/a3(t);f(t)=e(t)*(s1(t)/a1(t)+s2(t)/a2(t)+s3(t)/a3(t)end t=1:Nfigure(1);plot(t,s1,r,t,x1,b,t,f,g),xlabel(時(shí)間),ylabel(狀態(tài)x1)figure(2);plot(t,s2,r,t,x2,b,t,f,g),xlabel(時(shí)間),ylabel(狀態(tài)x2)figure(3);plot(t,s3,r,t,x3,b,t,f,g),xlabel(時(shí)間),ylabel(狀態(tài)x3)無(wú)重置濾波仿真程序：clearN=400;w(1)=0;w=randn(1,N)x1(1)=0;x2(1)=0;x3(1)=0;a=1;for k=2:N;x1(k)=x1(k-1)+w(k-1);x2(k)=0.4*x2(k-1)+w(k-1);x3(k)=0.9*x3(k-1)+w(k-1);end V=randn(1,N); q1=std(V); Rvv=q1.2; q3=std(w); Rww=q3.2; a=1;b=0.4;c=0.9;d=0.2; Y1=d*x1+V; Y2=d*x2+V;Y3=d*x3+V;a1(1)=0; a2(1)=0;a3(1)=0;s1(1)=0; s2(1)=0; s3(1)=0; e(1)=1f(1)=1 for t=2:N; p1(t)=a.2*3*e(t-1)+3*Rww;a1(t)=1/(1/p1(t)+ d.2/Rvv);s1(t)=a1(t)*(a*f(t-1)/p1(t)+d*Y1(t)/Rvv); p2(t)=b.2*3*e(t-1)+3*Rww;a2(t)=1/(1/p2(t)+ d.2/Rvv);s2(t)=a2(t)*(b*f(t-1)/p2(t)+d*Y2(t)/Rvv); p3(t)=c.2*3*e(t-1)+3*Rww;a3(t)=1/(1/p3(t)+ d.2/Rvv);s3(t)=a3(t)*(c*f(t-1)/p3(t)+d*Y3(t)/Rvv); e(t)=1/(1/a1(t)+1/a2(t)+