基于修正量測(cè)的UKF算法

崔 燦, 劉 攀, 張 敏

(1 湖北工業(yè)大學(xué)機(jī)械工程學(xué)院， 湖北 武漢 430068； 2 湖北工業(yè)大學(xué)工程技術(shù)學(xué)院， 湖北 武漢 430068)

在許多實(shí)際問(wèn)題中，大量的濾波問(wèn)題都是非線性的，解決非線性濾波問(wèn)題的兩種主要方法都是基于卡爾曼濾波，卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器), 它能夠從一系列的不完全包含噪聲的測(cè)量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)，然而簡(jiǎn)單的卡爾曼濾波必須應(yīng)用在符合高斯分布的系統(tǒng)中。第一種就是擴(kuò)展Kalman濾波(EKF)，它只取非線性函數(shù)泰勒展開(kāi)式的一階線性段，其高階項(xiàng)舍去，來(lái)到達(dá)線性化的目的。通過(guò)將非線性的階段線性化，然后舍棄高階項(xiàng)或逼近的方法來(lái)處理非線性問(wèn)題[1]。然而，當(dāng)非線性函數(shù)的泰勒展開(kāi)不能舍棄時(shí)，這種線性化會(huì)造成函數(shù)的較大誤差，甚至對(duì)最終的濾波結(jié)果造成很大影響。另一種是通過(guò)采樣的方法來(lái)逼近非線性分布的方法，將固定數(shù)量的參數(shù)支近似一個(gè)高斯分布的UT變換作為基礎(chǔ)，使用Kalman線性濾波框架，并且采樣不是隨機(jī)采樣，而是采取確定的sigma點(diǎn)，這種方法是無(wú)跡Kalman濾波[2]，并且它可以達(dá)到泰勒三階的近似效果。UKF無(wú)跡卡爾曼濾波是在卡爾曼濾波和變換的基礎(chǔ)上發(fā)展而來(lái)的，它是利用無(wú)損變換使線性假設(shè)下的卡爾曼濾波應(yīng)用于非線性系統(tǒng)，但是在UKF的計(jì)算中，當(dāng)存在隨機(jī)向量相關(guān)或奇異矩陣的時(shí)候[3]，則不能采用Cholesky分解，該分解是把一個(gè)對(duì)稱正定的矩陣表示成一個(gè)下三角矩陣L和其轉(zhuǎn)置的乘積的分解。這將影響方差矩陣平方根的計(jì)算，進(jìn)而嚴(yán)重影響sigma點(diǎn)的求解[4]。本文就上述問(wèn)題引入修正量測(cè)，將一個(gè)正定矩陣替換之前的奇異矩陣，使得協(xié)方差陣轉(zhuǎn)化為正定矩陣，進(jìn)而可以使濾波正常進(jìn)行[5]。

1 修正量測(cè)UKF

上文提到傳統(tǒng)的做法是將非線性函數(shù)轉(zhuǎn)變?yōu)榫€性化來(lái)解決問(wèn)題[6]，而無(wú)跡卡爾曼濾波運(yùn)用的是濾波框架和無(wú)跡變化來(lái)處理預(yù)測(cè)方程中的均值問(wèn)題和協(xié)方差非線性傳遞問(wèn)題[7]。無(wú)跡卡爾曼濾波算法逼近非線性函數(shù)的概率密度分布，并用一系列確定的樣本來(lái)逼近狀態(tài)的后驗(yàn)概率密度[8]。無(wú)跡卡爾曼濾波流程為：

Wk中包含協(xié)方差矩陣Q,Vk中包含協(xié)方差陣R

可以看出一旦產(chǎn)生奇異矩陣，UT變換時(shí)，算法精度會(huì)受到很大的影響，嚴(yán)重影響著算法的收斂。換言之，協(xié)方差矩陣R必須局部修正。因此，通過(guò)數(shù)次試驗(yàn)后本文選擇了一個(gè)正定矩陣

用來(lái)替換之前的協(xié)方差矩陣，整個(gè)協(xié)方差為正定矩陣，繼而在濾波器中可以使用構(gòu)造的C-UKF(Correction Unscented Kalman Filter)算法。 然而，這種調(diào)整需要付出相應(yīng)的代價(jià)，調(diào)整之后的R’不等于先前的R，導(dǎo)致濾波器變?yōu)榇蝺?yōu)[9]。下文將對(duì)這種調(diào)整進(jìn)行具體研究。

2 仿真實(shí)驗(yàn)設(shè)計(jì)與驗(yàn)證

