摘 要： 為了解決傳統(tǒng)Kalman濾波在處理非線性系統(tǒng)時(shí)的局限性，以及擴(kuò)展Kalman濾波(EKF)在處理強(qiáng)非線性系統(tǒng)時(shí)發(fā)散性和精度較差的問(wèn)題，結(jié)合動(dòng)態(tài)導(dǎo)航系統(tǒng)中的目標(biāo)跟蹤定位問(wèn)題，在不敏Kalman濾波(UKF)算法的基礎(chǔ)上，提出了一種基于平方根UKF的動(dòng)態(tài)跟蹤定位算法，在遞推運(yùn)算過(guò)程中采用協(xié)方差矩陣的平方根代替?zhèn)鹘y(tǒng)算法計(jì)算過(guò)程中的協(xié)方差矩陣。MATLAB仿真結(jié)果表明，平方根UKF算法的精度比EKF提升了54.7%，比UKF提升了14.8%。所提出的算法解決了Kalman處理非線性系統(tǒng)的局限性以及傳統(tǒng)EKF和UKF算法精度不高的問(wèn)題，為偽衛(wèi)星系統(tǒng)的高精度定位研究提供了有力支撐。

　　關(guān)鍵詞： 算法理論;動(dòng)態(tài)定位跟蹤;偽衛(wèi)星;平方根濾波;卡爾曼濾波算法;非線性

　　為了解決傳統(tǒng)衛(wèi)星星座在某些特定區(qū)域(如峽谷、隧道、密林等)受地形遮擋[1]定位授時(shí)精度難以達(dá)到要求以及信號(hào)功率相對(duì)較弱容易受到復(fù)雜環(huán)境影響干擾欺騙的問(wèn)題，作為一種區(qū)域增強(qiáng)系統(tǒng)，偽衛(wèi)星能有效彌補(bǔ)衛(wèi)星信號(hào)受遮擋的問(wèn)題，并提供較強(qiáng)的導(dǎo)航信號(hào)。近幾年，空基偽衛(wèi)星成為研究熱點(diǎn)。相比于陸基偽衛(wèi)星，空基偽衛(wèi)星雖有著更加靈活、應(yīng)用場(chǎng)合更為多樣、覆蓋區(qū)域更廣的優(yōu)勢(shì)，但也面臨著諸多問(wèn)題[2-4]。

　　空基偽衛(wèi)星這一動(dòng)態(tài)平臺(tái)的自身位置對(duì)整個(gè)系統(tǒng)的定位精度有著重要影響。針對(duì)動(dòng)態(tài)系統(tǒng)的定位跟蹤，常采用Kalman濾波算法及擴(kuò)展Kalman濾波算法。一般而言，Kalman濾波算法或者在其基礎(chǔ)上演變的相關(guān)算法，在處理線性或者弱非線性系統(tǒng)方面效果較為理想，但對(duì)于較為復(fù)雜的非線性系統(tǒng)，其處理結(jié)果對(duì)于較高精度的動(dòng)態(tài)跟蹤定位來(lái)說(shuō)很難達(dá)到要求[5-7]。針對(duì)上述問(wèn)題，本文提出一種基于平方根UKF的動(dòng)態(tài)目標(biāo)跟蹤定位算法，并與常見(jiàn)的EKF以及UKF算法進(jìn)行仿真分析對(duì)比。

　　1 傳統(tǒng)跟蹤定位算法

　　空基偽衛(wèi)星動(dòng)態(tài)平臺(tái)在運(yùn)動(dòng)過(guò)程中，偽衛(wèi)星飛行狀態(tài)之間、狀態(tài)與觀測(cè)量之間存在嚴(yán)重的非線性關(guān)系，因此需要采用非線性濾波技術(shù)得到其狀態(tài)變量的最優(yōu)估計(jì)值，將濾波算法運(yùn)用到動(dòng)目標(biāo)跟蹤定位中，通過(guò)引入濾波方法對(duì)動(dòng)目標(biāo)的行進(jìn)路線進(jìn)行預(yù)測(cè)與估計(jì)。常見(jiàn)的用于目標(biāo)跟蹤定位算法的濾波方法有Kalman濾波算法、EKF算法和UKF算法。

