用于實(shí)時(shí)彈道濾波的Sage-Husa改進(jìn)算法

段鵬偉,宮志華,徐 旭,趙春霞

(中國(guó)人民解放軍63861部隊(duì),吉林 白城 137001)

隨著測(cè)控技術(shù)的飛速發(fā)展,靶場(chǎng)測(cè)控裝備的種類和數(shù)量迅速增加,獲得的實(shí)時(shí)彈道測(cè)量數(shù)據(jù)也日益豐富。面對(duì)著種類和數(shù)量繁多的實(shí)時(shí)彈道數(shù)據(jù),實(shí)時(shí)彈道測(cè)量數(shù)據(jù)融合為測(cè)控系統(tǒng)準(zhǔn)確地獲取彈丸的實(shí)時(shí)飛行狀態(tài)和控制狀態(tài)提供了有效的手段,提升了試驗(yàn)指揮決策和安控判決的實(shí)時(shí)性和準(zhǔn)確性。對(duì)于實(shí)時(shí)數(shù)據(jù)融合,實(shí)時(shí)數(shù)據(jù)濾波是一個(gè)不可或缺的環(huán)節(jié)。

常規(guī)的卡爾曼濾波通常假定系統(tǒng)噪聲和量測(cè)噪聲滿足高斯-馬爾可夫假設(shè)條件,以各種先驗(yàn)信息來確定它們的協(xié)方差矩陣,再以恒定的協(xié)方差矩陣進(jìn)行遞推濾波。但是在實(shí)際的實(shí)時(shí)彈道濾波處理過程中,由于受彈丸特性、彈丸距離、彈丸姿態(tài)和試驗(yàn)環(huán)境因素等影響,量測(cè)噪聲往往具有時(shí)變的統(tǒng)計(jì)特性,屬于非平穩(wěn)時(shí)間序列,這時(shí)噪聲的統(tǒng)計(jì)特性將難以確定。若仍然采用常規(guī)卡爾曼濾波,勢(shì)必造成濾波精度的下降,嚴(yán)重時(shí)將導(dǎo)致濾波發(fā)散。

對(duì)于未知統(tǒng)計(jì)特性噪聲的自適應(yīng)濾波問題,1969年SAGE和HUSA對(duì)當(dāng)時(shí)的序貫自適應(yīng)估計(jì)算法進(jìn)行了總結(jié),提出了具有代表性的基于新息的自適應(yīng)濾波算法,即Sage-Husa卡爾曼濾波算法,簡(jiǎn)稱為Sage-Husa算法,可以在線估計(jì)線性離散系統(tǒng)的系統(tǒng)噪聲和量測(cè)噪聲。NARASIMHAPPA等將改進(jìn)的Sage-Husa算法與自回歸模型結(jié)合,應(yīng)用于光陀螺儀去噪,取得了良好的濾波效果。程建華等去除了Sage-Husa算法中系統(tǒng)噪聲和量測(cè)噪聲估計(jì)中的部分減號(hào)項(xiàng),保證噪聲協(xié)方差矩陣的半正定性,并將工程簡(jiǎn)化算法用于艦船捷聯(lián)慣導(dǎo)傳遞對(duì)準(zhǔn)。王佳偉等對(duì)算法進(jìn)行了簡(jiǎn)化改進(jìn),給出了濾波異常判斷準(zhǔn)則,并成功應(yīng)用于修正引信滾轉(zhuǎn)角測(cè)量中。李靜等和郝亮等分別將CKF算法和UKF算法與Sage-Husa算法結(jié)合起來,對(duì)車輛狀態(tài)參數(shù)進(jìn)行實(shí)時(shí)在線估計(jì)。還有一些學(xué)者將Sage-Husa算法的噪聲估計(jì)方法應(yīng)用于其他濾波算法,都得到了較為理想的濾波結(jié)果。

