基于WLS-KF的GPS非線性動態(tài)濾波研究

齊小強(qiáng),廉保旺,薛喆

摘 要：為了提高動態(tài)定位精度，將卡爾曼(KF)算法應(yīng)用到GPS非線性動態(tài)定位解算中，提出加權(quán)最小二乘-卡爾曼濾波(WLS-KF)算法。通過加權(quán)最小二乘(WLS)算法得到近似的線性化模型，再將KF算法應(yīng)用到這個線性化模型進(jìn)行校正。因此既保持了KF算法能夠?qū)ο到y(tǒng)狀態(tài)進(jìn)行最優(yōu)估算的優(yōu)點(diǎn)，同時(shí)對各個測量值進(jìn)行了聯(lián)系制約，具有更高的精度。結(jié)果表明，這種方法精度介于EKF和UKF之間，且實(shí)現(xiàn)容易，預(yù)測可靠，具有實(shí)際應(yīng)用價(jià)值。

關(guān)鍵詞：全球定位系統(tǒng); 卡爾曼濾波; 加權(quán)最小二乘; 非線性

中圖分類號：TN967-34

文獻(xiàn)標(biāo)識碼：A

文章編號：1004-373X(2011)09-0087-03

GPS Nonlinear Dynamic Filter Algorithm Based on Weighted Least Squares-Kalman Filter

QI Xiao-qiang，LIAN Bao-wang，XUE Zhe

(School of Electronic and Information, Northwestern Polytechnical University, Xian 710129, China)

Abstract： In order to improve the positioning accuracy, the weighted least squares-Kalman filter（WLS-KF）algorithm is proposed by applying Kalman filter （KF） into GPS nonlinear dynamic filter. An approximate linear system is formed from the WLS algorithm and regulated by KF. Thus it can keep the advantages of that the KF algorithm can optimally estimate the system status, restrict and contact each measured value. The results show that the accuracy range of this algorithm between EKF and UKF has reliable prediction and practical applications.

Keywords： GPS; Kalman filter; weighted least squares; nonlinear

0 引 言

卡爾曼濾波(KF)是一個不斷預(yù)測、修正的遞推過程，已經(jīng)越來越多地應(yīng)用于動態(tài)數(shù)據(jù)處理中［1］。然而在KF模型中，要求觀測方程是線性形式、狀態(tài)噪聲和測量噪聲是白噪聲［2］。為了解決這種矛盾，將KF理論應(yīng)用到非線性領(lǐng)域中，擴(kuò)展卡爾曼濾波(EKF)應(yīng)用而生，它圍繞狀態(tài)估計(jì)值對非線性系統(tǒng)進(jìn)行一階Taylor展開使其線性化，但它存在如下不足：一是當(dāng)非線性函數(shù)Taylor展開式的高階項(xiàng)無法忽略時(shí)，會產(chǎn)生較大的線性化誤差；二是EKF假定噪聲均為正態(tài)白噪聲，但是一個正態(tài)白噪聲經(jīng)過非線性變換后通常不再呈現(xiàn)正態(tài)性；三是只有在雅可比矩陣存在時(shí)才能線性化，而很多應(yīng)用中雅可比矩陣很難求［2-4］。針對這種情況，Julier和Uhlmann等人提出了一種基于非線性變換——Unscented變換(UT)的卡爾曼濾波算法(UKF)。它通過確定性采樣得到的一組sigma點(diǎn)，可以獲得更多的觀測假設(shè)，對系統(tǒng)狀態(tài)的均值和協(xié)方差的估計(jì)更為準(zhǔn)確，同時(shí)由于該算法采用了非線性的狀態(tài)方程和觀測方程，從而避免了線性化誤差［4-6］。目前，EKF和UKF算法被廣泛應(yīng)用于GPS非線性動態(tài)濾波研究中，并取得良好的定位效果［7-9］。

本文給出了一種加權(quán)最小二乘-卡爾曼濾波(WLS-KF)算法，它利用一組離散采樣點(diǎn)，通過WLS方法產(chǎn)生近似線性化預(yù)測模型，然后用KF算法對此模型進(jìn)行校正。以該算法為思路，將其應(yīng)用在GPS非線性動態(tài)濾波定位解算中，仿真結(jié)果表明該算法精度介于EKF和UKF之間，從而實(shí)現(xiàn)了對目標(biāo)較高精度的定位和跟蹤。

1 算法描述

1.1 WLS算法

LS算法是現(xiàn)代測量技術(shù)中數(shù)據(jù)處理的基本工具，這種方法的特點(diǎn)是算法簡單，在估計(jì)解算時(shí)，不需要知道與被估計(jì)量以及觀測量有關(guān)的任何統(tǒng)計(jì)信息。

