靳 標，李建行，朱德寬，郭 交※，蘇寶峰

?

基于自適應有限沖激響應-卡爾曼濾波算法的GPS/INS導航

靳 標1,2,3，李建行1，朱德寬1，郭 交1,2,3※，蘇寶峰1,2,3

（1. 西北農(nóng)林科技大學機電學院，楊凌 712100；2. 農(nóng)業(yè)農(nóng)村部農(nóng)業(yè)物聯(lián)網(wǎng)重點實驗室，楊凌 712100； 3. 陜西省農(nóng)業(yè)信息感知與智能服務重點實驗室，楊凌 712100）

導航定位系統(tǒng)一般采用卡爾曼濾波算法提高定位精度。傳統(tǒng)卡爾曼濾波算法的性能很大程度上依賴觀測噪聲的先驗統(tǒng)計信息，不精確的統(tǒng)計特性將會降低定位精度。針對此問題，該文提出一種基于FIR（finite impulse response）預測模型的卡爾曼濾波算法。將FIR預測模型與卡爾曼濾波結合，F(xiàn)IR預測模型的系數(shù)可以通過求解一個凸二次規(guī)劃問題得到。該凸二次規(guī)劃以目標的多項式運動規(guī)律為約束條件，以最小白噪聲增益為目標函數(shù)，具有閉式解。仿真試驗和實測結果均表明，在相同的參數(shù)設置條件下，基于FIR預測模型的卡爾曼濾波算法比傳統(tǒng)的卡爾曼濾波算法具有更高的估計精度，仿真結果表明定位精度提高29.54%，實測結果表明方向定位精度提高21.71%，方向定位精度提高22.62%。該算法可應用于GPS接收信號的降噪處理，提高目標狀態(tài)的定位精度。

導航；模型；FIR預測模型；自適應卡爾曼濾波；全球定位系統(tǒng)

0 引 言

全球定位系統(tǒng)（global positioning system, GPS）和慣性導航系統(tǒng)（inertial navigation system, INS）在目標定位、車輛導航和精準農(nóng)業(yè)等領域得到了廣泛的應用。然而由于衛(wèi)星信號遮擋、多路徑效應和觀測誤差等因素的影響，濾波結果通常會出現(xiàn)較大誤差[1-2]。

卡爾曼濾波是GPS數(shù)據(jù)處理中常用的濾波算法[3-8]。其性能取決于狀態(tài)向量的動態(tài)模型和描述噪聲特性的隨機模型[9-10]。目前相對應的自適應卡爾曼濾波算法有2種：一種是基于多模型的自適應估計（multiple-model-based adaptive estimation, MMAE）[11-13]，另一種是基于新息的自適應估計（innovation-based adaptive estimation, IAE）[14-15]。第一種方法是在不同的動態(tài)模型和統(tǒng)計信息下，利用一組并行運行的卡爾曼濾波器，將具有非零模型概率的所有模型結合起來。第二種方法是基于新息序列的變化直接通過計算觀測噪聲或過程噪聲協(xié)方差矩陣來完成自適應濾波。MMAE和IAE方法中通常采用離散時間微分模型，例如用恒定速度的CV（constant velocity）模型和恒定加速度的CA（constant acceleration）模型來描述狀態(tài)變量的變化過程[16]。然而，位置、速度和姿態(tài)等狀態(tài)向量是相關的，準確描述這些狀態(tài)的統(tǒng)計關系是非常困難的，當先驗信息不充分時，各個濾波狀態(tài)的耦合效應也將給定位結果造成較大誤差。基于離散時間微分模型的卡爾曼濾波算法的另一個缺點是它高度依賴過程噪聲和觀測噪聲的先驗統(tǒng)計信息。一般來說，過程噪聲和觀測噪聲的先驗統(tǒng)計信息取決于運動過程和應用場景，一般較難準確獲得。濾波器的先驗統(tǒng)計信息不足，會降低濾波狀態(tài)的估計精度，甚至導致濾波器估計結果出現(xiàn)發(fā)散現(xiàn)象。