上述改進(jìn)算法大都受應(yīng)用領(lǐng)域限制,通常假定量測(cè)噪聲統(tǒng)計(jì)特性未知但相對(duì)穩(wěn)定,忽略量測(cè)噪聲統(tǒng)計(jì)特性的時(shí)變性,采用較大的遺忘因子進(jìn)行遞推計(jì)算,估計(jì)的量測(cè)噪聲協(xié)方差矩陣隨著時(shí)間的推移趨于穩(wěn)定,因此無法對(duì)時(shí)變統(tǒng)計(jì)特性量測(cè)噪聲的進(jìn)行準(zhǔn)確估計(jì)。而且這些改進(jìn)沒有對(duì)濾波算法的抗差性以及如何保證噪聲協(xié)方差矩陣無偏估計(jì)的正定性進(jìn)行深入分析。

因此,本文提出了改進(jìn)的Sage-Husa自適應(yīng)卡爾曼濾波算法(ISHAKF算法),對(duì)Sage-Husa算法的實(shí)時(shí)自適應(yīng)性、抗差自適應(yīng)性和濾波穩(wěn)定性等進(jìn)行改進(jìn),實(shí)現(xiàn)實(shí)時(shí)自適應(yīng)濾波,以滿足靶場(chǎng)測(cè)控系統(tǒng)對(duì)實(shí)時(shí)彈道測(cè)量數(shù)據(jù)的實(shí)時(shí)濾波需求。

1 算法改進(jìn)

1.1 Sage-Husa卡爾曼濾波算法

對(duì)于線性離散模型,系統(tǒng)的狀態(tài)方程和量測(cè)方程為

=-1-1+-1
=+

(1)

式中:為時(shí)刻的狀態(tài)估計(jì)向量,-1為-1時(shí)刻到時(shí)刻的狀態(tài)矩陣,-1為系統(tǒng)噪聲向量,為時(shí)刻對(duì)應(yīng)的測(cè)量向量,為量測(cè)矩陣,為量測(cè)噪聲向量。且E(-1)=0,E(-1)=0,cov(,)=,,cov(,)=,,和分別為系統(tǒng)噪聲協(xié)方差矩陣和量測(cè)噪聲協(xié)方差矩陣,,為狄拉克函數(shù)。

引進(jìn)遺忘因子,僅對(duì)量測(cè)噪聲協(xié)方差矩陣進(jìn)行估計(jì),則算法可簡(jiǎn)化為

(2)

(3)

(4)

(5)

(6)

(7)

(8)

=(1-)(1-+1)

(9)

(10)

1.2 量測(cè)噪聲協(xié)方差矩陣正定改進(jìn)

(11)

同時(shí),可以計(jì)算得出:

(12)

(13)

1.3 遺忘因子實(shí)時(shí)自適應(yīng)改進(jìn)

Sage-Husa算法中,遺忘因子一般取095～099,在逐漸增大后,趨于(1-)。這種算法對(duì)量測(cè)噪聲協(xié)方差恒定但未知的系統(tǒng)進(jìn)行濾波,能夠兼顧估計(jì)的準(zhǔn)確性和穩(wěn)健性。但是對(duì)于量測(cè)噪聲統(tǒng)計(jì)特性時(shí)變甚至突變的系統(tǒng),這種算法將降低量測(cè)噪聲估計(jì)的準(zhǔn)確性和實(shí)時(shí)性,從而降低實(shí)時(shí)濾波的精度。

(14)

則遺忘因子可由來確定:

(15)

式中:,分別為遺忘因子的下限和自適應(yīng)動(dòng)態(tài)范圍;,,為控制常量。為了快速適應(yīng)量測(cè)噪聲統(tǒng)計(jì)特性的突變,遺忘因子下限可取065～08;由遺忘因子的一般取值,+取值在095～099;基于正態(tài)分布概率,為了保證系統(tǒng)穩(wěn)定判定準(zhǔn)確率大于99,可取17～19,可取7～9,大于9。用于控制量測(cè)噪聲協(xié)方差變小時(shí),量測(cè)噪聲協(xié)方差估計(jì)的快速收斂;用于保證量測(cè)噪聲協(xié)方差較為平穩(wěn)時(shí)的穩(wěn)定估計(jì);用于控制量測(cè)噪聲估計(jì)更新的拒絕域,>時(shí),量測(cè)噪聲估計(jì)不更新,令=0。

(16)

