基于純方位信息的su-h

基于純方位信息的su-h

傳統(tǒng)的目標跟蹤通常在笛卡爾坐標系進行，而傳感器的觀察則是在極坐標系中進行的。因此，實現(xiàn)跟蹤采用角坐標系、修正極坐標系、對齊極坐標系等方法。修正極坐標系和對數(shù)極坐標系對狀態(tài)變量的可觀測與不可觀測部分進行解耦,使濾波性能得到了很大的改善,近幾年得到了廣泛的應用。由于純方位跟蹤中,直角坐標系的觀測方程和極坐標系的狀態(tài)方程都是非線性隨機方程,由此引入了泰勒級數(shù)展開和UT變換線性化非線性方程的兩種方法。近年來出現(xiàn)的SUT算法是一種基于scaledunscentedtransformation(SUT)的新型算法。該算法摒棄了對非線性函數(shù)進行線性化的傳統(tǒng)做法,轉(zhuǎn)而從統(tǒng)計學的角度尋找解決問題的思路。與利用泰勒級數(shù)展開式相比,SUT算法精度高、適用范圍廣,特別適合于解決高階非線性問題。UKF與EKF濾波算法都是基于卡爾曼濾波框架,以最小均方差為準則的估計方法,其缺點是魯棒性能差、對狀態(tài)噪聲的不確定及觀測噪聲敏感。近年來H∞濾波算法越來越得到關(guān)注,主要是由于其不需要噪聲的先驗統(tǒng)計,以及可以抵抗模型不確定性等魯棒性能。與傳統(tǒng)的卡爾曼濾波框架下的最小均方差估計相比,H∞濾波算法的準則是使由于最大擾動引起的估計誤差最小,因此其魯棒性好。H∞濾波算法實際上是在卡爾曼濾波算法基礎(chǔ)上增加了一個魯棒環(huán)節(jié),該環(huán)節(jié)的作用是通過改變廣義狀態(tài)估計方差來以一定精度為代價換取滿意的魯棒性能。本文采用SUT變換線性化非線性方程,利用離散線性系統(tǒng)的H∞濾波算法,引入了SUT-H∞濾波算法,比較了該種算法在直角坐標系和修正極坐標系的跟蹤性能。1動的觀測方法純方位二維跟蹤場景如圖1所示,觀測載體與目標在x-y平面上運動,并周期地觀測到目標方位角。假設(shè)目標做近似勻速直線運動,觀測載體需要做特定的機動以實現(xiàn)狀態(tài)方程可觀測。下面分別介紹直角坐標系與修正極坐標系的狀態(tài)與觀測方程,以及兩種坐標系之間的轉(zhuǎn)換。1.1[2.2]計算傳統(tǒng)的純方位跟蹤是在直角坐標系下進行的。直角坐標系的離散狀態(tài)和觀測方程定義為{x(k+1)=Ax(k)+U(k+1,k)+Gw(k)z(k)=f(x(k))+v(k)=tan-1(x1(k)x3(k))+v(k)(1)其中:x(k)=[x1(k)x2(k)x3(k)x4(k)]=[xtg(k)-xob(k)˙xtg(k)-˙xob(k)ytg(k)-yob(k)˙ytg(k)-˙yob(k)](2)U(k+1,k)=[xob(k+1)-xob(k)-Τ˙xob(k)˙xob(k+1)-˙xob(k)yob(k+1)-yob(k)-Τ˙yob(k)˙yob(k+1)-˙yob(k)](3)A=[1Τ000100001Τ0001]?G=[Τ220Τ00Τ220Τ]1.2目標載體方位角Aidala和Hammel引入了修正極坐標系,實現(xiàn)了可觀測與不可觀測部分的解耦。修正極坐標離散狀態(tài)和觀測方程定義為{y(k+1)=fmpc(Afcmp(y(k))+U(k+1,k)+Gw(k))z(k)=h(y(k))+v(k)=y(k)+v(k)(4)其中:y(k)=[β(k)˙β(k)r(k)˙r(k)r(k)]Τ=[y1(k)y2(k)y3(k)y4(k)]T(5)fcmp(y(k))=[sin(y1(k))y3(k)y4(k)sin(y1(k))+y2(k)cos(y1(k))y3(k)cos(y1(k))y3(k)y4(k)cos(y1(k))-y2(k)sin(y1(k))y3(k)](6)fmpc(x(k))=[tan-1(x1(k)x3(k))x2(k)x3(k)-x1(k)x4(k)x21(k)+x23(k)1√x21(k)+x23(k)x2(k)x1(k)+x3(k)x4(k)x21(k)+x23(k)](7)v(k)～N(0,s2b)w(k)～N(0,Q)其中:r(k)為目標和觀測載體相對距離,β(k)為目標相對觀測載體的方位角。式(6)為修正極坐標系—直角坐標系轉(zhuǎn)換,式(7)為直角坐標系—修正極坐標系轉(zhuǎn)換。2sut-h濾波算法用于修正極坐標系和直角坐標系的算法根據(jù)參考文獻中的算法,結(jié)合直角坐標系與修正極坐標系的狀態(tài)與觀測方程,推導兩種坐標系的濾波算法。2.1zzk—直角坐標系中的SUT-H∞濾波算法預測:?xk|k-1=A?xk-1|k-1+U(k+1,k)Pk|k-1=APk-1|k-1AT+GQGTSigma點:χik|k-1=([?xk|k-1?xk|k-1±√(n+κ)Ρk|k-1])izik|k-1=f(χik|k-1),?zk|k-1=2n+1∑i=1ωmizik|k-1Ρzzk|k-1=2n+1∑i=1ωci(?zk|k-1-zik|k-1)(?zk|k-1-zik|k-1)Τ+RΡxzk|k-1=2n+1∑i=1ωci(?xk|k-1-xik|k-1)(?zk|k-1-zik|k-1)Τ其中:ωmi={λ/(n+λ)i=01/[2(n+λ)]i=1,2,?,2nωci={λn+λ+(1-α2+β2)i=01/[2(n+λ)]i=1,2,3,?,2nλ=α2(n+k)-n更新:?xk|k=?xk|k-1+Κk(zk-?zk|k-1),Κk=Ρxzk|k-1(Ρzzk|k-1)-1SUΤ-Η∞方差更新∶Ρk|k=Ρk|k-1-[Ρxzk|k-1Ρk|k-1]R-1e,k[[Ρxzk|k-1]ΤΡΤk|k-1]Re,k=[Ρzzk|k-1[Ρxzk|k-1]ΤΡxzk|k-1γ2Ι+Ρk|k-1]γ2k=ρmax{eig(P-1k|k-1+P-1k|k-1Pxzk|k-1R-1[P-1k|k-1Pxzk|k-1]T)-1}(8)2.2廣義濾波增益Sigma點:ywk-1|k-1=[yΤk-1|k-1wΤk-1]Τ?ywk-1|k-1=E(ywk-1|k-1)∈Rn+qΡwk-1|k-1=E(ywk-1|k-1-?ywk-1|k-1)(ywk-1|k-1-?ywk-1|k-1)Τ=[Ρk-1|k-1Ρywk-1Ρywk-1Qk-1]χik-1|k-1=([?ywk-1|k-1?ywk-1|k-1±√(n+q+κ)Ρwk-1|k-1])i預測:yik|k-1=fmpc(χik-1|k-1)?yk|k-1=2(n+q)+1∑i=1ωmiyik|k-1Ρk|k-1=2(n+q)+1∑i=1ωci(?yk|k-1-yik|k-1)(?yk|k-1-yik|k-1)Τzik|k-1=h(yik|k-1),?zk|k-1=2(n+q)+1∑i=1ωmizik|k-1Ρzzk|k-1=2(n+q)+1∑i=1ωci(?zk|k-1-zik|k-1)(?zk|k-1-zik|k-1)Τ+RΡyzk|k-1=2(n+q)+1∑i=1ωci(?yk|k-1-yik|k-1)(?zk|k-1-zik|k-1)Τ其中:ωmi={λ(n+q+λ)i=01/[2(n+q+λ)]i=1,2??,2(n+q)ωci={λn+q+λ+(1-α2+β2)i=01/[2(n+q+λ)]i=1,2,3,?,2(n+q)λ=α2(n+q+k)-(n+q)更新:?yk|k=?yk|k-1+Κk(zk-?zk|k-1),Κk=Ρyzk|k-1(Ρzzk|k-1)-1SUΤ-Η∞方差更新∶Ρk|k=Ρk|k-1-[Ρyzk|k-1Ρk|k-1]R-1e,k[[Ρyzk|k-1]ΤΡΤk|k-1]Re,k=[Ρzzk|k-1[Ρyzk|k-1]ΤΡyzk|k-1γ2Ι+Ρk|k-1]γ2k=ρmax{eig(P-1k|k-1+P-1k|k-1Pyzk|k-1R-1[P-1k|k-1Pyzk|k-1]T)-1}(9)從上面的算法可以看出,魯棒H∞濾波和標準卡爾曼濾波,其本質(zhì)區(qū)別在于濾波增益的算法是不同的。當γk～∞時,H∞魯棒濾波便退化為卡爾曼濾波。因此,可以推出,廣義濾波增益在γk→∞的過程中是不斷變化的,并且逐漸逼近卡爾曼濾波增益。式(8)和(9)的ρ取大于1的數(shù),隨觀測噪聲和狀態(tài)噪聲以及仿真環(huán)境進行調(diào)整。3模擬結(jié)果與分析3.1傳感器的初始距離二維仿真跟蹤場景如圖2所示。目標和觀測載體都做勻速運動,為了實現(xiàn)目標的可觀測,觀測載體需要做相應的機動,在本文中觀測載體速度3m/s,觀測載體分別在100s,400s做了兩次機動,航向角分別為90°、0°、90°。目標速度15m/s,航向角為180°。觀測傳感器的標準差選為σR=3°。本文中的初始化方法利用了傳感器的最大觀測距離,由于仿真中目標與感測載體的距離為10km,即假定傳感器最大觀測為10km,所以在每次MonteCarlo仿真,r取自于均勻分布的隨機數(shù),標準差為σr=5000。由于速度事先無先驗,所以速度初始化為0或取自于[030]的一個隨機數(shù),σ=30,σ=30。利用第一個觀測方位角,直角坐標系的初始化方法為x(0)=[rsinθ10rcosθ10]然后利用SUT變換及直角坐標與修正極坐標的轉(zhuǎn)換來初始化直角坐標系與修正極坐標系下狀態(tài)變量的初值與方差。3.2修正坐標系的純方位跟蹤本文進行了500次MonteCarlo仿真,圖3和4根據(jù)最有理論下界(PCRLB)利用0.95置信區(qū)間來表示位置估計,其中圖3是修正坐標系的位置跟蹤,圖4是直角坐標系的位置跟蹤。圖5～8是兩種坐標系四個狀態(tài)變量的RMS誤差曲線比較。從總體來看,在第一次機動后,兩種坐標系純方位跟蹤的四個狀態(tài)變量都開始收斂,但修正坐標系的純方位跟蹤明顯優(yōu)于直角坐標系的純方位跟蹤。特別是對于位置的跟蹤,修正坐標系的純方位跟蹤能很穩(wěn)定地收斂于穩(wěn)定的結(jié)果。從圖3和4來看,直角坐標系的跟蹤在收斂到一定的精度后,仍然會有一些點出現(xiàn)在0.95的置信區(qū)間之外,而修正坐標系中卻幾乎沒有。其原因是由于過大的傳感器噪聲使得模型出現(xiàn)了不可觀測,而修正坐標系實現(xiàn)了狀態(tài)可觀測與不可觀測的解耦,使得濾波器更加穩(wěn)定。從圖5和7看,修正坐標系的純方位跟蹤的位置跟蹤精度要好于直角坐標系的純方位跟蹤。從圖6和8看,修正坐標系的純方位跟蹤的速度和跟蹤精度要稍好于直角坐標系的純方位跟蹤。直角坐標系的速度的收斂時間要稍好于修正坐標系。與位置跟蹤相比,速度跟蹤的優(yōu)勢不明顯。4可觀測誤差從理論分析和仿真結(jié)果來看,SUT-H∞濾波在修正坐標系下的純方位跟蹤中的跟蹤性能要優(yōu)于在直角坐標系