基于多項式擬合的擴展卡爾曼濾波算法

吳漢洲 宋衛東 徐敬青

摘要：彈道修正彈內的彈載計算機必須實時對衛星定位接收機獲取的彈丸狀態數據進行濾波降噪，用于預測彈丸落點，傳統濾波方法濾波時間長，濾波實時性差，提出一種基于多項式擬合的方法。通過適當降低衛星定位接收機數據更新頻率，并用多項式擬合插值出的數據代替數據更新時間間隔內的彈丸狀態數據。仿真實驗表明，該算法在不降低濾波效果的前提下，較普通擴展卡爾曼濾波時間降低7/8，提高了濾波實時性，對于彈道修正彈關鍵技術的研究提供了重要參考。同時該方法可推廣應用到其他濾波算法當中，具有很強的可移植性。

關鍵詞：濾波算法；多項式擬合；彈道修正彈；衛星定位數據；濾波誤差

中圖分類號：TP301 文獻標志碼：A

Abstract： The data acquired by the satellite positioning receiver in the trajectory correction projectile must be filtered in realtime to predict the point. The calculation of traditional filtering method is timeconsuming， and is difficult to meet the requirements of realtime filtering. A kind of extended Kalman filtering algorithm based on polynomial fitting was proposed. The data of projectile flight in the time interval was replaced by the fitting interpolation data. In this way the filter frequency could be reduced. Simulation results show that the computation time of the proposed method can be reduced by 7/8 compared to traditional extended Kalman filtering without reducing the filtering precision， and the realtime performance is improved. This method can provide important reference for the research of key technology of trajectory correction projectile. At the same time， the method can be applied to other filtering algorithms， and has a strong portability.

Key words：filtering algorithm； polynomial fitting； trajectory correction projectile； satellite positioning data； filtering error

0 引言

隨著科技的發展，代表科技進步顯著成果之一的電子元器件的發展經歷了從無到有、從簡單到復雜的過程，并不斷向集成化、小型化、低成本等多方向發展。隨著軍事發展，對電子元器件額外提出的高抗過載能力問題也得到很好的解決[1]，這大大促進了彈道修正彈的發展。彈道修正彈內部裝有衛星定位系統，能夠實時獲取彈丸位置、速度等信息，利用該信息計算預測的彈丸落點，通過比較預測落點與目標點距離形成修正指令，修正彈丸飛行彈道，從而使彈丸飛向目標。其中衛星數據準確度直接影響到預測落點的準確度，由于衛星定位系統自身系統誤差，其定位數據不能滿足直接用來預測彈丸落點的要求，需要對定位數據進行降噪濾波，為滿足濾波實時性、準確性要求，基于狀態空間法的卡爾曼濾波理論[2-3]被廣泛應用。其中應用于非線性系統的主要為擴展卡爾曼濾波（Extended Kalman Filtering， EKF）。

EKF算法主要通過對非線性系統進行泰勒展開，忽略高階項來實現非線性系統的線性化。但這種方法處理強非線性系統時，會因對系統描述不準確而濾波效果不好。對此有關研究者提出了二階EKF算法[4-6]，即保留泰勒展開的一、二階項，忽略更高階項，但該方法造成了較大的運算量，影響了運算速度。也有研究者提出通過反復迭代狀態估計的方法[7-9]優化濾波效果，但該方法同樣存在運算量增大的問題。

本文通過適當降低濾波頻率，減少濾波次數的方法來整體提升濾波速率，并基于多項式擬合的方法在濾波點之間插值，補償定位數據更新慢的不足，仿真實驗表明該方法能夠在不擴大濾波誤差的情況下，明顯提高濾波效率。

1 彈道模型的選擇

彈道修正彈內的彈載計算機進行濾波時，必須以彈道模型為濾波基礎，彈道模型越精確濾波算法濾波效果越好，但是彈道模型越精確就意味著彈道模型越復雜，本文以某型100mm一維彈道修正榴彈的彈道為仿真對象，鑒于彈道濾波實時性、快速性的特點，本文以彈道模型較簡單的二維質點彈道[10]建立其彈道模型：

2.2 基于多項式擬合的擴展卡爾曼濾波

