劉 成，郭樹人，李 芳

(1. 北京跟蹤與通信技術(shù)研究所，北京100094； 2. 中國科學(xué)院國家天文臺，北京 100190)

衛(wèi)星導(dǎo)航定位卡爾曼濾波時延平滑方法研究

劉 成1，郭樹人1，李 芳2

(1. 北京跟蹤與通信技術(shù)研究所，北京100094； 2. 中國科學(xué)院國家天文臺，北京 100190)

為進一步提高衛(wèi)星導(dǎo)航定位精度和性能，介紹和討論了卡爾曼濾波延時平滑方法，并建立了一種卡爾曼濾波延時平滑定位模型與算法。與經(jīng)典卡爾曼濾波類似，該算法同樣具有狀態(tài)系統(tǒng)與測量系統(tǒng)兩類方程，能夠根據(jù)后續(xù)歷元測量值及定位結(jié)果向前進行逆向預(yù)測與更新，從而達到對整體定位過程再次平滑的目的，更大程度地獲得定位結(jié)果跟隨性與平滑性之間的平衡。通過多種不同情況下的GPS實測試驗，對方法的正確性和可行性進行了分析與驗證。結(jié)果表明，卡爾曼濾波延時平滑定位方法能夠適用于多種靜態(tài)、動態(tài)導(dǎo)航應(yīng)用領(lǐng)域，使輸出結(jié)果更為平滑和準確，在有效提高定位精度的同時具有更強的抗多徑和抗干擾性。

GPS；衛(wèi)星導(dǎo)航定位；卡爾曼濾波；固定時刻平滑；固定時延平滑；車載導(dǎo)航

卡爾曼濾波(Kalman filtering)方法[1]對衛(wèi)星導(dǎo)航定位具有重要意義?；谖矬w運動遵循慣性定律這一基本事實，它利用合適的運動模型，將相鄰時刻的位置狀態(tài)聯(lián)系起來，從而克服最小二乘法在不同時刻的定位值互不關(guān)聯(lián)這一缺點，將原本孤立雜亂的解結(jié)合起來，使濾波定位結(jié)果更為平滑和準確[2-4]。因此，卡爾曼濾波及其擴展卡爾曼濾波[5]、線性化卡爾曼濾波[6]、不敏卡爾曼濾波[7]等在衛(wèi)星導(dǎo)航中得到了廣泛的應(yīng)用，并不斷發(fā)展完善。

卡爾曼濾波通過狀態(tài)方程一步轉(zhuǎn)移實現(xiàn)系統(tǒng)狀態(tài)的預(yù)測與更新，能夠?qū)崟r獲得用戶位置。然而，它在“跟隨(tracking)”與“平滑(smoothing)”兩個特征之間仍然存在著矛盾。實際應(yīng)用中，往往通過調(diào)整濾波參數(shù)來實現(xiàn)這種平衡。如通過將濾波中狀態(tài)噪聲或測量噪聲的協(xié)方差矩陣系數(shù)刻意調(diào)大，可降低測量值權(quán)重[8-9]，從而使濾波結(jié)果更加平滑。但一方面，為保證良好的跟隨性，反映出最小二乘解的真實性，濾波結(jié)果往往容易顯得不夠平滑，仍具有最小二乘解的雜亂性；另一方面，為追求良好的平滑性，使濾波結(jié)果更為美觀，則又容易導(dǎo)致其偏離用戶真實位置，產(chǎn)生“失真”的感覺。

基于卡爾曼濾波原理的延時平滑方法能夠很好地解決這一問題。與根據(jù)經(jīng)驗和測試結(jié)果人為調(diào)整濾波參數(shù)不同，延時平滑方法從卡爾曼濾波模型出發(fā)進行理論公式推導(dǎo)，具有嚴謹?shù)臄?shù)學(xué)性，能夠根據(jù)后續(xù)測量值及定位結(jié)果進行反饋與修正，達到對整個定位過程再次平滑的目的。它適用于許多對實時性要求不甚敏感的導(dǎo)航應(yīng)用領(lǐng)域，如靜止定位、行人定位、健康與運動監(jiān)測、交通路況分析與規(guī)劃等；甚至在實時車載定位的某些情況下，由于車輛并非一直處于高速行駛狀態(tài)，因此也能夠獲得應(yīng)用。

