基于自適應(yīng)容積卡爾曼濾波的矢量跟蹤算法

鄒曉軍, 廉保旺, 丹澤升

(西北工業(yè)大學(xué) 電子信息學(xué)院, 陜西 西安 710072)

隨著用戶對(duì)導(dǎo)航定位需求的日益提高，對(duì)GNSS(global navigation satellite system)接收機(jī)性能的要求越來(lái)越高，傳統(tǒng)的GNSS信號(hào)接收技術(shù)越來(lái)越難以滿足要求。矢量跟蹤技術(shù)作為下一代GNSS接收機(jī)的關(guān)鍵接收技術(shù)[1]之一，以其潛在的性能優(yōu)勢(shì)，近年來(lái)受到越來(lái)越多國(guó)內(nèi)外學(xué)者的關(guān)注。相對(duì)于傳統(tǒng)接收機(jī)中使用的標(biāo)量跟蹤技術(shù)，矢量跟蹤技術(shù)在沒有外部設(shè)備輔助的情況下，通過(guò)挖掘衛(wèi)星信號(hào)之間的相關(guān)信息，能夠有效提高跟蹤能力，具有跟蹤更加微弱的衛(wèi)星信號(hào)、橋接被瞬時(shí)遮擋的衛(wèi)星信號(hào)以及忍受更大的用戶動(dòng)態(tài)等優(yōu)勢(shì)[2-3]。

在當(dāng)前的矢量跟蹤環(huán)路中，通過(guò)鑒別器[4-6]獲得碼相位誤差和載波頻率誤差，然后以此為觀測(cè)量來(lái)求解用戶位置和速度。這種方式結(jié)構(gòu)簡(jiǎn)單，計(jì)算復(fù)雜度低，但是，由于噪聲的影響，容易進(jìn)入鑒別器的非線性區(qū)域，使得鑒別器的輸出存在較大誤差，尤其是在弱信號(hào)的情況下，這種現(xiàn)象更加明顯。為了解決這個(gè)問(wèn)題，可以對(duì)相關(guān)積分累加之后的I/Q支路數(shù)據(jù)進(jìn)行濾波處理，這樣既可以降低噪聲的影響，同時(shí)還可以規(guī)避鑒別器的非線性問(wèn)題。鑒于I/Q支路數(shù)據(jù)的非線性特征，需要采用可處理非線性數(shù)據(jù)的濾波算法。容積卡爾曼濾波算法[7](CKF)，是近年來(lái)新提出的一種非線性濾波算法，它基于三階球面-相徑容積規(guī)則，能精確到泰勒展開的三階精度，并具有嚴(yán)格的理論推導(dǎo)和證明。CKF避免了擴(kuò)展卡爾曼濾波算法(EKF)對(duì)非線性函數(shù)的線性化處理，所以濾波精度更高，較無(wú)跡卡爾曼濾波算法(UKF)減少了一個(gè)采樣點(diǎn)，且采樣點(diǎn)權(quán)值均為正，具有更好的數(shù)值穩(wěn)定性和計(jì)算效率[8]。但是，CKF算法存在一個(gè)問(wèn)題，它完全依賴于初始化配置，不能夠根據(jù)環(huán)境的變化自適應(yīng)地調(diào)整自身參數(shù)，這樣會(huì)出現(xiàn)參數(shù)的配置與實(shí)際應(yīng)用不匹配的情況，導(dǎo)致濾波器性能下降甚至發(fā)散。針對(duì)這一問(wèn)題，使用新息[9]協(xié)方差能夠?qū)崟r(shí)估計(jì)量測(cè)噪聲的協(xié)方差矩陣，自適應(yīng)地調(diào)整量測(cè)噪聲的大小，進(jìn)而提高CKF算法應(yīng)對(duì)環(huán)境變化的能力。