在工程應用中，彈道修正彈內卡爾曼濾波算法是根據全球定位系統（Global Positioning System， GPS）接收數據的頻率進行循環濾波的，這樣造成了兩個問題：1） 如果GPS的數據更新頻率太快，則為了保證濾波實時性，要求濾波時間盡可能地短，這對于簡單系統或者簡單濾波算法來說一般還能滿足，對于復雜系統、復雜濾波算法，則會由于每次濾波時間太長，失去濾波實時性；2）如果降低濾波頻率，給濾波計算留出時間，則彈丸在進行落點預測時（落點預測頻率與GPS數據更新頻率相等）會因每次預測落點相隔時間太長，失去最佳修正時機，造成較大的落點偏差，大大影響射彈精度。

本文提出在保證一定濾波精度基礎上適當降低GPS數據更新頻率，從而降低濾波頻率，對于GPS數據更新之間的時間間隔里通過最小二乘擬合的方式擬合插值出新的更新數據[10]。以射程x為例加以說明，為簡化插值算法在短時間內可認為彈丸速度及位置數據變化符合某一元二次拋物線某一段曲線，因此進行數據擬合時最少可僅用三個點，設已濾波降噪得到三個點（x1，x2，x3），利用這三個點求出式（19）中拋物線未知系數a、b、c的值，其中時間t就取彈載計算機內時鐘每個點出現時刻：

利用該表達式，可求出上一次濾波時間與下一濾波時間之間更小時間間隔內的x值，彌補了GPS數據更新頻率低的不足。

設每一次對GPS數據的濾波處理耗時Tm，設tk表示該時刻要對GPS數據進行第k次濾波，圖1為一次完整的基于多項式擬合的擴展卡爾曼濾波流程。由于每次擬合插值計算相對于濾波計算耗時很少，故忽略該部分計算耗時。

3 仿真實驗

3.1 仿真背景設置

彈載計算機測得的彈丸位置及速度信息是基于WGS84協議下的地心地球固連坐標系（ECEF）內，在調用該數據時需要將該坐標下的數據通過坐標變化矩陣轉換到彈載計算機上的地面坐標系內[11]，本文假設已對GPS測量原始數據進行了坐標變化[12]。具體實現過程為，通過建立該彈丸6D剛體彈道模型，解算出一條彈道，獲得的彈道數據包括彈丸不同時刻位置、速度信息，以該彈道數據為原始數據，加上由仿真軟件產生的高斯白噪聲誤差即為模擬GPS定位數據，誤差范圍設置如表1所示。本文以一條射角為35°仿真彈道為基礎， 濾波算法對整條彈道都進行濾波降噪，取彈載計算機GPS數據更新頻率為1Hz，對GPS更新時間間隔插值9次，相當于GPS數據更新頻率變成了10Hz（彈載計算機GPS數據更新常用頻率）。

從圖2、圖4可看出，雖然基于插值擬合的EKF算法濾波頻率只有1Hz，但仍能保持較好的濾波效果，與濾波頻率為10Hz的普通EKF算法兩者濾波效果相當；圖3、圖5中GPS定位誤差曲線與以10Hz頻率濾波誤差曲線重合，說明普通EKF算法對誤差較小的vx、vy濾波效果不明顯，而基于插值擬合的EKF算法濾波誤差出現較大的波動，波動幅值與GPS定位誤差幅值相當，說明其對vx、vy濾波效果也不明顯；表2為不同算法濾波后數據方差表，從表中可看出，以10Hz濾波頻率的普通EKF算法與1Hz濾波頻率的基于插值擬合的EKF算法對x、y都具有較好的濾波效果，兩者對vx、vy濾波效果都不明顯。表3列出了基于Matlab軟件分別建立EKF模型和基于多項式擬合的EKF模型，并在不同仿真條件下對同一條模擬GPS彈道數據進行濾波所用時間，從表中可看出，1Hz濾波頻率的基于插值擬合的EKF算法較濾波頻率較1Hz濾波頻率的普通EKF算法濾波時間增加14.8%，但僅是濾波頻率為10Hz普通EKF濾波時間的1/8，整體濾波速率提升明顯。綜上所述，基于插值擬合的EKF算法在不影響濾波效果情況下，濾波時間上大大縮短。

