摘 要： 為了解決傳統Kalman濾波在處理非線性系統時的局限性，以及擴展Kalman濾波(EKF)在處理強非線性系統時發散性和精度較差的問題，結合動態導航系統中的目標跟蹤定位問題，在不敏Kalman濾波(UKF)算法的基礎上，提出了一種基于平方根UKF的動態跟蹤定位算法，在遞推運算過程中采用協方差矩陣的平方根代替傳統算法計算過程中的協方差矩陣。MATLAB仿真結果表明，平方根UKF算法的精度比EKF提升了54.7%，比UKF提升了14.8%。所提出的算法解決了Kalman處理非線性系統的局限性以及傳統EKF和UKF算法精度不高的問題，為偽衛星系統的高精度定位研究提供了有力支撐。
關鍵詞： 算法理論;動態定位跟蹤;偽衛星;平方根濾波;卡爾曼濾波算法;非線性
為了解決傳統衛星星座在某些特定區域(如峽谷、隧道、密林等)受地形遮擋[1]定位授時精度難以達到要求以及信號功率相對較弱容易受到復雜環境影響干擾欺騙的問題，作為一種區域增強系統，偽衛星能有效彌補衛星信號受遮擋的問題，并提供較強的導航信號。近幾年，空基偽衛星成為研究熱點。相比于陸基偽衛星，空基偽衛星雖有著更加靈活、應用場合更為多樣、覆蓋區域更廣的優勢，但也面臨著諸多問題[2-4]。
空基偽衛星這一動態平臺的自身位置對整個系統的定位精度有著重要影響。針對動態系統的定位跟蹤，常采用Kalman濾波算法及擴展Kalman濾波算法。一般而言，Kalman濾波算法或者在其基礎上演變的相關算法，在處理線性或者弱非線性系統方面效果較為理想，但對于較為復雜的非線性系統，其處理結果對于較高精度的動態跟蹤定位來說很難達到要求[5-7]。針對上述問題，本文提出一種基于平方根UKF的動態目標跟蹤定位算法，并與常見的EKF以及UKF算法進行仿真分析對比。
1 傳統跟蹤定位算法
空基偽衛星動態平臺在運動過程中，偽衛星飛行狀態之間、狀態與觀測量之間存在嚴重的非線性關系，因此需要采用非線性濾波技術得到其狀態變量的最優估計值，將濾波算法運用到動目標跟蹤定位中，通過引入濾波方法對動目標的行進路線進行預測與估計。常見的用于目標跟蹤定位算法的濾波方法有Kalman濾波算法、EKF算法和UKF算法。
1.1 Kalman濾波算法
Kalman濾波是根據一定的濾波準則，采用最優化估計方法對觀測系統的狀態進行估計與預測的一種最優估計或最優濾波。其思路是在估計過程中，用上一次的最優狀態估計和最優誤差估計計算這一次的先驗狀態估計和先驗誤差估計，再用得到的先驗誤差估計以及量測噪聲得到Kalman增益，結合前面所得到的先驗估計和Kalman增益得到本次的最優估計，不斷重復這個過程，用本次的最優估計來估算下一次的先驗估計[8-9]。
假設動態系統的狀態空間模型為
X(t+1)=ΦX(t)+ΓW(t)，
Y(t)=HX(t)+V(t)，
式中：X(t)為系統在時刻t的狀態;Y(t)為對狀態的觀測值;W(t)為系統噪聲，其方差矩陣為Q;V(t)為觀測噪聲，其方差陣為R;Φ為狀態轉移矩陣;H為觀測矩陣;Γ為系統噪聲驅動矩陣。Kalman濾波的計算流程如下。
計算狀態一步預測：
[AKX^](t+1|t)=Φ[AKX^](t|t);
計算新息：
ε(t+1)=Y(t+1)-H[AKX^](t+1|t);
計算狀態估計值：
[AKX^](k+1|k+1)=[AKX^](k+1|k)+K(k+1)ε(k+1);
計算Kalman濾波增益：
K(t+1)=P(t+1|t)HT[HP(t+1|t)HT+R]-1; (1)
計算一步預測均方誤差：
P(t+1|t)=ΦP(t|t)ΦT+ΓQΓT; (2)
計算一步預測估計均方誤差：
P(t+1|t+1)=[In-K(t+1)H]P(t+1|t)。 (3)
為了更形象地說明Kalman濾波原理，圖1給出了Kalman濾波的系統模型框圖。
1.2 EKF以及UKF濾波算法
Kalman濾波在處理線性高斯模型以及弱非線性系統時有著比較理想的效果，誤差顯著減少，但是在處理較強非線性系統時其濾波效果大部分情況下很難滿足要求。而在實際應用場景即空基偽衛星在飛行過程中存在嚴重的非線性問題，此時常采用EKF以及UKF濾波方法對動目標的行進路線進行預測與估計[7]。
EKF的基本思路是圍繞濾波值將非線性函數f(*)和h(*)展成Taylor級數，將一般的非線性系統處理為一個線性化系統的模型，再使用上述提到的傳統Kalman濾波進行濾波處理[10]。但是Kalman濾波存在數值穩定性以及模型偏差等問題，同時又要求系統模型和系統噪聲的統計特性精確已知，因此當系統具有較強的非線性或者初始誤差較大時，EKF的濾波精度就會明顯下降，甚至會出現發散[11]。
不敏Kalman濾波器(UKF)是針對非線性系統的一種改進型Kalman濾波器，采用Kalman線性濾波框架，對于一步預測方程使用不敏(UT)變換解決協方差以及均值的非線性處理問題。UKF實質上不是對非線性函數進行近似，而是對非線性函數的概率密度分布進行近似，同時也不需要對Jacobian矩陣求導，沒有忽略高階項，這就使得UKF克服了EKF精度低、穩定性差的缺點[11]。
UT變換與線性化方法的比較如圖2所示。
2 平方根UKF濾波算法
在傳統UKF濾波算法中，需要對每個采樣點進行非線性變換，計算量比較大，而且數值計算的誤差也比較明顯，估計誤差協方差矩陣的非負定性和對稱性會因此受到影響，從而影響濾波算法的收斂速度以及穩定性[12]。為了提高濾波算法的濾波效率以及濾波精度，在遞推運算過程中采用協方差矩陣的平方根來代替傳統算法計算過程中的協方差矩陣，這種方法稱為平方根不敏Kalman濾波算法[12]。
由式(1)—式(3)及初始值[AKX^]0=E[X0]，P0=E[(X0-[AKX^]0)(X0-[AKX^]0)T]，根據定義，在這里Pk及其預測Pk，k-1至少是非負定的，但是在舍去誤差的情況下，很難保證這一點。因此，在遞推過程中將Pk的遞推式改為Pk平方根Sk的遞推式，從而建立平方根濾波方程。
2.1 Pk的平方根遞推方程
根據定義，Pk-1具有對稱非負定性，設
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)可寫成
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 平方根濾波方程
平方根濾波方差可寫為
[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。