設(shè)X為待估參數(shù)矢量，觀測矢量為L。觀測方程為：

L=f(X)+v

(1)

式中:v為觀測誤差。用泰勒定理展開，得到線性化的觀測方程為:

L=f(X0)+A?ΔX+v

或者:

b=L－f(X0)=AΔX

(2)

式中:A是系數(shù)矩陣;f(X0)表示用先驗(yàn)參數(shù)計(jì)算的理論觀測向量;b表示擬合后的殘差;ΔX是對先驗(yàn)參數(shù)的小修正向量。

LS算法的指標(biāo)是使各次觀測量與由參數(shù)估計(jì)得到的觀測量之差的平方和最小，即：

P(ΔX)=‖A?ΔX－b‖2=min

(3)

要使上式達(dá)到最小，當(dāng)對觀測矢量的質(zhì)量有所了解時(shí)，設(shè)置各個測量值的權(quán)重，得到WLS算法的解為：

ΔX=(A琓WA)－1A琓Wb

(4)

這時(shí)觀測方程的解就從Xk－1更新為Xk:

Xk=Xk－1+ΔX

(5)

若更新后的Xk尚未達(dá)到求解精度，則Xk可作為第k+1次迭代的起始點(diǎn)，繼續(xù)進(jìn)行上述運(yùn)算。

1.2 KF算法

KF算法對一個離散時(shí)間線性系統(tǒng)的狀態(tài)進(jìn)行最優(yōu)估計(jì)，使系統(tǒng)狀態(tài)的估計(jì)值有最小均方誤差(MMSE)。

考慮一個多輸入多輸出的離散時(shí)間線性動態(tài)系統(tǒng)，其狀態(tài)方程可表示為：

xk=Axk－1+Buk－1+wk－1

(6)

觀測方程為：

yk=Cxk+vk

(7)

向量wk和vk分別表示狀態(tài)噪聲和測量噪聲，假設(shè)它們都是均值為零的正態(tài)白噪聲且相互獨(dú)立，即：

wk～N(0,Q)，vk～N(0,R)

E(wiv琓i)=0

(8)

KF算法通過如下的時(shí)間更新過程和測量更新過程來對系統(tǒng)狀態(tài)進(jìn)行估計(jì)。

時(shí)間更新過程：

－k=Ak－1+Buk－1

(9)

P－k=APk－1A琓+Q

(10)

測量更新過程：

Kk=P－kC琓(CP－kC琓+R)－1

(11)

k=－k+Kk(yk－C－k)

(12)

Pk=(I－KkC)P－k

(13)

計(jì)算完測量更新方程后，整個過程再次重復(fù)，上一次計(jì)算得到的后驗(yàn)估計(jì)被作為下一次計(jì)算的先驗(yàn)估計(jì)。

1.3 WLS-KF算法

1.3.1 基本思想

算法基本思想是通過WLS方法計(jì)算隨機(jī)變量經(jīng)過非線性變換后的分布參數(shù)，它利用離散個加權(quán)點(diǎn)來確定近似直線。

設(shè)待確定的近似線性化模型為:

y=x+

(14)

要得到此模型，可設(shè)變量:

u=［］琓

(15)

由:

Au=b

(16)

得:

u=(A琓WA)－1A琓Wb

(17)

在上式中，設(shè)：

A=x1x2…xn

11…1琓

b=［f(x1)f(x2)…f(xn)］琓

W=diag(p(x1),p(x2),…,p(xn))

式中：(xn,f(xn))表示設(shè)置的離散加權(quán)點(diǎn);p(xn)表示其對應(yīng)的權(quán)系數(shù)，n=1,2,…,n。

1.3.2 算法流程

將加權(quán)最小二乘線性化方法同KF算法相結(jié)合，便得到了WLS-KF算法。具體的算法流程如下：

(1) 設(shè)置矩陣A，b和加權(quán)矩陣W；

(2) 將A，b和W帶入式(17)，求出線性化系數(shù)矩陣A1，常數(shù)向量b1；

(3) 根據(jù)以上結(jié)果，得到近似的預(yù)測方程：

x－k=A1xk－1+b1

(18)

(4) P－k=A1Pk－1A琓1+Q(19)

(5) 重復(fù)(1)～(3)，利用x－k和P－k，得到測量矩陣H和測量向量的預(yù)測值y－k；

(6) K=P－kH琓(HP－kH琓+R)－1

(20)

(7)

xk=x－k+K(yk－y－k)

(21)

(8) Pk=(I－KH)P－k

(22)

2 實(shí)驗(yàn)結(jié)果與分析

