用于實時彈道濾波的Sage-Husa改進算法

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

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

隨著測控技術的飛速發展,靶場測控裝備的種類和數量迅速增加,獲得的實時彈道測量數據也日益豐富。面對著種類和數量繁多的實時彈道數據,實時彈道測量數據融合為測控系統準確地獲取彈丸的實時飛行狀態和控制狀態提供了有效的手段,提升了試驗指揮決策和安控判決的實時性和準確性。對于實時數據融合,實時數據濾波是一個不可或缺的環節。

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

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

上述改進算法大都受應用領域限制,通常假定量測噪聲統計特性未知但相對穩定,忽略量測噪聲統計特性的時變性,采用較大的遺忘因子進行遞推計算,估計的量測噪聲協方差矩陣隨著時間的推移趨于穩定,因此無法對時變統計特性量測噪聲的進行準確估計。而且這些改進沒有對濾波算法的抗差性以及如何保證噪聲協方差矩陣無偏估計的正定性進行深入分析。

因此,本文提出了改進的Sage-Husa自適應卡爾曼濾波算法(ISHAKF算法),對Sage-Husa算法的實時自適應性、抗差自適應性和濾波穩定性等進行改進,實現實時自適應濾波,以滿足靶場測控系統對實時彈道測量數據的實時濾波需求。

1 算法改進

1.1 Sage-Husa卡爾曼濾波算法

對于線性離散模型,系統的狀態方程和量測方程為

=-1-1+-1
=+

(1)

式中:為時刻的狀態估計向量,-1為-1時刻到時刻的狀態矩陣,-1為系統噪聲向量,為時刻對應的測量向量,為量測矩陣,為量測噪聲向量。且E(-1)=0,E(-1)=0,cov(,)=,,cov(,)=,,和分別為系統噪聲協方差矩陣和量測噪聲協方差矩陣,,為狄拉克函數。

引進遺忘因子,僅對量測噪聲協方差矩陣進行估計,則算法可簡化為

(2)

(3)

(4)

(5)

(6)

(7)

(8)

=(1-)(1-+1)

(9)

(10)

1.2 量測噪聲協方差矩陣正定改進

(11)

同時,可以計算得出:

(12)

(13)

1.3 遺忘因子實時自適應改進

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

(14)

則遺忘因子可由來確定:

(15)

式中:,分別為遺忘因子的下限和自適應動態范圍;,,為控制常量。為了快速適應量測噪聲統計特性的突變,遺忘因子下限可取065～08;由遺忘因子的一般取值,+取值在095～099;基于正態分布概率,為了保證系統穩定判定準確率大于99,可取17～19,可取7～9,大于9。用于控制量測噪聲協方差變小時,量測噪聲協方差估計的快速收斂;用于保證量測噪聲協方差較為平穩時的穩定估計;用于控制量測噪聲估計更新的拒絕域,>時,量測噪聲估計不更新,令=0。

(16)

在新息方差較為穩定時,這種可變的自適應遺忘因子保持了穩定性;同時在新息方差出現較大變化時,能夠迅速響應變小。

1.4 卡爾曼增益矩陣抗差自適應改進

對遺忘因子進行自適應改進后,當判定系統統計特性為非穩定狀態時,遺忘因子減小,中當前估計值權重增大,這樣可以保證算法對量測噪聲統計特性估計的準確性和實時性。另一方面,在遺忘因子為1時,雖然判定量測信息異常,量測噪聲協方差矩陣沒有更新,但是新息向量偏差較大,而且卡爾曼增益矩陣沒有改變,這時若仍然對預測值進行修正更新,必將導致濾波結果誤差偏大。濾波算法的抗差性能對于實時數據濾波的工程應用具有重要實際意義。因此,為了在一定程度上兼顧濾波算法的抗差自適應性,參考遺忘因子的確定方法,仍以中間變量對增益矩陣進行改進。

(17)

(18)

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

算法的抗差自適應性與算法的實時自適應性是一對無法調和的矛盾,因此,可以根據需求側重,適當調整控制常量。越大,算法的實時自適應性越強,反之,算法的抗差自適應性越強。

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

2 仿真驗證

對上述改進分別進行仿真對比,以驗證改進的正確和與實用性。

仿真條件設定:在量測噪聲統計特性未知的條件下,彈丸飛行的狀態模型為未知的二維變加速模型,方向和方向的加速度都設定為