本文從卡爾曼濾波基本原理出發(fā)，介紹卡爾曼濾波延時平滑方法，分析和討論其理論推導(dǎo)過程，并由此提出一類衛(wèi)星導(dǎo)航定位中的卡爾曼濾波延時平滑方法。該方法能夠根據(jù)后續(xù)歷元測量值及定位結(jié)果向前進行逆向預(yù)測與更新，具有狀態(tài)系統(tǒng)與測量系統(tǒng)兩類方程；能夠獲得跟隨與平滑之間的良好平衡，使定位結(jié)果更加準確和平滑。此外，由于可以使用后續(xù)狀態(tài)量及測量值，因此方法具有更高的抗多徑和抗遮擋性，能夠進一步提高定位精度與性能。最后，通過靜止定位、動態(tài)定位等多種不同情況下的GPS實測試驗，對該方法進行分析與驗證，以證明其正確性與可行性。

1 卡爾曼濾波定位方法

1.1 卡爾曼濾波回顧

卡爾曼濾波自1960年被提出以來，不斷得到完善和發(fā)展，已廣泛應(yīng)用于各個領(lǐng)域的信號與數(shù)據(jù)處理中。有關(guān)卡爾曼濾波的文獻也相當豐富，且涉及隨機變量、隨機過程等多方面的知識和內(nèi)容。因此，本文在此僅作簡要回顧，不作具體推導(dǎo)。

對于如下形式的狀態(tài)空間模型[10-11]

(1)

式中，xi、yi分別為系統(tǒng)狀態(tài)與測量向量；ni、vi分別為狀態(tài)噪聲與測量噪聲向量；Fi、Gi與Hi分別為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣、輸入關(guān)系矩陣與測量關(guān)系矩陣?？柭鼮V波假定ni和vi為零均值、不相關(guān)的高斯白噪聲，即滿足

(2)

式中，Qi、Ri分別為ni和vi的協(xié)方差矩陣；δij為克羅內(nèi)克函數(shù)(Kronecker Delta)。

測量更新方程

(3)

時間更新方程

(4)

1.2 卡爾曼濾波定位模型

衛(wèi)星導(dǎo)航遵循所有運動物體都符合牛頓定律這一基本事實，因此卡爾曼濾波定位模型可通過對不同運動特點的用戶采用不同的狀態(tài)方程模型來進行描述和實現(xiàn)。運動狀態(tài)方程既要盡可能真實、完整地描述用戶運動狀態(tài)，又要控制狀態(tài)向量的維數(shù)，減少計算量。

對于行人、汽車、艦船等低動態(tài)載體而言，其運動狀態(tài)一般可用8個狀態(tài)變量來描述，即3個位置分量(x,y,z)、3個速度分量(vx,vy,vz)和2個接收機鐘差變量(δtu,δfu)。該運動模型因不考慮載體機動因素，因此又稱為常速運動模型。其狀態(tài)向量為[2,12-14]

x=[xyzvxvyvzδtuδfu]T

(5)

根據(jù)所采用的運動狀態(tài)模型，其常系數(shù)狀態(tài)轉(zhuǎn)移矩陣F為

(6)

狀態(tài)噪聲協(xié)方差矩陣Q為

(7)

在實際應(yīng)用中，準確確定狀態(tài)量噪聲與測量值噪聲的均方誤差Q、R并不容易。對于狀態(tài)噪聲誤差Q，一方面可參考接收機說明書及性能測試指標等有關(guān)數(shù)據(jù)，另一方面可對此噪聲信號作自相關(guān)運算來進行估計。而對于測量值噪聲誤差R，可根據(jù)導(dǎo)航電文中的URA值、衛(wèi)星仰角、信號強度、接收機信號跟蹤狀況等信息進行估算，在實踐中也可通過試驗來測定測量誤差方差，然后建立一個關(guān)于信號信噪比或衛(wèi)星仰角的測量誤差方差函數(shù)[2,12]。