在新息方差較為穩(wěn)定時(shí),這種可變的自適應(yīng)遺忘因子保持了穩(wěn)定性;同時(shí)在新息方差出現(xiàn)較大變化時(shí),能夠迅速響應(yīng)變小。

1.4 卡爾曼增益矩陣抗差自適應(yīng)改進(jìn)

對(duì)遺忘因子進(jìn)行自適應(yīng)改進(jìn)后,當(dāng)判定系統(tǒng)統(tǒng)計(jì)特性為非穩(wěn)定狀態(tài)時(shí),遺忘因子減小,中當(dāng)前估計(jì)值權(quán)重增大,這樣可以保證算法對(duì)量測(cè)噪聲統(tǒng)計(jì)特性估計(jì)的準(zhǔn)確性和實(shí)時(shí)性。另一方面,在遺忘因子為1時(shí),雖然判定量測(cè)信息異常,量測(cè)噪聲協(xié)方差矩陣沒有更新,但是新息向量偏差較大,而且卡爾曼增益矩陣沒有改變,這時(shí)若仍然對(duì)預(yù)測(cè)值進(jìn)行修正更新,必將導(dǎo)致濾波結(jié)果誤差偏大。濾波算法的抗差性能對(duì)于實(shí)時(shí)數(shù)據(jù)濾波的工程應(yīng)用具有重要實(shí)際意義。因此,為了在一定程度上兼顧濾波算法的抗差自適應(yīng)性,參考遺忘因子的確定方法,仍以中間變量對(duì)增益矩陣進(jìn)行改進(jìn)。

(17)

(18)

式中:,為控制常量,與式(15)中的值相同,一般大于15,可根據(jù)先驗(yàn)誤差信息和預(yù)估誤差信息中極大值來確定,用于控制對(duì)預(yù)測(cè)值進(jìn)行修正更新的拒絕域。

算法的抗差自適應(yīng)性與算法的實(shí)時(shí)自適應(yīng)性是一對(duì)無法調(diào)和的矛盾,因此,可以根據(jù)需求側(cè)重,適當(dāng)調(diào)整控制常量。越大,算法的實(shí)時(shí)自適應(yīng)性越強(qiáng),反之,算法的抗差自適應(yīng)性越強(qiáng)。

由式(17)和式(18)替換Sage-Husa算法中的式(5),由式(13)～式(16)替換Sage-Husa算法中的式(8)和式(9),即形成了ISHAKF算法。

2 仿真驗(yàn)證

對(duì)上述改進(jìn)分別進(jìn)行仿真對(duì)比,以驗(yàn)證改進(jìn)的正確和與實(shí)用性。

仿真條件設(shè)定:在量測(cè)噪聲統(tǒng)計(jì)特性未知的條件下,彈丸飛行的狀態(tài)模型為未知的二維變加速模型,方向和方向的加速度都設(shè)定為

仿真時(shí)長(zhǎng)=100 s,采樣間隔為Δ=0.05 s,彈道狀態(tài)初值為=0 m,=0 m,=1 000 m/s,=0 m/s,仿真彈道數(shù)據(jù)真值如圖1所示。

圖1 仿真彈道數(shù)據(jù)曲線圖

2.1 正定改進(jìn)分析

量測(cè)噪聲均方差設(shè)定為(),分別采用Sage-Husa算法和經(jīng)過正定改進(jìn)的Sage-Husa算法對(duì)仿真的彈道測(cè)量數(shù)據(jù)進(jìn)行濾波,以驗(yàn)證正定性改進(jìn)的有效性。由于狀態(tài)模型及其統(tǒng)計(jì)特性都未知,為了保證算法的兼容性,采用勻加速狀態(tài)模型,優(yōu)選系統(tǒng)噪聲協(xié)方差矩陣=10·,以保證濾波效果和穩(wěn)定性,為6×6單位對(duì)角陣。兩種算法的濾波殘差結(jié)果如圖2和圖3所示。

圖2 Sage-Husa算法濾波殘差曲線

圖3 正定改進(jìn)的Sage-Husa算法濾波殘差曲線

2.2 遺忘因子改進(jìn)分析