　　1.1 Kalman濾波算法

　　Kalman濾波是根據(jù)一定的濾波準(zhǔn)則，采用最優(yōu)化估計(jì)方法對(duì)觀測(cè)系統(tǒng)的狀態(tài)進(jìn)行估計(jì)與預(yù)測(cè)的一種最優(yōu)估計(jì)或最優(yōu)濾波。其思路是在估計(jì)過(guò)程中，用上一次的最優(yōu)狀態(tài)估計(jì)和最優(yōu)誤差估計(jì)計(jì)算這一次的先驗(yàn)狀態(tài)估計(jì)和先驗(yàn)誤差估計(jì)，再用得到的先驗(yàn)誤差估計(jì)以及量測(cè)噪聲得到Kalman增益，結(jié)合前面所得到的先驗(yàn)估計(jì)和Kalman增益得到本次的最優(yōu)估計(jì)，不斷重復(fù)這個(gè)過(guò)程，用本次的最優(yōu)估計(jì)來(lái)估算下一次的先驗(yàn)估計(jì)[8-9]。

　　假設(shè)動(dòng)態(tài)系統(tǒng)的狀態(tài)空間模型為

　　X(t+1)=ΦX(t)+ΓW(t)，

　　Y(t)=HX(t)+V(t)，

　　式中：X(t)為系統(tǒng)在時(shí)刻t的狀態(tài);Y(t)為對(duì)狀態(tài)的觀測(cè)值;W(t)為系統(tǒng)噪聲，其方差矩陣為Q;V(t)為觀測(cè)噪聲，其方差陣為R;Φ為狀態(tài)轉(zhuǎn)移矩陣;H為觀測(cè)矩陣;Γ為系統(tǒng)噪聲驅(qū)動(dòng)矩陣。Kalman濾波的計(jì)算流程如下。

　　計(jì)算狀態(tài)一步預(yù)測(cè)：

　　[AKX^](t+1|t)=Φ[AKX^](t|t);

　　計(jì)算新息：

　　ε(t+1)=Y(t+1)-H[AKX^](t+1|t);

　　計(jì)算狀態(tài)估計(jì)值：

　　[AKX^](k+1|k+1)=[AKX^](k+1|k)+K(k+1)ε(k+1);

　　計(jì)算Kalman濾波增益：

　　K(t+1)=P(t+1|t)HT[HP(t+1|t)HT+R]-1; (1)

　　計(jì)算一步預(yù)測(cè)均方誤差：

　　P(t+1|t)=ΦP(t|t)ΦT+ΓQΓT; (2)

　　計(jì)算一步預(yù)測(cè)估計(jì)均方誤差：

　　P(t+1|t+1)=[In-K(t+1)H]P(t+1|t)。 (3)

　　為了更形象地說(shuō)明Kalman濾波原理，圖1給出了Kalman濾波的系統(tǒng)模型框圖。

　　1.2 EKF以及UKF濾波算法

　　Kalman濾波在處理線性高斯模型以及弱非線性系統(tǒng)時(shí)有著比較理想的效果，誤差顯著減少，但是在處理較強(qiáng)非線性系統(tǒng)時(shí)其濾波效果大部分情況下很難滿(mǎn)足要求。而在實(shí)際應(yīng)用場(chǎng)景即空基偽衛(wèi)星在飛行過(guò)程中存在嚴(yán)重的非線性問(wèn)題，此時(shí)常采用EKF以及UKF濾波方法對(duì)動(dòng)目標(biāo)的行進(jìn)路線進(jìn)行預(yù)測(cè)與估計(jì)[7]。

　　EKF的基本思路是圍繞濾波值將非線性函數(shù)f(*)和h(*)展成Taylor級(jí)數(shù)，將一般的非線性系統(tǒng)處理為一個(gè)線性化系統(tǒng)的模型，再使用上述提到的傳統(tǒng)Kalman濾波進(jìn)行濾波處理[10]。但是Kalman濾波存在數(shù)值穩(wěn)定性以及模型偏差等問(wèn)題，同時(shí)又要求系統(tǒng)模型和系統(tǒng)噪聲的統(tǒng)計(jì)特性精確已知，因此當(dāng)系統(tǒng)具有較強(qiáng)的非線性或者初始誤差較大時(shí)，EKF的濾波精度就會(huì)明顯下降，甚至?xí)霈F(xiàn)發(fā)散[11]。

