基于容積卡爾曼濾波的衛(wèi)星導(dǎo)航定位解算方法*

張杰，李婧華，胡超

(中國(guó)科學(xué)院國(guó)家天文臺(tái)， 北京 100101)(2019年10月15日收稿； 2019年12月16日收修改稿)

在衛(wèi)星導(dǎo)航定位解算中，通常釆用最小二乘法(least square method, LSM)[1-2]，其思想是在含有誤差與噪聲的各個(gè)測(cè)量值之間尋求一個(gè)最優(yōu)值，使得所有測(cè)量值的殘差平方和最小。因?yàn)橛啥ㄎ环匠探馑愠龅淖鴺?biāo)值與偽距測(cè)量值是非線性關(guān)系，為便于計(jì)算，使用最小二乘法之前需要對(duì)定位方程組進(jìn)行線性化展開，舍去高階項(xiàng)，其代價(jià)是會(huì)造成截?cái)嗾`差；同時(shí)最小二乘法只使用當(dāng)前歷元的偽距測(cè)量值，定位結(jié)果受測(cè)量噪聲影響較大。從一系列偽距測(cè)量值中確定位置的問(wèn)題可以表示為一個(gè)濾波問(wèn)題，因此可以借鑒卡爾曼濾波的思想[3]，通過(guò)用最新的測(cè)量值更新過(guò)去位置的估計(jì)值遞歸地估計(jì)當(dāng)前位置，可以獲得更加穩(wěn)定的位置估計(jì)結(jié)果。但經(jīng)典卡爾曼濾波通常只適用于線性系統(tǒng)，為解決非線性系統(tǒng)的濾波問(wèn)題，可以對(duì)非線性系統(tǒng)進(jìn)行線性化，即擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)[3]。但EKF也是通過(guò)舍去高階項(xiàng)對(duì)非線性系統(tǒng)進(jìn)行近似，同樣會(huì)引入高階項(xiàng)截?cái)嗾`差，只能達(dá)到一階精度。

容積卡爾曼濾波(cubature Kalman filter, CKF)[4]是Arasaratnam I和Haykin S在2009年提出的基于貝葉斯估計(jì)和容積變換的一種非線性濾波方法。它將高斯域非線性濾波問(wèn)題歸結(jié)為求解非線性函數(shù)和高斯概率密度乘積的數(shù)值積分[4-6]，計(jì)算出一組具有相同權(quán)重的點(diǎn)，并直接通過(guò)非線性系統(tǒng)方程對(duì)系統(tǒng)狀態(tài)和誤差進(jìn)行遞推估計(jì)，其精度可以達(dá)到三階以上[7]，在非線性濾波領(lǐng)域得到越來(lái)越多的應(yīng)用[5-12]。

1 容積卡爾曼濾波

1.1 貝葉斯估計(jì)

在實(shí)際應(yīng)用中，不論是線性系統(tǒng)還是非線性系統(tǒng)，其狀態(tài)估計(jì)問(wèn)題都可以用貝葉斯估計(jì)來(lái)描述。貝葉斯估計(jì)[6]的核心思想是：利用已知系統(tǒng)初始狀態(tài)概率密度和系統(tǒng)測(cè)量值，以遞推的方式求得狀態(tài)后驗(yàn)概率密度函數(shù)。狀態(tài)后驗(yàn)概率密度函數(shù)可以描述非線性系統(tǒng)的統(tǒng)計(jì)特性，求得后驗(yàn)概率密度函數(shù)就意味著得到了系統(tǒng)的最優(yōu)估計(jì)。遞推估計(jì)的過(guò)程可以看作是濾波的過(guò)程，主要目的是將系統(tǒng)的狀態(tài)通過(guò)包含噪聲的觀測(cè)量估計(jì)出來(lái)。對(duì)于線性系統(tǒng)而言，最優(yōu)濾波估計(jì)就是經(jīng)典卡爾曼濾波；但對(duì)于非線性系統(tǒng), 由于需要進(jìn)行無(wú)窮維積分運(yùn)算[6]，使得最優(yōu)估計(jì)無(wú)法實(shí)現(xiàn)。為此人們提出大量次優(yōu)的近似非線性濾波方法，如擴(kuò)展卡爾曼濾波、無(wú)跡卡爾曼濾波和容積卡爾曼濾波等。

