模糊卡爾曼濾波北斗偽距定位

曹學瑤， 胡黃水， 韓 博

(長春工業大學 計算機科學與工程學院，吉林 長春 130012)

0 引 言

北斗衛星導航系統(以下稱為北斗系統)是我國自主創建研發的衛星導航系統，也是全球第三個成熟的導航系統。在交通運輸、農業林業、水力監測等領域都得到了普遍應用[1-2]。由于北斗系統可以為用戶端提供全天候、全天時的高精度定位服務，所以提高北斗定位精度極其重要。

根據觀測量的不同，北斗定位可以分為偽距定位和載波相位定位[3]。相比載波相位定位來說,偽距定位不要求高強度信號，也不存在整周模糊度的問題,并且偽距定位速度較快[4]。隨著定位技術的日漸成熟，國內外專家對偽距定位算法展開了深入研究，目前的主流算法有極大似然估計、最小二乘法、卡爾曼濾波和牛頓迭代法等。文獻[5]提出加權最小二乘法，對可信度高的觀測值分配大的權值。但加權最小二乘法對觀測數據中的粗差不具備處理能力，所以定位誤差大。文獻[6]采用抗差M估計算法，將最小二乘算法的解作為抗差初值，然后,根據偽距殘差矢量和等價權矩陣進行計算位置坐標。和最小二乘法相比，抗差M估計定位精度更高。文獻[7]采用卡爾曼濾波進行定位，直接將位置與鐘差的信息作為狀態變量求解,忽略了卡爾曼濾波對定位的初始位置敏感的特點，導致了定位精度不高。文獻[8]提出在擴展卡爾曼濾波的基礎上加入加權最小二乘法。首先,利用加權最小二乘法對系統進行線性化處理后,用擴展卡爾曼濾波對用戶端位置進行預測。該算法相對于文獻[7]提升了線性化的精度，但由于觀測量中粗差和卡爾曼濾波中量測噪聲固定不變的影響，難以滿足高精度定位要求。

為了解決最小二乘法對觀測數據中的粗差無法進行處理,以及卡爾曼濾波對初始位置敏感且量測噪聲方差不變的問題，提出融合抗差M估計和模糊卡爾曼濾波算法進行偽距定位。首先,利用抗差M估計解算出用戶端的位置，然后將解算結果作為卡爾曼濾波的初始狀態位置,根據模糊控制系統不斷調節濾波增益Kk值進行定位解算，從而提高定位精度。

1 偽距定位原理

偽距定位原理是通過北斗導航系統中導航衛星的三維位置坐標信息,以及衛星到接收端的距離得到用戶端的三維位置坐標信息的過程[9]。其中,導航衛星的三維位置坐標信息可以通過導航電文中的星歷數據獲得，由于衛星發射的信號會受到各種誤差的干擾,導致用戶端接收到的觀測距離并不準確。將帶有誤差的觀測距離稱為偽距[10],公式如下：

ρi=ri+δρ1+δρ2+c(δtu-δt1)，

(1)

(2)

式中:ri——第i顆衛星與用戶端的真實距離：

ρi——衛星與用戶設備之間的真實距離；

δtu——電流層的時延距離；

δρ1——對流層的時延距離；

δt1——用戶設備的鐘差；

δρ2——衛星的鐘差；

c——信號的傳播速度,c=2.997 924 58 m/s;

(xi,yi,zi)——第i顆衛星的三維坐標；

(x,y,z)——用戶接收機的位置坐標。

忽略可修正項，偽距定位公式為

ρi=ri+c(δtu-δt1)=

cδt,

(3)

i=1,2,…,n，

式中：δt——接收機的鐘差。

2 算法設計

利用抗差M估計構造等價權函數，避免粗差影響未知參數的估值。將該估值作為模糊卡爾曼濾波器的初始值，并對量測噪聲方差實時進行調整，進而提高系統的定位精度。

2.1 抗差M估計計算初值

在北斗導航系統中，最小二乘法是偽距定位系統的觀測方程。初始值

x0=(x0,y0,z0,δt0)

處線性化后得到的系統觀測模型為

Δy=H·Δx+e,

(4)

式中：Δx——m×1的矢量，表示狀態殘差，即真實位置與初始化后得到的位置之間的差值;

m——當前歷元下的衛星個數;

Δy——m×1的矢量，表示偽距殘差,即偽距的觀測值與估計值之間的差值；

H——m×4的觀測矩陣；

e——m×1的誤差矢量，其元素一般假設為相互獨立的高斯隨機過程。

首先用最小二乘法解算此線性方程，目標函數為

(5)

初始值處的最小二乘解為

(6)

偽距殘差矢量為

Δr=Δy-HΔx=(In-H(HTH)-1HT)e。

(7)

當測量噪聲為高斯分布時，最小二乘估計為最優估計[11-13]。但在北斗導航系統中，觀測量中含有粗差會直接影響最小二乘定位精度，因此采用M估計抑制粗差[14]，其目標函數為

(8)

(9)

采用Huber[15]函數給含有粗差的觀測值賦予小的權值

(10)

式中:k——根據M估計方差性能確定的常數。

抗差M估計表達式為

(11)