所有仿真條件與正定性改進(jìn)比對(duì)仿真相同,分別采用經(jīng)過正定改進(jìn)的Sage-Husa算法和經(jīng)過正定及遺忘因子改進(jìn)的Sage-Husa算法對(duì)仿真的彈道測(cè)量數(shù)據(jù)進(jìn)行濾波,以檢驗(yàn)遺忘因子的改進(jìn)效果,Sage-Husa算法取遺忘因子=0.97。

遺忘因子改進(jìn)主要是為了準(zhǔn)確估計(jì)量測(cè)噪聲,以保證在量測(cè)噪聲突變時(shí),遞推濾波能夠快速收斂,兩種算法對(duì)量測(cè)噪聲均方差的實(shí)時(shí)估計(jì)結(jié)果如圖4所示。

圖4 遺忘因子改進(jìn)對(duì)量測(cè)噪聲均方差實(shí)時(shí)估計(jì)的影響

從圖中對(duì)比可以看到,一方面,為了保證算法的量測(cè)噪聲方差突變的自適應(yīng)性,經(jīng)過正定及遺忘因子改進(jìn)的Sage-Husa算法估計(jì)量測(cè)噪聲的結(jié)果抖動(dòng)劇烈一些。另一方面,在收斂速度上,當(dāng)量測(cè)噪聲方差突然變大時(shí),兩種算法都能夠快速響應(yīng),但是正定及遺忘因子改進(jìn)的Sage-Husa算法收斂速度較快,這也使得該算法在此刻的濾波效果略優(yōu)于正定改進(jìn)的Sage-Husa算法。在量測(cè)噪聲方差突然變小時(shí),對(duì)于量測(cè)噪聲方差的估計(jì),正定及遺忘因子改進(jìn)的Sage-Husa算法能夠迅速收斂,而正定改進(jìn)的Sage-Husa算法收斂稍慢,這使得它在此刻的濾波收斂速度同樣較慢。通過50次蒙特卡羅實(shí)驗(yàn),兩種算法在量測(cè)噪聲方差突然變小時(shí)的濾波殘差均方差結(jié)果如圖5所示。

圖5 遺忘因子改進(jìn)對(duì)濾波殘差均方差的影響

在Sage-Husa算法中,遺忘因子不僅反映歷史和當(dāng)前量測(cè)信息對(duì)量測(cè)噪聲估值的貢獻(xiàn)大小,還直接影響量測(cè)信息和模型信息對(duì)濾波結(jié)果權(quán)重,從而影響量測(cè)噪聲統(tǒng)計(jì)特性突變時(shí)的彈道濾波效果。因此,經(jīng)過遺忘因子改進(jìn)之后,可進(jìn)一步提高算法對(duì)量測(cè)噪聲統(tǒng)計(jì)特性的準(zhǔn)確估計(jì)能力和對(duì)其變化的實(shí)時(shí)自適應(yīng)能力。

2.3 抗差性改進(jìn)分析

在上一仿真條件的基礎(chǔ)上,每10 s對(duì)彈道測(cè)量數(shù)據(jù)的5個(gè)連續(xù)采樣點(diǎn)疊加-200 m的誤差,采用經(jīng)過遺忘因子及正定改進(jìn)但是未經(jīng)過抗差性能改進(jìn)的Sage-Husa算法與ISHAKF算法對(duì)仿真的測(cè)量數(shù)據(jù)進(jìn)行濾波,兩種算法的濾波殘差結(jié)果如圖6和圖7所示。

圖6 遺忘因子及正定改進(jìn)的Sage-Husa算法濾波殘差曲線

圖7 ISHAKF算法濾波殘差曲線