　　不敏Kalman濾波器(UKF)是針對(duì)非線性系統(tǒng)的一種改進(jìn)型Kalman濾波器，采用Kalman線性濾波框架，對(duì)于一步預(yù)測(cè)方程使用不敏(UT)變換解決協(xié)方差以及均值的非線性處理問(wèn)題。UKF實(shí)質(zhì)上不是對(duì)非線性函數(shù)進(jìn)行近似，而是對(duì)非線性函數(shù)的概率密度分布進(jìn)行近似，同時(shí)也不需要對(duì)Jacobian矩陣求導(dǎo)，沒(méi)有忽略高階項(xiàng)，這就使得UKF克服了EKF精度低、穩(wěn)定性差的缺點(diǎn)[11]。

　　UT變換與線性化方法的比較如圖2所示。

　　2 平方根UKF濾波算法

　　在傳統(tǒng)UKF濾波算法中，需要對(duì)每個(gè)采樣點(diǎn)進(jìn)行非線性變換，計(jì)算量比較大，而且數(shù)值計(jì)算的誤差也比較明顯，估計(jì)誤差協(xié)方差矩陣的非負(fù)定性和對(duì)稱(chēng)性會(huì)因此受到影響，從而影響濾波算法的收斂速度以及穩(wěn)定性[12]。為了提高濾波算法的濾波效率以及濾波精度，在遞推運(yùn)算過(guò)程中采用協(xié)方差矩陣的平方根來(lái)代替?zhèn)鹘y(tǒng)算法計(jì)算過(guò)程中的協(xié)方差矩陣，這種方法稱(chēng)為平方根不敏Kalman濾波算法[12]。

　　由式(1)—式(3)及初始值[AKX^]0=E[X0]，P0=E[(X0-[AKX^]0)(X0-[AKX^]0)T]，根據(jù)定義，在這里Pk及其預(yù)測(cè)Pk，k-1至少是非負(fù)定的，但是在舍去誤差的情況下，很難保證這一點(diǎn)。因此，在遞推過(guò)程中將Pk的遞推式改為Pk平方根Sk的遞推式，從而建立平方根濾波方程。

　　2.1 Pk的平方根遞推方程

　　根據(jù)定義，Pk-1具有對(duì)稱(chēng)非負(fù)定性，設(shè)

　　Pk-1=Sk-1STk-1，

　　于是

　　Pk/k-1=Φk/k-1Pk-1ΦTk/k-1=Φk/k-1Sk-1STk-1ΦTk/k-1=Sk/k-1STk/k-1，

　　將式中的Sk/k-1=Φk/k-1Sk-1代入到式(3)中，

　　Pk=[I-KkHk]Pk/k-1=Sk/k-1{I-STk/k-1HTk[HkSk/k-1STk/k-1HTk+Rk]-1HkSk/k-1}STk/k-1， (4)

　　記Fk=STk/k-1HTk，式(4)可寫(xiě)成

　　Pk=Sk/k-1{I-Fk[FTkFk+Rk]-1FTk}STk/k-1=Sk/k-1[I-akFkFTk]STk/k-1，

　　式中ak=[FTkFk+Rk]-1。

　　2.2 平方根濾波方程

　　平方根濾波方差可寫(xiě)為

　　[AKX^]k=Φk/k-1[AKX^]k/k-1+Kk[Zk-HkΦk/k-1[AKX^]k-1]，

　　[AKX^]0=E[X0]，

　　Kk=akSk/k-1STk/k-1HTk=akSk/k-1Fk，

　　Fk=STk/k-1HTk，

　　Sk/k-1=Φk/k-1Sk-1， P0=S0ST0，

　　Sk=Sk/k-1[I-akrkFkFTk]， rk=11±akRk。

　　推薦閱讀：基于PCO+FFT的B1C導(dǎo)航信號(hào)捕獲算法