式中：Wk+1——等價權矩陣，公式為

(12)

抗差M估計定位的具體步驟為：

4)返回2)，直到兩相鄰步驟的回歸系數差值小于設定閾值時，迭代結束，即

max|xk+1-xk|<ξ。

2.2 模糊自適應Kalmen濾波定位

擴展卡爾曼濾波有兩個系統模型[16]：系統觀測模型以及系統狀態模型。

狀態方程

xk=Ak,k-1xk-1+ωk-1,

(13)

式中：xk——歷元k下的系統狀態向量；

xk-1——歷元k-1下的系統狀態向量;

Ak,k-1——歷元k-1到歷元k的狀態轉移矩陣;

ωk-1——高斯白噪聲，均值為零，且相互獨立。

觀測方程

yk=Bkxk+vk,

(14)

式中：yk——歷元k的觀測量；

Bk——狀態量和觀測量之間的關系矩陣；

vk——高斯白噪聲，均值為零，且相互獨立。

擴展卡爾曼濾波包含預測和更新兩個過程[16-17]，其預測方程為：

狀態先驗估計值

(15)

求解先驗估計值的協方差

(16)

式中：Qk——系統噪聲序列的方差陣。

其更新方程為：

濾波增益矩陣

(17)

式中：Rk——測量噪聲序列的方差陣。

對狀態更新

(18)

對協方差陣更新

(19)

量測噪聲具有隨機性[18],但擴展卡爾曼濾波中量測噪聲的方差始終都采用初始計算時的先驗值[19],因此,采用模糊卡爾曼濾波[17]調整量測噪聲的協方差矩陣[20]來減小濾波發散。

新息(rk)是觀測量估計值與觀測量真實值的差，公式為

rk=yk-Bkxk,k-1。

(20)

式(18)可以寫為

(21)

(22)

(23)

由模糊推理系統(FIS)可以計算出調整因子tk。tk可以對量測噪聲方差進行調整。模糊推理系統[21]采用單輸入、單輸出Mamdani模糊邏輯控制器[22]，將每一步的殘差實測方差與理論方差的比值作為輸入，輸出為tk。采用的隸屬度函數如圖1所示。

圖1 Ck與tk的隸屬度函數

模糊規則為：

If1>,then1>

If,then

If,then

Fk殘差的實測方差陣受新息影響,

(24)

式中：N——統計數。

Dk是殘差的理論方差陣，

(25)

Ck是殘差的實測方差矩陣和殘差的理論方差矩陣的商,

(26)

每進行一次濾波FIS更新一次輸出值,即tk，再將tk代入式(22)、式(23)中,對量測噪聲方差矩陣進行在線調整。

因此,基于M估計的模糊卡爾曼濾波定位解算步驟如下:

1)將M估計解算位置和鐘差信息作為擴展卡爾曼濾波的狀態初始值，然后初始化協方差陣;

2)建立模糊卡爾曼濾波系統方程，對狀態向量和觀測向量協方差陣進行估計。通過FIS計算調整因子tk，然后進行預測和更新過程;

3)判斷歷元是否解算完畢，未完成返回2),繼續解算。

3 仿真分析

采用Matlab軟件對RINEX文件數據[23]進行解算。在觀測文件為 450個歷元數據的情況下，分別采用擴展卡爾曼濾波,以及模糊自適應卡爾曼濾波方法對X、Y、Z三 個方向進行解算，然后再將定位結果進行對比分析。誤差對比如圖2所示。

(a)擴展卡爾曼濾波算法在X、Y、Z三個方向的誤差結果

圖2中,X、Y、Z三個方向的平均誤差分別為-3.96、-8.96、-13.47 m，明顯可以看出，模糊卡爾曼濾波算法的誤差更小，濾波結果也更加平滑。模糊卡爾曼濾波的定位解算結果相較于擴展卡爾曼濾波的定位精度有明顯提高，相較于圖2(a)的收斂過程也明顯縮短，實現了更加精確的定位。

利用Matlab軟件將擴展卡爾曼濾波與模糊卡爾曼濾波的運動軌跡與接收機真實軌跡進行模擬。

將擴展卡爾曼濾波、真實軌跡以及模糊卡爾曼濾波進行誤差對比，如圖3所示。

圖3 擴展卡爾曼濾波、真實軌跡以及模糊卡爾曼濾波誤差對比

由圖3可以看出，擴展卡爾曼濾波與真實值之間存在的誤差較大,且擬合度不高; 而模糊卡爾曼濾波運動軌跡更加集中，而且均勻分布在真實軌跡四周，擬合程度較好。故模糊卡爾曼濾波對定位誤差具有極好的改進。

4 結 語

結合M估計和模糊卡爾曼濾波的定位解算算法，將抗差M估計的定位結果作為擴展卡爾曼濾波算法的初值，通過模糊推理系統得到的調整因子來調整量測噪聲抑制濾波發散情況，進而提高定位精度。解決了傳統最小二乘算法對粗差不具抵抗力，以及卡爾曼濾波收斂過程慢且對初值敏感的缺點。

通過 Matlab 處理數據的實驗結果也表明，模糊卡爾曼濾波算法的收斂速度與定位精度均明顯提高。