2 卡爾曼濾波時延平滑方法

2.1 固定時刻平滑

根據(jù)式(4)中的卡爾曼濾波時間更新方程，可遞推得知當i0>i時有[10]

(8)

式(8)即可視為利用狀態(tài)轉(zhuǎn)移矩陣對狀態(tài)量未來的狀態(tài)進行i0-i步預(yù)測。現(xiàn)在,反過來考慮i0

(9)

其協(xié)方差矩陣為

(10)

由修正量的正交性原則可推導(dǎo)得[15]

(11)

(12)

對于如式(1)給出的一個標準的狀態(tài)空間模型，當j≥i時有

(13)

對式(13)進行矩陣求逆可得

(14)

再定義矩陣

(15)

(16)

(17)

將式(17)代入式(16)可得

(18)

由式(12)和式(3)、式(4)可得當i≥i0時有

(19)

(20)

結(jié)合式(19)、式(20)可得協(xié)方差矩陣

(21)

為計算矩陣M的遞推公式，由式(13)—式(15)可推導(dǎo)得到

(22)

再將式(15)代入式(22)即有

(23)

2.2 固定時延平滑

(24)

式中，協(xié)方差P提供了平滑值的誤差特征情況。事實上它只是數(shù)學(xué)上的存在，在整個遞推計算過程中是非必需的，但實際使用中也可利用它進行數(shù)值正確性的監(jiān)測與檢驗。

2.3 卡爾曼濾波時延平滑定位流程

圖1 卡爾曼濾波延時平滑計算流程

卡爾曼延時平滑定位方法既可應(yīng)用于靜止定位情況，也可應(yīng)用于車載定位等動態(tài)情況，以及定位數(shù)據(jù)事后處理與分析等領(lǐng)域。

以靜止定位數(shù)據(jù)處理為例，往往通過對一段時間的定位數(shù)據(jù)取平均來獲得當前位置更精確的坐標值。然而，由于接收機測量精度及衛(wèi)星星座幾何分布都并非固定不變的，因此定位結(jié)果之間往往并不具有相同的誤差分布，從而使得這種籠統(tǒng)的均值處理方法顯得過于粗糙。另外，由于缺乏誤差特征指標，因此當測量或定位出現(xiàn)粗差時也難以及時發(fā)現(xiàn)?；诳柭鼮V波原理的平滑計算方法則不同，它具有嚴謹?shù)臄?shù)學(xué)原理及模型，不同時刻的測量精度及衛(wèi)星分布信息均包含在協(xié)方差矩陣Pj|j-1和Pj|j中。因此，在連貫的誤差指標指引下，它能夠獲得最優(yōu)的遞歸最小二乘平滑結(jié)果，并且具有較強的抗差性。

3 試驗與分析

利用搭載由中科微電子有限公司研制與提供的ATGM332D定位芯片的衛(wèi)星導(dǎo)航接收機模塊，以1 Hz頻率通過串口傳輸方式輸出GPS衛(wèi)星廣播星歷及單頻、C/A碼偽距觀測數(shù)據(jù)?；谠撃K及其測量輸出進行Arm編程實現(xiàn)，并在北京市中科院奧運村科學(xué)園區(qū)進行實時定位試驗。

3.1 靜止定位試驗

首先，在園區(qū)已知坐標點上進行了時間約2 h的靜止定位測量，按照圖1中流程分別采用固定延時L為1、5、10及30 min的卡爾曼延時平滑方法進行處理，并將其與實時卡爾曼濾波定位結(jié)果相比較，如圖2所示。

圖2 靜止定位結(jié)果比較

統(tǒng)計各類處理方法誤差特征，結(jié)果見表1。

表1 各濾波定位方法均方誤差統(tǒng)計 m

圖2及表1中的試驗結(jié)果驗證了該方法的正確性和有效性。從中也可以看出，該方法特別適用于靜止定位處理領(lǐng)域，能夠顯著改善定位精度。并且，其效果一般與延時時間成正比；隨著可容忍的延時時間的增加，定位精度也隨之提高。