本文采用上述具有自適應(yīng)調(diào)節(jié)能力的CKF算法對(duì)I/Q支路數(shù)據(jù)進(jìn)行濾波處理，然后將濾波輸出的碼相位誤差和載波頻率誤差作為導(dǎo)航濾波器的觀測(cè)量。導(dǎo)航濾波器集中處理所有接收通道的數(shù)據(jù)，進(jìn)行用戶位置和速度的解算，并同時(shí)輸出各個(gè)接收通道的NCO(numerical controlling oscillator)控制參數(shù)，完成各個(gè)接收通道衛(wèi)星信號(hào)的跟蹤。最后，將本文算法與基于鑒別器方式的矢量跟蹤算法進(jìn)行了對(duì)比測(cè)試，驗(yàn)證了算法改進(jìn)后的有效性。

1 矢量跟蹤環(huán)路結(jié)構(gòu)

1.1 系統(tǒng)結(jié)構(gòu)模型

對(duì)于采用標(biāo)量跟蹤環(huán)路的GNSS接收機(jī)來(lái)說(shuō)，每顆衛(wèi)星信號(hào)的載波頻率和碼相位都由一個(gè)獨(dú)立的通道進(jìn)行跟蹤鎖定，通道之間相互沒有聯(lián)系。矢量跟蹤環(huán)路與之最明顯的區(qū)別就是各通道間的數(shù)據(jù)都匯聚到導(dǎo)航濾波器進(jìn)行集中處理，從而實(shí)現(xiàn)各個(gè)通道間的信息共享。這種結(jié)構(gòu)的好處是強(qiáng)信號(hào)能夠輔助弱信號(hào)的接收，實(shí)現(xiàn)微弱信號(hào)的跟蹤。矢量跟蹤環(huán)路的結(jié)構(gòu)如圖1所示。

圖1 基于CKF的矢量跟蹤結(jié)構(gòu)模型

圖1中，CKF代替碼相位鑒別器和載波頻率鑒別器，同時(shí)輸出碼相位誤差Δτi和載波頻率誤差Δfi,作為導(dǎo)航濾波器的輸入數(shù)據(jù)使用。i∈[1,N],表示第i個(gè)接收通道,N為衛(wèi)星的總個(gè)數(shù)。只有當(dāng)Δτi和Δfi足夠小的時(shí)候,導(dǎo)航濾波器才可能輸出準(zhǔn)確的位置和速度,以及各環(huán)路的控制量。所以說(shuō),Δτi和Δfi的誤差大小,直接影響著整個(gè)矢量跟蹤環(huán)路的性能。

1.2 信號(hào)模型

第i顆衛(wèi)星的信號(hào)經(jīng)接收機(jī)射頻前端處理后,其中頻信號(hào)的信號(hào)模型可以由(1)式表示

(1)

式中,AIF,i表示中頻信號(hào)的幅度;C(t-τi)表示偽碼;D(t-τi)為調(diào)制的數(shù)據(jù)碼;τi表示信號(hào)的傳播延時(shí),單位為s;fc為載波頻率,單位為Hz;fd,i為信號(hào)的多普勒頻移,單位為Hz;θi表示載波相位,單位為rad。

信號(hào)被捕獲后,接著進(jìn)入跟蹤階段。如圖1所示,接收信號(hào)首先與本地產(chǎn)生的2路相位相差90°的載波信號(hào)相乘,實(shí)現(xiàn)載波剝離,然后再分別與本地產(chǎn)生的3路偽碼數(shù)據(jù)相乘,實(shí)現(xiàn)偽碼的剝離,最后完成積分累加后輸出到CKF濾波器。經(jīng)過(guò)上述步驟的處理,第i個(gè)接收通道6路I/Q數(shù)據(jù)的數(shù)學(xué)表達(dá)式如下所示:

式中,Ai表示信號(hào)幅度;R(Δτi+δ)表示偽碼的自相關(guān)函數(shù);Δτi表示碼相位誤差;δ表示本地偽碼超前和滯后的間隔,一般設(shè)置為二分之一個(gè)碼片長(zhǎng)度;Tcoh表示積分累加的時(shí)間;Δfi表示載波頻率誤差;Δθi表示載波相位誤差。