基于運(yùn)動載體為質(zhì)點(diǎn)的假設(shè)建立目標(biāo)運(yùn)動模型，考慮一個二維平面曲線運(yùn)動系統(tǒng)，狀態(tài)定義為X=［x y ω δt］，分別表示二維方向上的位置變量、速度變量、載體相對于起始位置的轉(zhuǎn)彎角度及接收機(jī)鐘差。

系統(tǒng)狀態(tài)方程為：

xk+1=f(xk,k)=xk2+25xk1+x2k+8cos(1.2k)+wk

(23)

yk+1=g(yk,k)=yk2+25yk1+x2k+8sin(1.2k)+vk

(24)

測量方程為：

Zk=h(Xk)=X2k20+εk

(25)

式中:wk，vk和εk為相互獨(dú)立的高斯白噪聲序列，wk～N(0,10)，vk～N(0,10)，εk～N(0,1)。此系統(tǒng)是具有可加噪聲的離散非線性時(shí)變系統(tǒng) ［10-11］，觀測時(shí)間為100 s，觀測周期為1 s。WLS-KF和EKF,UKF算法的二維預(yù)測結(jié)果分別如圖1,圖2所示，預(yù)測結(jié)果誤差分析見表1。

實(shí)線為真實(shí)值，虛線為預(yù)測值

圖1 WLS-KF,EKF,UKF在x方向預(yù)測結(jié)果比較

實(shí)線為真實(shí)值，虛線為預(yù)測值

圖2 WLS-KF,EKF,UKF在y方向預(yù)測結(jié)果比較

表1 WLS-KF,EKF,UKF預(yù)測結(jié)果誤差分析

MEANSTD

dim 1dim 2dim 1dim 2

WLS-KF0.173 00.273 610.248 09.842 9

EKF0.657 1-0.820 011.043 09.006 8

UKF-0.071 20.720 89.946 79.944 6

為便于比較，算法中使用的隨機(jī)數(shù)據(jù)均來自于提前保存好的隨機(jī)序列。結(jié)果表明，WLS-KF算法的計(jì)算量

適中，精度介于EKF和UKF之間，且效率同KF接近。

3 結(jié) 語

本文引入了一種基于加權(quán)最小二乘的線性化方法，將其與KF算法相結(jié)合，給出了可以應(yīng)用于GPS非線性動態(tài)濾波系統(tǒng)的WLS-KF算法。該算法不需要求偏導(dǎo)數(shù)，算法精度介于EKF和UKF之間，可以實(shí)現(xiàn)對目標(biāo)較高精度的定位和跟蹤，有一定的應(yīng)用價(jià)值。

參考文獻(xiàn)

［1］KAPLAN Elliott D,HEGARTY Christopher J.GPS原理與應(yīng)用［M］.2版.北京：電子工業(yè)出版社，2007.

［2］宋迎春.動態(tài)定位中的卡爾曼濾波研究［D］.長沙：中南大學(xué)，2006.

［3］謝鋼.GPS原理與接收機(jī)設(shè)計(jì)［M］.北京：電子工業(yè)出版社，2009.

［4］劉國海，李沁雪，施維.動態(tài)卡爾曼濾波在導(dǎo)航試驗(yàn)狀態(tài)估計(jì)中的應(yīng)用［J］.儀器儀表學(xué)報(bào)，2009,30(2)：396-400.

［5］VAN DER MERWE R, WAN E A. The square-root unscented Kalman filter for state and parameter estimation [J]. Acoustics, Speech and Signal Processing Proceedings，2001, 6： 3461-3464.

［6］JULIER S J, UHLMANN J K. Unscented filtering and nonlinear estimation [J]. Proceedings of the IEEE, 2004, 92, (3)：401-422.

［7］孫紅星，付建紅，袁修素,等.基于多歷元遞推最小二乘卡爾曼濾波方法的模糊度解算［J］.武漢大學(xué)學(xué)報(bào)，2008，33(7)：735-739.

［8］蘇偉，王國宏.有色噪聲下基于總體最小二乘和卡爾曼濾波的無源定位方法［J］.彈箭與制導(dǎo)學(xué)報(bào)，2009，29(4)，217-219.

［9］孫慶祥，戴邵武.加權(quán)最小二乘-卡爾曼濾波算法在動基座對準(zhǔn)中的應(yīng)用［J］.海軍航空工程學(xué)院學(xué)報(bào)，2008，23(4)，395-397.

［10］SCHON Thomas B. Estimation of nonlinear dynamic systems theory and applications [D]. [S.l.]: Linkopings University, 2006.

［11］CORDON N J, SALMOND D J, SMITH A F M. Novel approach to non-lineat/non-Gaussian Bayesian state estimation [J]. IEEE Proc. on Radar and Processing, 1993, 140(2): 107-113.

注：本文中所涉及到的圖表、注解、公式等內(nèi)容請以PDF格式閱讀原文