3.2 動態(tài)定位試驗

同樣基于固定時延平滑方法在中科院奧運村園區(qū)的道路上進行了車載定位試驗，并實時計算和顯示定位結(jié)果。然后，分別采用固定延時L為1、3、10 s的卡爾曼延時平滑方法進行處理，并與實時卡爾曼濾波定位結(jié)果相比較，如圖3所示。

圖3 動態(tài)定位結(jié)果比較

從圖3可以看出，即使固定延時僅為1個歷元，平滑結(jié)果與實時濾波定位結(jié)果之間仍然具有一定的區(qū)別，但效果還不夠明顯。而當延時時間增加至10個歷元時，即可明顯看出兩者之間的差別，從圖3中所標識的1、2、3、4號等位置觀察得尤為明顯。其中，固定時延平滑結(jié)果要比實時濾波定位結(jié)果更平滑，與實際行駛道路更為吻合，抗遮擋及多徑環(huán)境的性能也更加優(yōu)秀。

4 結(jié) 論

在導(dǎo)航數(shù)據(jù)處理中，需要追求定位結(jié)果跟隨性與平滑性之間的平衡。即在使得位置結(jié)果盡量平滑、準確和美觀的同時，又不損失用戶真實運動軌跡的細節(jié)特征，不造成失真?？柭鼮V波延時平滑方法可以很好地解決這個問題，它具體包括固定時刻平滑與固定時延平滑方法。與人為根據(jù)經(jīng)驗和測試結(jié)果去調(diào)整濾波器參數(shù)不同，延時平滑方法同樣具有狀態(tài)更新和時間更新兩類系統(tǒng)和方程，相當于再逆向進行了一次卡爾曼濾波過程，具有嚴謹?shù)臄?shù)學(xué)性。

具體而言，這種卡爾曼濾波延時平滑定位方法具有以下幾個特點：

(1) 由卡爾曼濾波理論出發(fā)推導(dǎo)得到，其原理和形式均與經(jīng)典卡爾曼濾波方法保持一致，數(shù)學(xué)上嚴謹、規(guī)范。

(2) 與人為調(diào)整和修改參數(shù)不同，通過向前逆向預(yù)測和更新來對先驗歷元定位進行平滑，能夠在進一步平滑結(jié)果、減小隨機誤差的同時，不造成平滑過度和失真。

(3) 在計算量與耗時上，由于延時平滑方法遞推公式基于狀態(tài)一步轉(zhuǎn)移，因此可以很快向前計算，不會造成明顯計算負荷。

(4) 模型時延長度可根據(jù)實際應(yīng)用情況進行適應(yīng)與更改。

通過不同情況下的GPS實測試驗，驗證、分析了卡爾曼濾波延時平滑定位方法的正確性和可行性。它不僅能夠在靜止定位或數(shù)據(jù)后處理時大幅提高定位精度、縮短工作時間，還能在行人、車載等實時動態(tài)定位領(lǐng)域根據(jù)實際情況調(diào)整延時長度，更好地獲得定位結(jié)果跟隨與平滑之間的平衡，并同時具有更強的抗多徑與遮擋的能力。該方法原理嚴謹、易于實現(xiàn)，具有良好的實用效果與價值。

[1] KALMAN R.A New Approach to Linear Filtering and Prediction Problems[J].Transactions of ASME—Journal of Basic Engineering，1960(82)：34-45.

[2] 謝鋼.GPS原理與接收機設(shè)計[M].北京：電子工業(yè)出版社，2009.

[3] SORENSON W.Least-squares Estimation：from Gauss to Kalman[J].IEEE Spectrum，1970,7(7)：63-68.

[4] TED B，RAMA C.Estimation of Object Motion Parameters from Noisy Images[J].IEEE Transactions on Pattern Analysis and Machine Intelligence，1986，8 (1)：90-99.

[5] WELCH G，BISHOP G.An Introduction to the Kalman Filter[M].Ashville：University of North Carolina，2004.