自適應卡爾曼濾波算法的研究主要集中在在線計算過程噪聲或觀測噪聲的協(xié)方差，目前很少有關于自適應狀態(tài)模型的研究報道[17-18]。一個有效的狀態(tài)模型將在很大程度上有利于從觀測數(shù)據(jù)中提取有用的狀態(tài)信息。針對以上問題，本文針對白噪聲背景提出一種基于FIR預測模型的卡爾曼濾波算法。將FIR預測模型嵌入卡爾曼濾波算法，F(xiàn)IR預測模型的系數(shù)可以通過求解一個凸二次規(guī)劃問題得到。該凸二次規(guī)劃以目標的多項式運動規(guī)律為約束條件，以最小白噪聲增益為目標函數(shù)，具有閉式解。仿真試驗和實測結果均表明在相同的參數(shù)設置條件下，基于FIR預測模型的卡爾曼濾波算法比傳統(tǒng)的卡爾曼濾波算法具有更高的估計精度。該算法可應用于GPS/INS系統(tǒng)中的信息融合前的單狀態(tài)估計，或適用于GPS接收機后處理過程中的降噪。

1 FIR預測模型

目前，卡爾曼濾波算法的運動狀態(tài)模型大多數(shù)都屬于離散時間微分模型，本質(zhì)上是多項式模型。多項式模型能準確地刻畫目標運動狀態(tài)的變化規(guī)律，并且可以提供目標在不同時刻的位置、速度和加速度等信息。但是這類模型是固定的，不能隨著濾波的過程和觀測噪聲強度的變化而改變，這在某種程度上影響了算法的性能[19]。本文提出一種針對白噪聲背景的狀態(tài)模型——FIR預測模型。該模型不僅能刻畫目標的運動狀態(tài)，還可以濾除白噪聲。

1.1 由多項式模型到FIR模型

將式（3）帶入式（2），可得：

式（5）兩端都是關于的級數(shù)和，并且形式相同，因此通項公式相等，即

當=0時，由式（6）可得：

當>0時，假設當-1時，式（6）成立，即

由式（6）可得

根據(jù)二項式定理可得

將式（10）帶入式（9），可得

由式（8）可知，式（11）等號右邊第二項為0，即可得到

即式（8）成立。

式（7）和式（8）可以表示為矩陣形式

1.2 最小白噪聲增益條件下的FIR預測模型

實際情況中觀測信號都是混有噪聲的，理論分析時通常假定噪聲為白噪聲。為了使噪聲通過濾波器的增益最小，通常希望白噪聲的濾波器增益[22]最小。白噪聲的濾波器增益為：

結合約束條件，以最小白噪聲增益作為優(yōu)化準則，可得FIR預測模型的目標函數(shù)

目標函數(shù)（12）屬于帶有線性約束的凸二次規(guī)劃問題[23-24]，這是一個標準的具有條件的最小二乘問題，通常應用于測量或大地測量中非線性問題。定義目標函數(shù)式（18）的拉格朗日方程為

其中為拉格朗日乘子向量。

拉格朗日方程的梯度為

令矩陣

由于矩陣為行滿秩，因此矩陣為非奇異，于是可以得到式（19）拉格朗日方程的唯一最小解

即

其中，最優(yōu)拉格朗日乘子向量

即式（25）是優(yōu)化問題的唯一全局最小值。

2 基于FIR預測模型的卡爾曼濾波算法

目標狀態(tài)的觀測方程為

基于FIR預測模型的卡爾曼濾波算法具體流程為：

1）濾波器初始化：

給定，值，并且≥+2；

2）狀態(tài)轉移矩陣計算：

3）狀態(tài)預測：

4）狀態(tài)更新：

增益矩陣：

后驗估計均值：

后驗估計誤差協(xié)方差：

5）令1，返回步驟3）。

3 試驗與結果分析

3.1 仿真試驗