仿真時長=100 s,采樣間隔為Δ=0.05 s,彈道狀態初值為=0 m,=0 m,=1 000 m/s,=0 m/s,仿真彈道數據真值如圖1所示。

圖1 仿真彈道數據曲線圖

2.1 正定改進分析

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

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

圖3 正定改進的Sage-Husa算法濾波殘差曲線

2.2 遺忘因子改進分析

所有仿真條件與正定性改進比對仿真相同,分別采用經過正定改進的Sage-Husa算法和經過正定及遺忘因子改進的Sage-Husa算法對仿真的彈道測量數據進行濾波,以檢驗遺忘因子的改進效果,Sage-Husa算法取遺忘因子=0.97。

遺忘因子改進主要是為了準確估計量測噪聲,以保證在量測噪聲突變時,遞推濾波能夠快速收斂,兩種算法對量測噪聲均方差的實時估計結果如圖4所示。

圖4 遺忘因子改進對量測噪聲均方差實時估計的影響

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

圖5 遺忘因子改進對濾波殘差均方差的影響

在Sage-Husa算法中,遺忘因子不僅反映歷史和當前量測信息對量測噪聲估值的貢獻大小,還直接影響量測信息和模型信息對濾波結果權重,從而影響量測噪聲統計特性突變時的彈道濾波效果。因此,經過遺忘因子改進之后,可進一步提高算法對量測噪聲統計特性的準確估計能力和對其變化的實時自適應能力。

2.3 抗差性改進分析

在上一仿真條件的基礎上,每10 s對彈道測量數據的5個連續采樣點疊加-200 m的誤差,采用經過遺忘因子及正定改進但是未經過抗差性能改進的Sage-Husa算法與ISHAKF算法對仿真的測量數據進行濾波,兩種算法的濾波殘差結果如圖6和圖7所示。

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

圖7 ISHAKF算法濾波殘差曲線

從圖6中可以看到,對于未經過抗差性能改進的Sage-Husa算法,無論量測噪聲方差大小,測量數據只要存在野值,都會對濾波效果產生影響,該算法基本上不具備抗差性能。從圖7中可以看到,當彈道測量數據中的野值點相對量測噪聲較大時,ISHAKF能夠有效判定野值并消除野值影響,能夠保持濾波的穩定性。但是當野值相對量測噪聲相差不大時,由于算法對時變量測噪聲具有自適應能力,導致算法無法判定野值。同時,當連續野值采樣點超過一定數量時,即使野值相對量測噪聲較大,算法會逐漸向含有野值的彈道數據收斂,這與理論分析結果一致。因此,算法僅能夠對相對較大的野值具有一定的抗差能力,而且當連續野值點數量過多,濾波結果仍會出現偏移。

3 實例驗證

對某型導彈彈道數據進行實時濾波處理,其中方向數據如圖8所示。對彈道方向測量數據進行ISHAKF實時濾波處理,以高精度彈道測量數據為真值,可以得到測量數據的殘差、濾波后殘差和量測噪聲的實時均方差估計,結果如圖9所示。

圖8 某型導彈彈道x方向測量數據

圖9 IHAKF濾波殘差和量測噪聲均方差估計

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

4 結束語

為解決靶場測控系統實時數據濾波處理過程中量測噪聲統計特性自適應的實際問題,對Sage-Husa卡爾曼濾波算法進行了多種有效改進。

針對量測噪聲方差較小時,Sage-Husa算法會出現量測噪聲估計矩陣非正定的問題,以量測噪聲協方差矩陣的殘差估計替換新息估計,將量測噪聲協方差估計矩陣變換為半正定矩陣和正定矩陣之和的形式,彌補了量測噪聲協方差估計矩陣非正定導致濾波異常的缺陷。針對測噪聲統計特性的自適應問題,通過設計新的自適應遺忘因子,解決了量測噪聲統計特性突變時,Sage-Husa算法收斂較慢的問題,兼顧了量測噪聲估計的實時性和穩定性。針對抗差性能,通過對卡爾曼增益矩陣的改進,增強了算法的魯棒性;綜合上述算法改進,構建了ISHAKF算法。

通過仿真比對驗證和彈道數據濾波實例驗證,證明了在實時彈道測量數據濾波方面,ISHAKF算法克服了Sage-Husa算法的欠缺,具有更高的實時性、自適應性和魯棒性,濾波效果提升明顯。