在實(shí)際中高斯分布是最常見(jiàn)且應(yīng)用最廣泛的分布，因此本文重點(diǎn)針對(duì)高斯域的估計(jì)問(wèn)題。高斯域非線性離散系統(tǒng)模型[4-6,12]如下：

xk=f(xk-1)+wk-1，

(1)

zk=h(xk)+vk-1，

(2)

式中：xk為系統(tǒng)狀態(tài)向量；zk為測(cè)量向量；f為系統(tǒng)非線性狀態(tài)轉(zhuǎn)移函數(shù)；h為測(cè)量函數(shù)；wk為系統(tǒng)狀態(tài)轉(zhuǎn)移噪聲；vk為測(cè)量噪聲。wk和vk均為高斯白噪聲且不相關(guān)。式(1)和式(2)分別表示系統(tǒng)包含的2個(gè)隨機(jī)過(guò)程：狀態(tài)轉(zhuǎn)移和測(cè)量。系統(tǒng)狀態(tài)估計(jì)的目的是利用含有噪聲的測(cè)量信息估計(jì)系統(tǒng)狀態(tài)xk，即從式(2)描述的測(cè)量方程結(jié)合式(1)狀態(tài)轉(zhuǎn)移方程遞推估計(jì)xk。

將系統(tǒng)狀態(tài)轉(zhuǎn)移模型和測(cè)量模型概率化，則對(duì)高斯域非線性離散系統(tǒng)模型式(1)和式(2)的狀態(tài)估計(jì)[4-5,13]一般形式如下：

(3)

式中：Pk|k為狀態(tài)轉(zhuǎn)移誤差協(xié)方差矩陣；Pk|k-1為一步預(yù)測(cè)誤差協(xié)方差矩陣；Lk為卡爾曼增益；Pzz為測(cè)量自相關(guān)協(xié)方差矩陣；Pxz為狀態(tài)測(cè)量互相關(guān)協(xié)方差矩陣。上述變量具體計(jì)算可歸結(jié)為求解形如“非線性函數(shù)×高斯概率密度”的函數(shù)的積分問(wèn)題[4-5,13]，如下所示

(4)

式中:x為積分向量；f(x)為非線性函數(shù)；Rn為積分區(qū)域，即n維實(shí)向量空間。一般情況下，式(4)所示積分的解析值無(wú)法得到，需用近似方法獲得其解析值的近似解。采用不同的數(shù)值積分近似算法就可以推導(dǎo)出高斯域各種非線性近似濾波算法。

1.2 球面-徑向容積準(zhǔn)則

CKF是根據(jù)貝葉斯估計(jì)理論及“球面-徑向”容積(spherical-radial cubature)準(zhǔn)則得出的濾波算法，使用一組容積點(diǎn)逼近具有附加高斯噪聲的非線性系統(tǒng)的狀態(tài)均值和誤差協(xié)方差，可以有效地解決非線性系統(tǒng)的狀態(tài)估計(jì)問(wèn)題[4-6]。

利用“球面-徑向”容積準(zhǔn)則，式(4)可近似計(jì)算[4-6]為

(5)

其中，n為系統(tǒng)狀態(tài)向量維數(shù)。

(6)

其中，[1]i表示矩陣[1]的第i列。

對(duì)標(biāo)準(zhǔn)高斯分布函數(shù)f(x)求解積分IN(f)

(7)

其中，N(x;0,I)為f(x)的概率密度函數(shù)，表示f(x)滿足均值為0、方差為單位陣I的高斯分布。因此式(7)可寫為

(8)

結(jié)合式(4)、式(5)，式(8)可寫為

(9)

其中

(10)

式中：ξi為容積點(diǎn)；ωi為權(quán)值；m為容積點(diǎn)數(shù)。

對(duì)于均值為μ、方差為∑的一般高斯分布函數(shù)f(x)求解積分

(11)

由以上推導(dǎo)可知，根據(jù)“球面—徑向”容積準(zhǔn)則，共選取2n個(gè)權(quán)值相等的容積點(diǎn)即可計(jì)算形如式(4)的積分。

1.3 容積卡爾曼濾波原理

使用容積點(diǎn)集計(jì)算方程(3)中的各個(gè)參量，即可得出CKF遞推公式，在每個(gè)濾波周期內(nèi)進(jìn)行時(shí)間更新和測(cè)量更新[4-6,10]。

1)時(shí)間更新

