基于修正量測的UKF算法

崔 燦, 劉 攀, 張 敏

(1 湖北工業大學機械工程學院， 湖北 武漢 430068； 2 湖北工業大學工程技術學院， 湖北 武漢 430068)

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

1 修正量測UKF

上文提到傳統的做法是將非線性函數轉變為線性化來解決問題[6]，而無跡卡爾曼濾波運用的是濾波框架和無跡變化來處理預測方程中的均值問題和協方差非線性傳遞問題[7]。無跡卡爾曼濾波算法逼近非線性函數的概率密度分布，并用一系列確定的樣本來逼近狀態的后驗概率密度[8]。無跡卡爾曼濾波流程為：

Wk中包含協方差矩陣Q,Vk中包含協方差陣R

可以看出一旦產生奇異矩陣，UT變換時，算法精度會受到很大的影響，嚴重影響著算法的收斂。換言之，協方差矩陣R必須局部修正。因此，通過數次試驗后本文選擇了一個正定矩陣

用來替換之前的協方差矩陣，整個協方差為正定矩陣，繼而在濾波器中可以使用構造的C-UKF(Correction Unscented Kalman Filter)算法。 然而，這種調整需要付出相應的代價，調整之后的R’不等于先前的R，導致濾波器變為次優[9]。下文將對這種調整進行具體研究。

2 仿真實驗設計與驗證

在雷達跟蹤系統的東-南-高(ESU)的坐標系中(圖1)，方位角ψ的取值范圍一般為-π<ψ≤π若此時存在一架飛行器穿過YOZ平面自西向東進行飛行，那么此時雷達跟蹤系統的觀測值會從π瞬間突變到-π，如圖2所示。

圖 1 飛行器坐標系

圖2 飛行角度與雷達觀測值變化

如果方位角和量測信號突變，將導致濾波器發散。選用方位角的余弦和正弦值代替方位角信息，觀測向量等效為

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

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

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

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

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

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

將雷達系統中的協方差矩陣視為非奇異矩陣。將方位角信息轉化為Vψ，造成量測的局部相關性，且Rψ是奇異矩陣，表現在協方差矩陣上為

在這種情況下，則

R在這里必為奇異矩陣。

在三維空間中，設定飛行器以恒定的速度在飛行，在 ESU 坐標系下，地面有一雷達對其進行觀測。其狀態方程在n時刻表示為

狀態轉移方程為

目標到達雷達距離ρ，俯仰角θ，方位角ψ，這三個參數構成了雷達的觀測向量Z=[ρθΨ]T，觀測方程為

式中：wn是由高斯隨機變量組成的向量，協方差矩陣是

修改之后的觀測方程為

仿真試驗分兩個方面進行:

1)設定飛行軌跡使之不通過角度突變區域，圖3所示。

2) 設定飛行軌跡使之通過角度突變區域，如圖4所示。

圖 3 飛行軌跡(經過角度突變)

圖 4 飛行軌跡(不經過角度突變)

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

(a)

(b)

(c)

(d)圖 5 仿真結果圖

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

3 結束語

通過雷達觀測飛行器時角度突變的問題[11]，將導致濾波器發散的情況來仿真對比 UKF和C-UKF算法[12]。實驗結果表明，C-UKF通過Cholesky分解，進而選取sigma點，使正定矩陣替換之前的奇異矩陣，通過修正量測的UT，很好的處理了奇異矩陣的問題，讓其自身變得精度更高。