[6] FARRELL J，BARTH M.The Global Positioning System and Inertial Navigation[M].New York：McGraw-Hill，1999.

[7] JULIER S，UHLMANN J.A New Extension of the Kalman Filter to Nonlinear Systems[C] ∥Proceedings of AeroSense：The 11th International Symposium on Aerospace/Defense Sensing，Simulation and Controls.Orlando，F(xiàn)L:[s.n.]，1997：192-193.

[8] GREWAL M，ANDREWS A.Kalman Filtering: Theory and Practice Using MATLAB[M]. 2nd ed. New York：John Wiley & Sons，Inc,2001.

[9] 季海福.基于SIGSO同行衛(wèi)星的導(dǎo)航星座原理與算法研究[M].北京：中國科學(xué)院國家天文臺，2013.

[10] CATTIVELLI F，SAYED A.Diffusion Strategies for Distributed Kalman Filtering and Smoothing[J].IEEE Transactions on Automatic Control，2010，55(9)：2069-2084.

[11] SAYED H，KAILATH T.A State-space Approach to Adaptive RLS Filtering[J].IEEE Signal Processing Magazine，1994，11(3)：18-60.

[12] PARKINSON B，SPILKER J，AXELRAD P，et al.Global Positioning System：Theory and Applications[M].[S.l.]: American Institute of Aeronautics and Astronautics，1996.

[13] LEANDRO R，SANTOS M.Stochastic Models for GPS Positioning：An Empirical Approach[J].GPS World，2007.

[14] HIDE C，MOORE T，HILL C，et al.Low Cost，High Accuracy Positioning in Urban Environments[J].Journal of Navigation，2006, 59(3)：365-379.

[15] KAILATH T，SAYED H，HASSIBI B.Linear Estimation[M].New Jersey：Prentice Hall，2000.

[16] KELLY C，ANDERSON B.On the Stability of Fixed-lag Smoothing Algorithms[J].Journal of the Franklin Institute，1971，291(4)：271-281.

[17] CATTIVELLI F，LOPES C，SAYED A.Diffusion Strategies for Distributed Kalman Filtering：Formulation and Performance Analysis[C]∥ Proceedings of Cognitive Information. Santorini：[s.n.],2008.

StudyonKalmanFilteringTime-lagSmoothingMethodinSatellitePositioning

LIU Cheng1，GUO Shuren1，LI Fang2

(1. Beijing Institute of Tracking and Telecommunication Technology,Beijing 100094,China; 2. National Astronomical Observatories,Chinese Academy of Sciences,Beijing 100190,China)

In order to improve the accuracy and performance of satellite navigation positioning，a method of Kalman filtering time-lag smoothing was introduced and discussed，and a Kalman filtering smoothing positioning model and its algorithm were built in this paper.Similar to classical Kalman filteing，the algorithm also has two kinds of equations—status system and measuring system，which could help to predict and update backward on the basis of follow-up epoch measured values and positioning results，thereby the overall positioning process can be smoothed again so that we can obtain the balance between positioning result tracking and smoothing performance.The paper analyzed and verified validity and feasibility of the method on the basis of GPS field experiment in various circumstances.Experiment results indicate that Kalman filtering time-lag smoothing positioning method can be adopted in various static and dynamic state navigation application field，which could make position results more smooth and accurate.The method has better anti-multipath and anti-interference performance, and it could also effectively improve positioning accuracy.

GPS; satellite navigation and positioning; Kalman filtering; fixed-point smoother; fixed-lag smoother; vehicle navigation

劉成，郭樹人，李芳.衛(wèi)星導(dǎo)航定位卡爾曼濾波時延平滑方法研究[J].測繪通報，2017(11)：6-10.

10.13474/j.cnki.11-2246.2017.0338.

P228

A

0494-0911(2017)11-0006-05

2017-02-22

國家自然科學(xué)基金(61601009)

劉 成(1987—)，男，博士，助理研究員，主要從事衛(wèi)星導(dǎo)航與空間技術(shù)研究工作。E-mail:liucheng@beidou.gov.cn