4 結語

在彈道修正彈研發過程中，彈載計算機必須實時對衛星接收到的彈丸位置、速度等信息進行濾波降噪，傳統濾波方法濾波時間較長，跟不上衛星定位接收機數據更新頻率，大大影響了濾波的實時性，本文提出適當降低衛星定位接收機數據更新頻率，利用多項式插值擬合的方式，插值擬合出衛星定位接收機數據更新時間間隔內的彈丸位置、速度信息。仿真實驗表明，該方法在不降低濾波效果的前提下，能夠明顯降低濾波時間，提高彈丸位置、速度信息更新的實時性，對于彈道修正彈關鍵技術的研究提供了重要方法。同時該方法可移植性強，可推廣應用到其他濾波算法當中。

參考文獻：

[1]GRACE J. GPS guidance system increases projectile accuracy [J]. IEEE Aerospace and Electronic Systems Magazine， 2000， 15（6）： 15-17.

[2]KALMAN R E. A new approach to linear filtering and prediction theory [J]. Journal of Basic Engineering， 1960， 82（1）： 35-45.

[3] KALMAN R E， BUCY R S. New results in linear filtering and prediction theory [J]. Journal of Basic Engineering， 1961， 83（1）： 95-108.

[4]JAWINSKI A H. Filtering for nonlinear dynamical systems [J]. IEEE Transactions on Automatic Control， 1966， 11（4）： 765-766.

[5]范煒，李勇.近似二階擴展卡爾曼濾波方法研究[J].空間控制技術與應用，2009，35（1）：30-35. （FAN W， LI Y. A method of approximate secondorder extended Kalman filter [J]. Aerospace Control and Application， 2009， 35（1）： 30-35.）

[6]孟真，閻躍鵬，于進勇.基于二階泰勒展開的擴展卡爾曼濾波測頻算法[J].江蘇大學學報（自然科學版），2010， 31（5）：564-569. （MENG Z， YAN Y P， YU J Y. Extended Kalman filter for frequency measurement based on twoorder Taylor expansion [J]. Journal of Jiangsu University （Natural Science Edition）， 2010， 31（5）： 564-569.）

[7]BELL B M， CATHEY F W. The iterated Kalman filter update as a GaussNewton method[J]. IEEE Transactions on Automatic Control， 1993， 38（2）： 294-297.

[8]朱建峰.擬線性最優平滑濾波在指令控制——維彈道修正彈上的應用研究[D].南京：南京理工大學，2012：32-34. （ZHU J F. Application research of quasilinear optimum smoothing filtering to onedimensional ballistic in command control [D]. Nanjing： Nanjing University of Science and Technology， 2012： 32-34.）

[9]歐陽廣帥，周晶.基于卡爾曼濾波的高精度彈道濾波算法研究[J].電子測量技術，2014，37（11）：16-19，24. （OUYANG G S， ZHOU J. High accuracy ballistic filtering algorithm based on the Kalman filter [J]. Electronic Measurement Technology， 2014， 37（11）： 16-19， 24.）

[10]梁民贊，黃子豪，曹占啟.曲線擬合與卡爾曼濾波器的濾波精度評估[J].無線電工程，2013，43（3）：36-39. （LIANG M Z， HUANG Z H， CAO Z Q. Accuracy assessment of filtering based on curve fitting and Kalman filter [J]. Radio Engineering of China， 2013，43（3）： 36-39.）

[11]邱致和，王萬義.GPS原理與應用[M].北京：電子工業出版社，2002：10-36. （QIU Z H， WANG W Y. GPS Principle and Application[M]. Beijing： Publishing House of Electronics Industry， 2002： 10-36.）

[12]申強，葛靦，彭博，等.基于GPS彈道測量的卡爾曼濾波參數估計算法[J].北京理工大學學報，2009，29（12）：1048-1051. （SHEN Q， GE M， PENG B， et al. Parameters estimation algorithm by Kalman filtering based on GPS measurement for projectile trajectory [J]. Transactions of Beijing Institute of Technology， 2009， 29（12）： 1048-1051.）