根據(jù)理論推導可知速度和初始距離對該模型無影響，因此假設目標以=20 m/s的速度勻速運動，初始距離為=10 km。觀測噪聲的方差=100 m2，采樣間隔Δ= 1 s，總的觀測時間為100 s[19]。根據(jù)本文的理論推導，設置FIR預測模型系數(shù)的個數(shù)分別為=2，3；在觀測噪聲為白噪聲的情況下，F(xiàn)IR預測模型系數(shù)的個數(shù)也為=2，3。FIR預測模型的階數(shù)均為=1。FIR預測模型的狀態(tài)噪聲協(xié)方差由式（17）給出，為了與FIR預測模型保持一致，CV模型的狀態(tài)噪聲協(xié)方差為[15]

從圖1的仿真結果可以看出：

1）在白噪聲背景下FIR預測模型（=1，=2）的情況和CV模型有相同的定位精度（由于噪聲的隨機性引起圖1中CV模型與FIR預測模型（=1,=2）的2條線不完全重合，如果Monte-Carlo次數(shù)此時足夠多是可以重合的），證明了上文中提到的FIR預測模型與CV模型等價，并且都可以用來描述勻速運動。

2）在定位精度上，F(xiàn)IR預測模型（=1，=3）的情況優(yōu)于CV模型，其精度提高29.54%。因為該模型不僅可以滿足多項式運動的CV模型，而且具有一定的降噪效果。另外，在白噪聲背景下，考慮到使濾波的增益最小，在某種程度上進一步降低了噪聲的影響。

3）比較圖1a和圖1b，可以看出最優(yōu)FIR預測模型在參數(shù)設置與實際運動不匹配時，要優(yōu)于傳統(tǒng)的離散時間微分模型。

注：qr代表狀態(tài)噪聲強度，N代表FIR預測模型的階數(shù)，M表示FIR預測模型系數(shù)的個數(shù)。

3.2 實測試驗

為了評估所提出算法的定位精度，2018年8月10日在西北農(nóng)林科技大學北校區(qū)操場進行多次實際測試。測試設備如圖2a所示，GNSS RTK系統(tǒng)由HI-TARGET測量儀器有限公司制造，型號為A10，其定位精度為（10+ 1′10–6′）m，表示移動站與基站之間的距離，實測時≤300 m。手持式GNSS接收機為集思寶G130型號。手持式GNSS接收機只接收GPS衛(wèi)星信號，理論定位精度為3～5 m。以GNSS RTK系統(tǒng)測得的軌跡作為真實軌跡的參考值，手持式GNSS接收機測得的數(shù)據(jù)作為需要濾波的軌跡。根據(jù)理論推導可知，F(xiàn)IR預測模型與運動速度無關，因此移動站與手持式GNSS接收機由1個人手持以近似1 m/s勻速繞操場1周。

測試10次取平均值，將數(shù)據(jù)導入Google earth，測試軌跡如圖2b所示。與仿真試驗的采樣間隔保持一致，2個接收機的采樣頻率均為1 Hz。

圖2 測試設備及測試軌跡

圖3 X方向和Y方向位置誤差

圖3a和圖3b分別給出了CV模型、白噪聲狀態(tài)背景下FIR預測模型（=1,=3）算法濾波后方向（東西方向）和方向（南北）位置誤差。由圖3可以看出，在白噪聲狀態(tài)背景下FIR預測模型（=1,=3）算法明顯優(yōu)于CV模型，經(jīng)計算東西方向定位精度提高21.71%，南北方向定位精度提高22.62%。因為在勻速直線運動時FIR預測模型可以在最小均方誤差的意義下盡可能地降低白噪聲的影響，并且FIR預測模型可以通過新息序列利用在線信息。

4 結 論

基于FIR預測模型的卡爾曼濾波算法可以由求解凸二次規(guī)劃問題得到最優(yōu)解。白噪聲背景下的FIR預測模型不僅滿足狀態(tài)變量的多項式約束，而且降低了噪聲的影響。該算法可應用于GPS/INS系統(tǒng)中的信息融合前的單狀態(tài)估計，或適用于GPS接收機后處理過程中的降噪。相比于傳統(tǒng)算法，該算法具有以下優(yōu)點：