假設(shè)k-1時(shí)刻后驗(yàn)概率密度函數(shù)已知，通過(guò)Cholesky分解誤差協(xié)方差矩陣Pk-1|k-1

(12)

計(jì)算容積點(diǎn)(i=1,2,…m,m=2n)

(13)

其中ξi見(jiàn)式(10)。

通過(guò)狀態(tài)轉(zhuǎn)移方程計(jì)算傳播容積點(diǎn)

(14)

根據(jù)式(11)，估計(jì)k時(shí)刻的狀態(tài)預(yù)測(cè)值

(15)

估計(jì)k時(shí)刻的狀態(tài)誤差協(xié)方差預(yù)測(cè)值

(16)

其中Q為狀態(tài)轉(zhuǎn)移噪聲協(xié)方差矩陣。

2)測(cè)量更新

通過(guò)Cholesky分解Pk|k-1

(17)

計(jì)算容積點(diǎn)(i=1,2,…m,m=2n)

(18)

通過(guò)測(cè)量函數(shù)h計(jì)算傳播容積點(diǎn)

Zi,k|k-1=h(Xi,k|k-1).

(19)

估計(jì)k時(shí)刻的測(cè)量預(yù)測(cè)值

(20)

估計(jì)測(cè)量自相關(guān)協(xié)方差矩陣

(21)

其中Rk為測(cè)量噪聲協(xié)方差。

估計(jì)狀態(tài)測(cè)量互相關(guān)協(xié)方差矩陣

(22)

估計(jì)卡爾曼增益

(23)

k時(shí)刻狀態(tài)估計(jì)值

(24)

k時(shí)刻狀態(tài)誤差協(xié)方差估計(jì)值

(25)

2 基于容積卡爾曼濾波的定位解算

在衛(wèi)星導(dǎo)航定位中，需要對(duì)接收機(jī)的5個(gè)狀態(tài)進(jìn)行估計(jì)：接收機(jī)三維位置坐標(biāo)(xu,yu,zu)、接收機(jī)時(shí)鐘偏差Δtu以及接收機(jī)時(shí)鐘頻率漂移Δfu。按照時(shí)間更新和測(cè)量更新2個(gè)環(huán)節(jié)進(jìn)行估計(jì)。

1)時(shí)間更新

根據(jù)式(10)，容積點(diǎn)ξi及權(quán)值ωi分別為

(26)

k時(shí)刻接收機(jī)位置坐標(biāo)利用遞推方式表示為

(27)

則狀態(tài)轉(zhuǎn)移矩陣為

(28)

式中T為觀測(cè)歷元時(shí)間間隔。

將式(26)、式(28)代入式(13)、式(14)，即可計(jì)算時(shí)間更新環(huán)節(jié)傳播容積點(diǎn)。

狀態(tài)轉(zhuǎn)移噪聲協(xié)方差矩陣[2,14]為

(29)

式中：SP為三維位置噪聲功率譜密度，Sf為接收機(jī)時(shí)鐘頻率的噪聲功率譜密度。

2)測(cè)量更新

為確定接收機(jī)的位置坐標(biāo)(xu,yu,zu)，接收機(jī)需測(cè)量出自身與3顆衛(wèi)星的距離ρ1、ρ2和ρ3。但由于接收機(jī)時(shí)鐘與衛(wèi)星時(shí)鐘存在鐘差Δtu(假設(shè)不同衛(wèi)星之間的鐘差為0)，必須將鐘差Δtu也作為1個(gè)未知數(shù)進(jìn)行求解。因此，至少需要再增加1顆衛(wèi)星參與測(cè)量，相應(yīng)增加1個(gè)方程。實(shí)際中可視衛(wèi)星數(shù)量往往多于4個(gè)，構(gòu)成定位方程組

(30)

式中：(xn,yn,zn)為衛(wèi)星的三維坐標(biāo)，N為可視衛(wèi)星總數(shù)，c為光速。

式(30)即為式(19)中的測(cè)量函數(shù)h，根據(jù)式(19)、式(30)，即可計(jì)算測(cè)量更新環(huán)節(jié)傳播容積點(diǎn)。

(31)

式中，θn為第n顆衛(wèi)星相對(duì)于接收機(jī)的高度角。根據(jù)以上參數(shù)，即可通過(guò)式(12)～式(25)遞推估計(jì)接收機(jī)的位置坐標(biāo)。