2 基于自適應(yīng)容積卡爾曼濾波的矢量跟蹤算法

2.1 濾波器模型

一個(gè)衛(wèi)星信號(hào)對(duì)應(yīng)一個(gè)接收通道,對(duì)于任意一個(gè)接收通道,CKF的狀態(tài)量定義為:

(8)

鑒于載波頻率、載波相位、碼相位之間的關(guān)系,濾波器在k+1時(shí)刻的狀態(tài)方程可表示為:

(9)

式中,fcode表示偽碼頻率,Wk為過(guò)程噪聲向量,其協(xié)方差矩陣為Qk。

以6路I/Q數(shù)據(jù)作為觀測(cè)量,量測(cè)方程可表示為:

(10)

2.2 CKF算法

CKF算法是一種確定性采樣非線性算法,基于容積規(guī)則選取一組容積點(diǎn),容積點(diǎn)經(jīng)非線性函數(shù)傳遞后,來(lái)近似非線性高斯濾波中的高斯積分。

為了提高濾波的穩(wěn)定度,避免誤差協(xié)方差矩陣失去正定性,采用奇異值分解[10](SVD)代替原算法中的Cholesky分解。CKF算法包含時(shí)間更新和測(cè)量更新2個(gè)部分,計(jì)算步驟如下:

2.2.1 時(shí)間更新

1) 對(duì)k-1時(shí)刻的狀態(tài)誤差協(xié)方差矩陣Pk-1作SVD分解,有

(11)

式中,Sk-1為對(duì)角陣,Sk-1=diag{s1,k-1,s2,k-1,…sn,k-1},sn,k-1為奇異值,Uk-1和Vk-1的列向量為對(duì)應(yīng)的奇異向量。

2) 計(jì)算容積點(diǎn)

(12)

(13)

式中,i=1,2,…,m,m表示容積點(diǎn)個(gè)數(shù),等于狀態(tài)量維數(shù)n的2倍。文中,m=10。[1]i表示點(diǎn)集[1]中的第i列,[1]表示完整全對(duì)稱點(diǎn)集。

3) 計(jì)算經(jīng)狀態(tài)方程傳遞后的容積點(diǎn)

(14)

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

(15)

5)發(fā)計(jì)算誤差協(xié)方差陣的預(yù)測(cè)值

(16)

2.2.2 測(cè)量更新

1) 對(duì)矩陣Pk|k-1作SVD分解,有

(17)

2) 計(jì)算容積點(diǎn)

(18)

3) 計(jì)算經(jīng)量測(cè)方程傳遞后的容積點(diǎn)

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

(19)

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

(20)

5) 計(jì)算新息協(xié)方差陣:

(21)

6) 計(jì)算互相關(guān)協(xié)方差陣

(22)

7) 計(jì)算k時(shí)刻的濾波增益

(23)

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

(24)

式中,zk表示的是觀測(cè)量。

9) 計(jì)算k時(shí)刻的狀態(tài)誤差協(xié)方差矩陣

(25)

2.3 自適應(yīng)CKF算法

(21)式中的Rk是量測(cè)誤差的協(xié)方差矩陣,表示的是觀測(cè)量的量測(cè)噪聲,其初始值可表示為:

(26)

在實(shí)際應(yīng)用中,環(huán)境是不斷變化的,量測(cè)誤差也會(huì)隨之發(fā)生變化,如果Rk始終使用固定的初始值R0,那么CKF會(huì)因?yàn)椴荒軌蜻m應(yīng)環(huán)境的變化而降低性能甚至發(fā)散,所以,需要根據(jù)環(huán)境的變化動(dòng)態(tài)調(diào)整Rk的值?；谛孪⒌淖赃m應(yīng)卡爾曼濾波方法能夠?qū)崟r(shí)估計(jì)Rk,而且理論清晰,計(jì)算簡(jiǎn)單。該方法是基于線性的卡爾曼濾波算法推導(dǎo)而來(lái)的,所以不能夠直接套用其公式,本文針對(duì)CKF算法進(jìn)行了適配修改。