在雷達(dá)跟蹤系統(tǒng)的東-南-高(ESU)的坐標(biāo)系中(圖1)，方位角ψ的取值范圍一般為-π<ψ≤π若此時(shí)存在一架飛行器穿過(guò)YOZ平面自西向東進(jìn)行飛行，那么此時(shí)雷達(dá)跟蹤系統(tǒng)的觀測(cè)值會(huì)從π瞬間突變到-π，如圖2所示。

圖 1 飛行器坐標(biāo)系

圖2 飛行角度與雷達(dá)觀測(cè)值變化

如果方位角和量測(cè)信號(hào)突變，將導(dǎo)致濾波器發(fā)散。選用方位角的余弦和正弦值代替方位角信息，觀測(cè)向量等效為

[ρθcos(ψ) sin(ψ)]

Vψ=[cos(ψ) sin(ψ)]T

令ψ的微小變化量為δψ，可得微小變化量δVψ

δVψ=[-sin(ψ)δψcos(ψ)δψ]T=

[-sin(ψ) cos(ψ)]Tδψ

令微小變化量δψ代表方位角ψ的測(cè)量誤差，可以得到Vψ的協(xié)方差矩陣

將雷達(dá)系統(tǒng)中的協(xié)方差矩陣視為非奇異矩陣。將方位角信息轉(zhuǎn)化為Vψ，造成量測(cè)的局部相關(guān)性，且Rψ是奇異矩陣，表現(xiàn)在協(xié)方差矩陣上為

在這種情況下，則

R在這里必為奇異矩陣。

在三維空間中，設(shè)定飛行器以恒定的速度在飛行，在 ESU 坐標(biāo)系下，地面有一雷達(dá)對(duì)其進(jìn)行觀測(cè)。其狀態(tài)方程在n時(shí)刻表示為

狀態(tài)轉(zhuǎn)移方程為

目標(biāo)到達(dá)雷達(dá)距離ρ，俯仰角θ，方位角ψ，這三個(gè)參數(shù)構(gòu)成了雷達(dá)的觀測(cè)向量Z=[ρθΨ]T，觀測(cè)方程為

式中：wn是由高斯隨機(jī)變量組成的向量，協(xié)方差矩陣是

修改之后的觀測(cè)方程為

仿真試驗(yàn)分兩個(gè)方面進(jìn)行:

1)設(shè)定飛行軌跡使之不通過(guò)角度突變區(qū)域，圖3所示。

2) 設(shè)定飛行軌跡使之通過(guò)角度突變區(qū)域，如圖4所示。

圖 3 飛行軌跡(經(jīng)過(guò)角度突變)

圖 4 飛行軌跡(不經(jīng)過(guò)角度突變)

在雷達(dá)系統(tǒng)中，兩種方法對(duì)位移的估計(jì)性能如圖3所示，以速度為測(cè)量的雷達(dá)系統(tǒng)中，兩種方法對(duì)速度的估計(jì)性能如圖4所示。對(duì)象圖表中的誤差估計(jì)是由AVE(Absolute value error)與RMSE(Root Mean Square Error)得到，可表示為:

(a)

(b)

(c)

(d)圖 5 仿真結(jié)果圖

仿真結(jié)果:上述仿真分別是飛行器通過(guò)角突變區(qū)時(shí)，軌跡在X，Y,Z軸上位置的均方根誤差。當(dāng)我們使用一個(gè)較小的值替換奇異矩陣的原值時(shí)[10]，雖然會(huì)損失一定的濾波精度，對(duì)奇異矩陣進(jìn)行了修改，但是解決原先濾波中奇異協(xié)方差矩陣的問(wèn)題，使得C-UKF精度比傳統(tǒng)UKF精度更高，對(duì)于濾波中的奇異矩陣誤差變大的問(wèn)題具有更好的解決性能。

3 結(jié)束語(yǔ)

通過(guò)雷達(dá)觀測(cè)飛行器時(shí)角度突變的問(wèn)題[11]，將導(dǎo)致濾波器發(fā)散的情況來(lái)仿真對(duì)比 UKF和C-UKF算法[12]。實(shí)驗(yàn)結(jié)果表明，C-UKF通過(guò)Cholesky分解，進(jìn)而選取sigma點(diǎn)，使正定矩陣替換之前的奇異矩陣，通過(guò)修正量測(cè)的UT，很好的處理了奇異矩陣的問(wèn)題，讓其自身變得精度更高。