3 實(shí)測(cè)數(shù)據(jù)分析

圖1 CKF算法估計(jì)的三維坐標(biāo)Fig.1 3-D coordinate estimated by CKF algorithm

對(duì)IGS監(jiān)測(cè)接收機(jī)連續(xù)24 h的觀測(cè)值,利用CKF估計(jì)的結(jié)果與實(shí)際坐標(biāo)偏差如圖2所示。

圖2 CKF算法估計(jì)結(jié)果與實(shí)際坐標(biāo)偏差Fig.2 Deviation of CKF algorithm estimation result from the actual coordinates

連續(xù)24 h可視的GPS衛(wèi)星幾何精度因子(geometric dilution precision, GDOP)如圖3所示。

圖3 24 h GDOP值Fig.3 GDOP for 24 h

基于同樣的IGS監(jiān)測(cè)站數(shù)據(jù)，分別用LSM、EKF和CKF 3種算法估計(jì)接收機(jī)三維坐標(biāo)，3種算法定位偏差局部放大對(duì)比如圖4所示。

圖4 3種算法估計(jì)的定位偏差Fig.4 Positioning bias estimated by three algorithms

由圖4可見(jiàn)，EKF算法和CKF算法估計(jì)結(jié)果更加平緩，定位精度更高。由于初始坐標(biāo)未知，接收機(jī)隨機(jī)設(shè)置的初始坐標(biāo)不準(zhǔn)，EKF算法和CKF算法都需要經(jīng)過(guò)震蕩收斂，如圖5所示。

圖5 EKF與CKF估計(jì)收斂速度對(duì)比Fig.5 Comparison of convergence rates of EKF and CKF estimates

由圖5可見(jiàn)，隨著濾波次數(shù)的增加，EKF和CKF坐標(biāo)估計(jì)結(jié)果都可迅速收斂，而CKF比EKF具有更快的收斂速度。這是因?yàn)榕cEKF算法相比，由于基于貝葉斯估計(jì)和容積變換的CKF算法更適用于非線性的定位解算方程組，每次估計(jì)結(jié)果誤差更小，因此定位解算結(jié)果可以更快地收斂。

在相同的場(chǎng)景下，對(duì)24 h的定位解算結(jié)果進(jìn)行對(duì)比，3種算法估計(jì)位置坐標(biāo)標(biāo)準(zhǔn)差結(jié)果如表1所示。

表1 3種算法估計(jì)誤差對(duì)比Table 1 Comparison of estimation errors of three algorithms

可見(jiàn)，在定位解算的應(yīng)用中，與傳統(tǒng)的LSM算法相比，EKF算法和CKF算法可以獲得更高的精度，CKF算法又優(yōu)于EKF算法。

用Matlab對(duì)3種算法進(jìn)行實(shí)現(xiàn)并進(jìn)行測(cè)試，對(duì)全天2 880個(gè)觀測(cè)歷元的觀測(cè)數(shù)據(jù)進(jìn)行定位解算，3種算法的運(yùn)行時(shí)間如表2所示。

由表2可見(jiàn)，EKF算法的運(yùn)算開銷最少，而CKF算法由于涉及較多的矩陣變量，需要經(jīng)過(guò)更多步驟的矩陣運(yùn)算，因此運(yùn)算量開銷較大。綜合性能與代價(jià)比較，在靜態(tài)衛(wèi)星導(dǎo)航定位解算中其經(jīng)濟(jì)性不如EKF算法。

表2 3種算法運(yùn)行時(shí)間對(duì)比Table 2 Comparison of running time of three algorithms

4 結(jié)論

本文將基于貝葉斯估計(jì)的非線性濾波方法CKF應(yīng)用于導(dǎo)航定位解算。介紹CKF濾波算法的基本原理和遞推過(guò)程，建立衛(wèi)星導(dǎo)航定位解算的狀態(tài)轉(zhuǎn)移模型和測(cè)量誤差模型。利用IGS實(shí)際觀測(cè)數(shù)據(jù)進(jìn)行驗(yàn)證，并與傳統(tǒng)的LSM和EKF進(jìn)行對(duì)比。結(jié)果顯示：相比于LSM和EKF 方法，CKF算法具有更高的導(dǎo)航定位解算精度，與EKF相比具有更好的收斂特性，為衛(wèi)星導(dǎo)航定位提供了一種新的濾波解算途徑。