定義新息為

(27)

新息的理論協(xié)方差矩陣為:

(28)

利用一段新息樣本數(shù)據(jù)可對(duì)新息的協(xié)方差矩陣進(jìn)行近似估計(jì),有:

(29)

式中,L表示樣本數(shù)據(jù)的個(gè)數(shù)。

而在CKF算法[7]中,新息的協(xié)方差矩陣又可表示為:

(30)

所以,結(jié)合(29)式和(30)式,Rk的遞推估計(jì)值為

(31)

2.4 導(dǎo)航濾波器

導(dǎo)航濾波器作為整個(gè)矢量跟蹤環(huán)路的核心,基于EKF算法,為了方便建立濾波器模型,選取系統(tǒng)狀態(tài)的誤差信息作為EKF的狀態(tài)變量ΔX,定義如下:

(32)

對(duì)于低速運(yùn)動(dòng)物體,一般常使用Position Velocity(PV)模型[11],對(duì)應(yīng)的狀態(tài)方程如下所示:

ΔXk+1=FΔXk+Yk

(33)

(34)

式中,F為狀態(tài)轉(zhuǎn)移矩陣,T為相鄰狀態(tài)間的時(shí)間間隔,Yk為過(guò)程噪聲向量。

(35)

(36)

式中,fi,code為偽碼頻率,fi,carr為載波頻率,c為光速。

于是,量測(cè)方程可表示為:

(37)

(38)

3 實(shí)驗(yàn)及結(jié)果

3.1 實(shí)驗(yàn)方法

本文主要針對(duì)GPS L1信號(hào)進(jìn)行測(cè)試,實(shí)驗(yàn)方法是先采用中頻采集器將數(shù)據(jù)收集起來(lái),然后使用基于Matlab平臺(tái)的軟件接收機(jī)對(duì)數(shù)據(jù)進(jìn)行處理。中頻采集器的型號(hào)為NordNav R30,中頻為4.02 MHz,采樣速率為20 MHz。設(shè)置相關(guān)器的積分累加時(shí)間為10 ms。信號(hào)模擬器型號(hào)為HWA-RNSS-7300,使用該信號(hào)模擬器作為信號(hào)源,生成高低2種功率的GPS數(shù)據(jù),驗(yàn)證算法在這2種情況下的靜態(tài)跟蹤誤差。

同時(shí),采用相同的數(shù)據(jù),將本文算法與基于鑒別器的矢量跟蹤算法進(jìn)行對(duì)比測(cè)試,以驗(yàn)證二者跟蹤性能的差異。其中,碼相位鑒別器采用非相干超前減滯后幅值法[12],載波頻率鑒別器采用叉積鑒頻[12]的方法。

3.2 實(shí)驗(yàn)結(jié)果

1) 高功率信號(hào)測(cè)試

使用信號(hào)模擬器輸出載噪比C/N0為45 dB/Hz的數(shù)據(jù),該信號(hào)強(qiáng)度模擬衛(wèi)星信號(hào)正常到達(dá)地面時(shí)的情況。接收機(jī)共有8個(gè)接收通道,每個(gè)通道的信號(hào)處理方式相同,不一一贅述,只以通道1的接收情況為例進(jìn)行說(shuō)明。

圖2和圖3分別描述的是2種算法輸出的碼相位誤差和載波頻率誤差,從圖中可以看出,自適應(yīng)CKF(ACKF)算法輸出的碼相位誤差集中在0.02個(gè)碼片以內(nèi),載頻頻率誤差集中在2 Hz以內(nèi),相對(duì)于鑒別器的輸出,幅度明顯減小,說(shuō)明了跟蹤精度有所提高。

圖2 載噪比為45 dB/Hz時(shí)的碼相位誤差圖3 載噪比為45 dB/Hz時(shí)的載波頻率誤差

