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

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

0 引言
    卡爾曼濾波(KF)是一個(gè)不斷預(yù)測(cè)、修正的遞推過(guò)程，已經(jīng)越來(lái)越多地應(yīng)用于動(dòng)態(tài)數(shù)據(jù)處理中。然而在KF模型中，要求觀(guān)測(cè)方程是線(xiàn)性形式、狀態(tài)噪聲和測(cè)量噪聲是白噪聲。為了解決這種矛盾，將KF理論應(yīng)用到非線(xiàn)性領(lǐng)域中，擴(kuò)展卡爾曼濾波(EKF)應(yīng)用而生，它圍繞狀態(tài)估計(jì)值對(duì)非線(xiàn)性系統(tǒng)進(jìn)行一階Taylor展開(kāi)使其線(xiàn)性化，但它存在如下不足：一是當(dāng)非線(xiàn)性函數(shù)Taylor展開(kāi)式的高階項(xiàng)無(wú)法忽略時(shí)，會(huì)產(chǎn)生較大的線(xiàn)性化誤差；二是EKF假定噪聲均為正態(tài)白噪聲，但是一個(gè)正態(tài)自噪聲經(jīng)過(guò)非線(xiàn)性變換后通常不再呈現(xiàn)正態(tài)性；三是只有在雅可比矩陣存在時(shí)才能線(xiàn)性化，而很多應(yīng)用中雅可比矩陣很難求。針對(duì)這種情況，Julier和Uhlmann等人提出了一種基于非線(xiàn)性變換——Un-scented變換(UT)的卡爾曼濾波算法(UKF)。它通過(guò)確定性采樣得到的一組sigma點(diǎn)，可以獲得更多的觀(guān)測(cè)假設(shè)，對(duì)系統(tǒng)狀態(tài)的均值和協(xié)方差的估計(jì)更為準(zhǔn)確，同時(shí)由于該算法采用了非線(xiàn)性的狀態(tài)方程和觀(guān)測(cè)方程，從而避免了線(xiàn)性化誤差。目前，EKF和UKF算法被廣泛應(yīng)用于GPS非線(xiàn)性動(dòng)態(tài)濾波研究中，并取得良好的定位效果。
    本文給出了一種加權(quán)最小二乘一卡爾曼濾波(WLS-KF)算法，它利用一組離散采樣點(diǎn)，通過(guò)WLS方法產(chǎn)生近似線(xiàn)性化預(yù)測(cè)模型，然后用KF算法對(duì)此模型進(jìn)行校正。以該算法為思路，將其應(yīng)用在GPS非線(xiàn)性動(dòng)態(tài)濾波定位解算中，仿真結(jié)果表明該算法精度介于EKF和UKF之間，從而實(shí)現(xiàn)了對(duì)目標(biāo)較高精度的定位和跟蹤。

1 算法描述
1．1 WLS算法
    LS算法是現(xiàn)代測(cè)量技術(shù)中數(shù)據(jù)處理的基本工具，這種方法的特點(diǎn)是算法簡(jiǎn)單，在估計(jì)解算時(shí)，不需要知道與被估計(jì)量以及觀(guān)測(cè)量有關(guān)的任何統(tǒng)計(jì)信息。
    設(shè)X為待估參數(shù)矢量，觀(guān)測(cè)矢量為L(zhǎng)。觀(guān)測(cè)方程為：
   
    式中：v為觀(guān)測(cè)誤差。用泰勒定理展開(kāi)，得到線(xiàn)性化的觀(guān)測(cè)方程為：
   
    式中：A是系數(shù)矩陣；f(X0)表示用先驗(yàn)參數(shù)計(jì)算的理論觀(guān)測(cè)向量；b表示擬合后的殘差；△X是對(duì)先驗(yàn)參數(shù)的小修正向量。
    LS算法的指標(biāo)是使各次觀(guān)測(cè)量與由參數(shù)估計(jì)得到的觀(guān)測(cè)量之差的平方和最小，即：
   
    要使上式達(dá)到最小，當(dāng)對(duì)觀(guān)測(cè)矢量的質(zhì)量有所了解時(shí)，設(shè)置各個(gè)測(cè)量值的權(quán)重，得到WLS算法的解為：
   
    若更新后的Xk尚未達(dá)到求解精度，則Xk可作為第k+1次迭代的起始點(diǎn)，繼續(xù)進(jìn)行上述運(yùn)算。
1．2 KF算法
    KF算法對(duì)一個(gè)離散時(shí)間線(xiàn)性系統(tǒng)的狀態(tài)進(jìn)行最優(yōu)估計(jì)，使系統(tǒng)狀態(tài)的估計(jì)值有最小均方誤差(MMSE)。
    考慮一個(gè)多輸入多輸出的離散時(shí)間線(xiàn)性動(dòng)態(tài)系統(tǒng)，其狀態(tài)方程可表示為：
   
    向量wk和vk分別表示狀態(tài)噪聲和測(cè)量噪聲，假設(shè)它們都是均值為零的正態(tài)白噪聲且相互獨(dú)立，即：
   
    KF算法通過(guò)如下的時(shí)間更新過(guò)程和測(cè)量更新過(guò)程來(lái)對(duì)系統(tǒng)狀態(tài)進(jìn)行估計(jì)。
  
    計(jì)算完測(cè)量更新方程后，整個(gè)過(guò)程再次重復(fù)，上一次計(jì)算得到的后驗(yàn)估計(jì)被作為下一次計(jì)算的先驗(yàn)估計(jì)。
1．3 WLS-KF算法
1．3．1 基本思想
    算法基本思想是通過(guò)WLS方法計(jì)算隨機(jī)變量經(jīng)過(guò)非線(xiàn)性變換后的分布參數(shù)，它利用離散個(gè)加權(quán)點(diǎn)來(lái)確定近似直線(xiàn)。
    設(shè)待確定的近似線(xiàn)性化模型為：

    式中：(xn，f(xn))表示設(shè)置的離散加權(quán)點(diǎn)；p(xn)表示其對(duì)應(yīng)的權(quán)系數(shù)，n=1，2，…，n。
1．3．2 算法流程
    將加權(quán)最小二乘線(xiàn)性化方法同KF算法相結(jié)合，便得到了WLS-KF算法。具體的算法流程如下：
    (1)設(shè)置矩陣A，b和加權(quán)矩陣W；
    (2)將A，b和W帶入式(17)，求出線(xiàn)性化系數(shù)矩陣A1，常數(shù)向量b1；


2 實(shí)驗(yàn)結(jié)果與分析
    基于運(yùn)動(dòng)載體為質(zhì)點(diǎn)的假設(shè)建立目標(biāo)運(yùn)動(dòng)模型，考慮一個(gè)二維平面曲線(xiàn)運(yùn)動(dòng)系統(tǒng)，狀態(tài)定義為，分別表示二維方向上的位置變量、速度變量、載體相對(duì)于起始位置的轉(zhuǎn)彎角度及接收機(jī)鐘差。

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

    為便于比較，算法中使用的隨機(jī)數(shù)據(jù)均來(lái)自于提前保存好的隨機(jī)序列。結(jié)果表明，WLS-KF算法的計(jì)算量適中，精度介于EKF和UKF之間，且效率同KF接近。

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