從圖6中可以看到,對(duì)于未經(jīng)過抗差性能改進(jìn)的Sage-Husa算法,無論量測(cè)噪聲方差大小,測(cè)量數(shù)據(jù)只要存在野值,都會(huì)對(duì)濾波效果產(chǎn)生影響,該算法基本上不具備抗差性能。從圖7中可以看到,當(dāng)彈道測(cè)量數(shù)據(jù)中的野值點(diǎn)相對(duì)量測(cè)噪聲較大時(shí),ISHAKF能夠有效判定野值并消除野值影響,能夠保持濾波的穩(wěn)定性。但是當(dāng)野值相對(duì)量測(cè)噪聲相差不大時(shí),由于算法對(duì)時(shí)變量測(cè)噪聲具有自適應(yīng)能力,導(dǎo)致算法無法判定野值。同時(shí),當(dāng)連續(xù)野值采樣點(diǎn)超過一定數(shù)量時(shí),即使野值相對(duì)量測(cè)噪聲較大,算法會(huì)逐漸向含有野值的彈道數(shù)據(jù)收斂,這與理論分析結(jié)果一致。因此,算法僅能夠?qū)ο鄬?duì)較大的野值具有一定的抗差能力,而且當(dāng)連續(xù)野值點(diǎn)數(shù)量過多,濾波結(jié)果仍會(huì)出現(xiàn)偏移。

3 實(shí)例驗(yàn)證

對(duì)某型導(dǎo)彈彈道數(shù)據(jù)進(jìn)行實(shí)時(shí)濾波處理,其中方向數(shù)據(jù)如圖8所示。對(duì)彈道方向測(cè)量數(shù)據(jù)進(jìn)行ISHAKF實(shí)時(shí)濾波處理,以高精度彈道測(cè)量數(shù)據(jù)為真值,可以得到測(cè)量數(shù)據(jù)的殘差、濾波后殘差和量測(cè)噪聲的實(shí)時(shí)均方差估計(jì),結(jié)果如圖9所示。

圖8 某型導(dǎo)彈彈道x方向測(cè)量數(shù)據(jù)

圖9 IHAKF濾波殘差和量測(cè)噪聲均方差估計(jì)

從圖9中可以看到,在線估計(jì)的量測(cè)噪聲均方差與測(cè)量數(shù)據(jù)殘差的均方差極為吻合。在濾波效果上,該測(cè)量數(shù)據(jù)殘差均值為0.4 m,均方差為122.1 m,采用ISHAKF濾波后,殘差均值0.2 m,均方差為42.5 m,消除了超過65%的隨機(jī)誤差,可以有效消除常規(guī)卡爾曼濾波算法由量測(cè)噪聲變化導(dǎo)致的濾波發(fā)散現(xiàn)象。

4 結(jié)束語(yǔ)

為解決靶場(chǎng)測(cè)控系統(tǒng)實(shí)時(shí)數(shù)據(jù)濾波處理過程中量測(cè)噪聲統(tǒng)計(jì)特性自適應(yīng)的實(shí)際問題,對(duì)Sage-Husa卡爾曼濾波算法進(jìn)行了多種有效改進(jìn)。

針對(duì)量測(cè)噪聲方差較小時(shí),Sage-Husa算法會(huì)出現(xiàn)量測(cè)噪聲估計(jì)矩陣非正定的問題,以量測(cè)噪聲協(xié)方差矩陣的殘差估計(jì)替換新息估計(jì),將量測(cè)噪聲協(xié)方差估計(jì)矩陣變換為半正定矩陣和正定矩陣之和的形式,彌補(bǔ)了量測(cè)噪聲協(xié)方差估計(jì)矩陣非正定導(dǎo)致濾波異常的缺陷。針對(duì)測(cè)噪聲統(tǒng)計(jì)特性的自適應(yīng)問題,通過設(shè)計(jì)新的自適應(yīng)遺忘因子,解決了量測(cè)噪聲統(tǒng)計(jì)特性突變時(shí),Sage-Husa算法收斂較慢的問題,兼顧了量測(cè)噪聲估計(jì)的實(shí)時(shí)性和穩(wěn)定性。針對(duì)抗差性能,通過對(duì)卡爾曼增益矩陣的改進(jìn),增強(qiáng)了算法的魯棒性;綜合上述算法改進(jìn),構(gòu)建了ISHAKF算法。

通過仿真比對(duì)驗(yàn)證和彈道數(shù)據(jù)濾波實(shí)例驗(yàn)證,證明了在實(shí)時(shí)彈道測(cè)量數(shù)據(jù)濾波方面,ISHAKF算法克服了Sage-Husa算法的欠缺,具有更高的實(shí)時(shí)性、自適應(yīng)性和魯棒性,濾波效果提升明顯。