圖4 載噪比為45 dB/Hz時(shí)的位置誤差曲線圖 圖5 載噪比為45 dB/Hz時(shí)的速度誤差曲線圖

表1給出了2種算法輸出的各項(xiàng)誤差數(shù)據(jù)的均方根誤差(RMSE),它衡量的是各項(xiàng)誤差的波動(dòng)情況,客觀地反映了2種算法受噪聲影響的程度。

表1 載噪比為45 dB/Hz時(shí)各項(xiàng)誤差的RMSE

2) 低功率信號(hào)測(cè)試

使用信號(hào)模擬器輸出載噪比C/N0為30 dB/Hz的數(shù)據(jù),該信號(hào)強(qiáng)度相對(duì)于正常衛(wèi)星信號(hào)的強(qiáng)度,有較大程度的衰減,模擬衛(wèi)星信號(hào)經(jīng)過(guò)遮擋、多徑等因素干擾后的情況。

如圖6和圖7所示,由于信號(hào)功率降低,受噪聲影響加劇,2種算法輸出的碼相位誤差和載波頻率誤差都明顯增大,ACKF算法的碼相位誤差的集中在0.06個(gè)chip以內(nèi),載波頻率誤差增加到7 Hz左右。但是,ACKF算法仍然能夠保持對(duì)信號(hào)的跟蹤,而且誤差比鑒別器小。說(shuō)明ACKF算法能夠根據(jù)信號(hào)功率的變化,自動(dòng)地調(diào)整量測(cè)噪聲的協(xié)方差陣Rk,維持濾波器對(duì)輸入信號(hào)的穩(wěn)定處理。

圖6 載噪比為30 dB/Hz時(shí)的碼相位誤差 圖7 載噪比為30 dB/Hz時(shí)的載波頻率誤差

導(dǎo)航濾波器輸出的位置誤差和速度誤差分別如圖8和圖9所示,衛(wèi)星信號(hào)的功率降低后,導(dǎo)航解的精度也有所降低,但是,ACKF算法的精度仍然優(yōu)于鑒別器。

圖8 載噪比為30 dB/Hz時(shí)的位置誤差曲線圖 圖9 載噪比為30 dB/Hz時(shí)的速度誤差曲線圖

通過(guò)對(duì)比表1和表2的數(shù)據(jù)還可以發(fā)現(xiàn),雖然信號(hào)功率降低后,兩者算法各項(xiàng)誤差的RMSE都有所增加,但是,增加幅度并不相同,ACKF相對(duì)要小一些。最為明顯的是載波頻率誤差,ACKF算法的載波頻率誤差只增加了約1.6 Hz,而鑒別器卻增加了9.2 Hz,增加的比例分別是2.8倍和3.3倍,說(shuō)明了載波頻率鑒別器在弱信號(hào)時(shí)的誤差較大,ACKF算法更適合在弱信號(hào)環(huán)境中應(yīng)用。

表2 載噪比為30 dB/Hz時(shí)各項(xiàng)誤差的RMSE

4 結(jié) 論

本文不僅根據(jù)I/Q支路數(shù)據(jù)的特征建立了合適的濾波器模型，而且采用數(shù)值穩(wěn)定性更好的SVD分解替換原CKF算法中的Cholesky分解，提升了數(shù)據(jù)處理的穩(wěn)定度。利用新息協(xié)方差實(shí)時(shí)修正量測(cè)噪聲的協(xié)方差矩陣，自適應(yīng)地估計(jì)量測(cè)噪聲的方差，使得算法對(duì)環(huán)境的變化具有魯棒性。將本文算法與基于鑒別器的矢量跟蹤算法進(jìn)行了對(duì)比測(cè)試，測(cè)試結(jié)果表明，無(wú)論是高功率信號(hào)還是低功率信號(hào)，算法改進(jìn)后輸出結(jié)果的RMSE都更小，尤其是在低載噪比的情況下，改進(jìn)效果更加明顯，說(shuō)明該算法更加有利于弱信號(hào)的接收，具有一定的應(yīng)用價(jià)值。