1）基于FIR預測模型的濾波算法可認為對測量數(shù)據(jù)進行2次濾波，即首先由FIR濾波，然后用卡爾曼濾波器對濾波數(shù)據(jù)進行濾波。因此，基于FIR預測模型的卡爾曼濾波算法的去噪效果優(yōu)于傳統(tǒng)的對數(shù)據(jù)進行1次濾波的去噪效果。

2）基于FIR預測模型的卡爾曼濾波算法通過一定程度的多項式逼近運動軌跡，利用FIR預測模型預測位置。線性化的技術可以應用到相似的的非線性估計中。

3）基于FIR預測模型的卡爾曼濾波算法的計算量與傳統(tǒng)卡爾曼濾波相當，F(xiàn)IR預測模型的系數(shù)具有閉式解。該算法定位精度高于傳統(tǒng)的卡爾曼濾波算法，實測試驗結果表明，東西方向定位精度提高21.71%，南北方向定位精度提高22.62%。

[1] 李增科，王堅，高井祥，等. 自適應聯(lián)邦濾波器在GPS-INS-Odometer組合導航的應用[J]. 測繪學報，2016，45(2)：157－163. Li Zengke, Wang Jian, Gao Jingxiang, et al. The application of adaptive federated filter in GPS-INS-Odometer integrated navigation[J]. Acta Geodaetica et Cartographica Sinica, 2016, 45(2): 157－163. (in Chinese with English abstract)

[2] 楊元喜. 自適應動態(tài)導航定位[M]. 北京：測繪出版社，2006.

[3] 譚興龍，王堅，韓厚增. 支持向量回歸輔助的GPS/INS組合導航抗差自適應算法[J]. 測繪學報，2014，43(6)：590－597，606. Tan Xinglong, Wang Jian, Han Houzeng. SVR aided adaptive robust filtering algorithm for GPS/INS integrated navigation[J]. Acta Geodaetica et Cartographica Sinica, 2014, 43(6): 590－597, 606. (in Chinese with English abstract)

[4] Zhao, H S, Miao, L J, Shao, H J. Adaptive two-stage kalman filter for SINS/Odometer integrated navigation systems[J]. Journal of Navigation, 2016, 70(2), 242－261.

[5] Kim J, Bálint Kiss, Lee D. An adaptive unscented kalman filtering approach using selective scaling[C]//IEEE International Conference on Systems. IEEE, 2017: 784－789.

[6] Lin S G. Assisted adaptive extended kalman filter for low- cost single-frequency GPS/SBAS kinematic positioning[J]. GPS Solutions, 2015, 19(2): 215－223.

[7] 譚興龍，王堅，趙長勝. 神經(jīng)網(wǎng)絡輔助的GPS/INS組合導航自適應UKF算法[J]. 測繪學報，2015，44(4)：384－391. Tan Xinglong, Wang Jian, Zhao Changsheng. Neural network aided adaptive UKF algorithm for GPS/INS integrated navigation[J]. Acta Geodaetica et Cartographica Sinica, 2015, 44(4): 384－391. (in Chinese with English abstract)

[8] Sarkka S, Tolvanen V, Kannala J, et al. Adaptive kalman filtering and smoothing for gravitation tracking in mobile systems[C]//IEEE International Conference on Indoor Positioning and Indoor Navigation. IEEE, 2015: 1－7.

[9] 秦康，董新民，陳勇. 基于Huber的魯棒高階容積卡爾曼濾波算法[J]. 計算機工程與應用，2017，53(7)：21－29. Qin Kang, Dong Xinmin, Chen Yong. Huber-based robust high-degree cubature kalman filter algorithm[J]. Computer Engineering and Applications, 2017, 53(7): 21－29. (in Chinese with English abstract)

[10] Niehen W. Adaptive kalman filtering based on matched filtering of the innovation sequence[C]//Proceedings of the 7th International Conference on Information Fusion. International Society of Information Fusion, 2004: 362－369.

[11] Hide C, Moore T, Smith M. Multiple model kalman filtering for GPS and low-cost INS integration[C]// Proceedings of the 17th International Technical Meeting of the Satellite Division of the Institute of Navigation. Long Beach, CA, 2004: 1096－1103.

[12] Lan J, Li X R, Mu C. Best model augmentation for variable structure multiple-model estimation[J]. IEEE Transactions on Aerospace & Electronic Systems, 2011, 47(3): 2008－2025.

[13] Jin B, Jiu B, Su T, et al. Switched kalman filter-interacting multiple model algorithm based on optimal autoregressive model for maneuvering target tracking[J]. IET Radar Sonar Navigation, 2015, 9(2): 199－209.

[14] Mehra R K. Approaches to adaptive filtering[J]. IEEE Transactions on Automatic Control, 1972, 17(5): 693－698.

[15] Mohamed A H, Schwarz K P. Adaptive kalman filtering for INS/GPS[J]. Journal of Geodesy, 1999, 73(4): 193－203.

[16] Li X R, Jilkov V P. Survey of maneuvering target tracking. Part I. Dynamic models[J]. IEEE Transactions on Aerospace & Electronic Systems, 2003, 39(4): 1333－1364.

[17] Ding W, Wang J, Rizos C, et al. Improving adaptive kalman estimation in GPS/INS integration[J]. Journal of Navigation, 2007, 60(3): 517－529.

[18] 甘雨，隨立芬，劉長建，等. 自適應抗差Kalman 濾波

在多天線原始觀測值順勢姿態(tài)確定中的應用[J]. 測繪學報，2015，44(9)：945－951. Gan Yu, Sui Lifen, Liu Changjian, et al. Instantaneous attitude determination based on original multi-antenna observations using adaptively robust kalman filtering[J]. Acta Geodaetica et Cartographica Sinica, 2015, 44(9): 945－951. (in Chinese with English abstract)

[19] Jin B, Guo J, He D, et al. Adaptive kalman filtering based on optimal autoregressive predictive model[J]. GPS Solutions, 2017, 21(2): 1－11.

[20] Perez D, Quintana Y. A survey on the weierstrass approximation theorem[J]. Divulgaciones Matematicas, 2008, 16(1): 231－247.

[21] Shcherbakov M A, Krevchik V D, Sazonov V V. An algebraicapproach to implementation of generalized polynomial filters[C]// IEEE International Siberian Conference on Control & Communications. IEEE, 2015: 1－3.

[22] 高羽，張建秋，尹建君. 機動目標的多項式預測模型及跟蹤算法[J]. 航空學報，2009，30(8)：1479－1489. Gao Yu, Zhang Jianqiu, Yin Jianjun. Polynomial prediction model and tracking algorithm of maneuver target[J]. Acta Aeronautica et Astronautica Sinica, 2009, 30(8): 1479－1489. (in Chinese with English abstract)

[23] Elias Munapo, Santosh Kumar. A new heuristic for the convex quadratic programming problem[J]. American Journal of Operations Research, 2015, 5(5): 373－383.

[24] Cheng L, Deng Z, Jin Q. An eigenvalue decomposition based branch-and-bound algorithm for nonconvex quadratic programming problems with convex quadratic constraints[J]. Journal of Global Optimization, 2017, 67(3): 1－19.

[25] Cao Z, Guo H, Zhang J, et al. Improving the efficiency of stochastic vehicle routing: A partial lagrange multiplier method[J]. IEEE Transactions on Vehicular Technology, 2016, 65(6): 3993－4005.

[26] Jin B, Su T, Zhang W, et al. Joint optimization of predictive model and transmitted waveform for extended target tracking[C]//IEEE International Conference on Signal Processing. IEEE, 2015: 1914－1918.

GPS/INS navigation based on adaptive finite impulse response-Kalman filter algorithm

Jin Biao1,2,3, Li Jianxing1, Zhu Dekuan1, Guo Jiao1,2,3※, Su Baofeng1,2,3

(1.712100; 2.712100,; 3.712100,)

Global positioning system (GPS) and inertial navigation system (INS) are widely used in target positioning, vehicle navigation, precision agriculture and other fields. However, due to factors such as satellite signal occlusion, multi-path effect and observation error, the filtering results usually have large errors. Kalman filtering algorithm is generally used in navigation and positioning system to improve positioning accuracy. The performance of kalman filter algorithm depends on the dynamic model of state vector and the random model describing noise characteristics. There are 2 corresponding adaptive kalman filtering algorithms: one is the multiple-model-based adaptive estimation (MMAE); the other is the innovation-based adaptive estimation (IAE). The first method is to combine all models with non-zero model probability by using a set of parallel kalman filters under different dynamic models and statistical information. The second method complete the adaptive filtering directly by calculating the observation noise or process noise covariance matrix based on the change of information sequence. In MMAE and IAE methods, discrete time differential models are usually adopted, such as the constant velocity CV model and constant acceleration CA model, to describe the change process of state variables. However, the state vectors such as position, velocity and attitude are correlated, and it is very difficult to accurately describe the statistical relations of these states. When the prior information is not sufficient, the coupling effect of each filtering state will also cause large errors to the positioning results. Another disadvantage of kalman filter algorithm based on discrete time differential model is that it highly depends on the prior statistical information of process noise and observation noise. Generally, the prior statistical information of process noise and measurement noise depends on the motion process and application scene, which is difficult to be obtained accurately. Insufficient prior statistical information of the filter will reduce the estimation accuracy of the filter state and even lead to the divergence of the filter estimation results. The research of adaptive kalman filter algorithm is mainly focused on the covariance of online calculation process noise or measured noise. In order to improve the accuracy of navigation and positioning, an adaptive kalman filter algorithm based on FIR (finite impulse response) prediction model for white noise background was proposed in this paper. Firstly, the continuous trajectory function of moving target was approximated by an-order polynomial with arbitrary precision, and the FIR prediction model polynomial was obtained. The FIR prediction model coefficient was obtained by solving a convex quadratic programming problem, and the optimal solution of FIR prediction model coefficient was solved by lagrange multiplier method. The convex quadratic programming taken the polynomial motion law of the target as the constraint condition and the minimum white noise gain as the objective function, and the optimal solution could not only satisfy the constraints of the target's motion state, but also had the effect of de-noising to a certain extent. Finally, the proposed FIR prediction model was combined with kalman filter. Simulation test and the measurement results showed that kalman filtering algorithm based on FIR prediction model had higher estimation accuracy than the traditional kalman filtering algorithm under the same parameter settings, and the simulation experiment results showed that the localization precision was increased by 29.54%, the measured experimental results showed that positioning accuracy ineast-west direction increased by 21.71%, positioning error innorth-south direction increased by 22.62%. The proposed algorithm could be used for single state estimation before information fusion in loosely coupled GPS/INS, and also for noise reduction in post-processing of GPS receivers.

navigation; models; FIR prediction model; adaptive Kalman filtering; global positioning system

靳 標，李建行，朱德寬，郭 交，蘇寶峰. 基于自適應有限沖激響應-卡爾曼濾波算法的GPS/INS導航[J]. 農(nóng)業(yè)工程學報，2019，35(3)：75－81. doi：10.11975/j.issn.1002-6819.2019.03.010 http://www.tcsae.org

Jin Biao, Li Jianxing, Zhu Dekuan, Guo Jiao, Su Baofeng. GPS/INS navigation based on adaptive finite impulse response-Kalman filter algorithm[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(3): 75－81. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2019.03.010 http://www.tcsae.org

2018-09-11

2019-01-10

國家自然科學基金（61701416）；中央高?；究蒲袠I(yè)務費專項資金（2452017127）；農(nóng)業(yè)農(nóng)村部農(nóng)業(yè)物聯(lián)網(wǎng)重點實驗室開放基金課題（2017AIOT-06）

靳 標，講師，博士，研究方向為GPS信號處理和精準農(nóng)業(yè)。Email：biaojin@nwafu.edu.cn

郭 交，副教授，博士，研究方向為GPS信號處理和精準農(nóng)業(yè)。Email：jiao.g@163.com

10.11975/j.issn.1002-6819.2019.03.010

TP391.4

A

1002-6819(2019)-03-0075-07