本發(fā)明屬于目標跟蹤技術(shù)領(lǐng)域，尤其涉及一種基于確定性采樣的增廣卡爾曼濾波方法。

背景技術(shù)：

目標跟蹤主要通過雷達或者無線傳感器接收數(shù)據(jù)，由卡爾曼濾波器進行狀態(tài)估計，從而獲取運動目標的位置信息和速度信息。目標跟蹤的應用已從早期的軍事領(lǐng)域擴展到了日常生活中，是卡爾曼濾波算法較為重要的應用研究領(lǐng)域之一。為了有效地解決目標追蹤過程中建模系統(tǒng)的高非線性以及環(huán)境中未知干擾的影響，實現(xiàn)較好的跟蹤效果，各類非線性卡爾曼濾波算法相繼被提出。應用最為廣泛的是擴展卡爾曼濾波器(extended Kalman filter，EKF)。對非線性函數(shù)的一階線性化使得EKF的濾波精度直接受系統(tǒng)非線性程度的影響，在非線性程度較高時，會出現(xiàn)濾波精度下降甚至發(fā)散等情況。此外，EKF算法需要計算系統(tǒng)的雅克比矩陣，這對于某些非線性系統(tǒng)來說難以實現(xiàn)。因此，各種各樣的積分方法被提出，用以有效地近似非線性濾波中的高斯加權(quán)積分，因而產(chǎn)生了一類基于確定性采樣的非線性卡爾曼濾波器。其中，基于無跡變換(unscented transformation，UT)的無跡卡爾曼濾波器(unscented Kalman filter，UKF)通過一系列含有狀態(tài)分布信息的加權(quán)Sigma點能獲取高斯變量的一階和二階統(tǒng)計特性。但是，UKF中最優(yōu)參數(shù)的選取仍然在研究中。人們大都通過實驗或者依據(jù)經(jīng)驗來設置參數(shù)，這在一定程度上影響了濾波的精度。相比之下，容積卡爾曼濾波器(cubature Kalman filter，CKF)以及球面單純形-徑向容積卡爾曼濾波器(simplex spherical-radial cubature Kalman filter，SSRCKF),因其較高的數(shù)值穩(wěn)定性以及無需考慮參數(shù)優(yōu)化問題成為近年來的研究熱點。與EKF相比，在非線性程度較高的系統(tǒng)中，UKF，CKF和SSRCKF均能在利用較少計算量的同時獲取較高的精度和穩(wěn)定性。而高階(主要指五階)容積準則的提出雖然在一定程度上產(chǎn)生了較好的濾波效果，但高階權(quán)重負值的出現(xiàn)直接影響了濾波穩(wěn)定性，因而低階(三階)的重要性依然得到了更多的認可。基于確定性采樣的非線性卡爾曼濾波算法在預測和糾錯兩個階段均需獨立采點一次，而在某些情況下，點的重復生成會引起高階統(tǒng)計信息的丟失，從而降低濾波精度。最早出現(xiàn)的是增廣無跡卡爾曼濾波器(augmented unscented Kalman filter，AUKF)，它的優(yōu)越性體現(xiàn)在非加性噪聲環(huán)境下，狀態(tài)與過程噪聲以及觀測噪聲融合后經(jīng)一次采樣傳遞狀態(tài)的統(tǒng)計信息，使得AUKF的濾波性能明顯優(yōu)于UKF；但是，在過程噪聲和觀測噪聲均為加性的假設下，UKF可以在減小計算量的同時獲得與AUKF相似的濾波效果。

綜上所述，解決傳統(tǒng)的低階非線性卡爾曼濾波算法在處理高維非線性跟蹤問題時出現(xiàn)的濾波精度下降，魯棒性差等問題很有必要。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的在于提供一種基于確定性采樣的增廣卡爾曼濾波方法，旨在解決傳統(tǒng)的低階非線性卡爾曼濾波算法在處理高維非線性跟蹤問題時出現(xiàn)的濾波精度下降，魯棒性差的問題。

本發(fā)明是這樣實現(xiàn)的，一種基于確定性采樣的增廣卡爾曼濾波方法，所述基于確定性采樣的增廣卡爾曼濾波方法采用狀態(tài)重構(gòu)方法，在預測階段將狀態(tài)向量x與過程噪聲q以及觀測噪聲r三者融合起來，使得新的狀態(tài)由原來n維的x擴展到N＝n+n+m維的以增廣UKF為例，在預測階段用增廣的狀態(tài)量產(chǎn)生一次Sigma點，而在糾錯階段，保留了第一次采樣取得的Sigma點。

進一步，所述基于確定性采樣的增廣卡爾曼濾波方法包括：

i)系統(tǒng)初始化:在給定了狀態(tài)估計值的初始值和協(xié)方差后，設置增廣狀態(tài)的初始值為協(xié)方差矩陣為：

ii)預測:獲取采樣點

得到的N維采樣點中，第1到n行為狀態(tài)采樣點第n+1到2n行為過程噪聲采樣點第2n+1到2N行為觀測噪聲采樣點

把采樣點和代入狀態(tài)過程方程x_k＝f(x_k-1)+q_k-1中，得到一系列經(jīng)過轉(zhuǎn)換的采樣點：

計算增廣狀態(tài)的預測值和相應的誤差協(xié)方差矩陣：

iii)糾錯:計算經(jīng)過觀測方程轉(zhuǎn)換后的點：

分別計算觀測向量的預測值，新息的協(xié)方差，以及狀態(tài)與觀測向量之間的互協(xié)方差矩陣：

狀態(tài)估計值以及狀態(tài)估計誤差協(xié)方差矩陣的更新同公式和

本發(fā)明的另一目的在于提供一種由所述基于確定性采樣的增廣卡爾曼濾波方法制備的增廣容積卡爾曼濾波器。

本發(fā)明的另一目的在于提供一種由所述基于確定性采樣的增廣卡爾曼濾波方法制備的增廣球面單純形-徑向容積卡爾曼濾波器。

本發(fā)明提供的基于確定性采樣的增廣卡爾曼濾波方法，與傳統(tǒng)增廣形式相比，本發(fā)明增廣卡爾曼濾波器中狀態(tài)融合了過程噪聲以及觀測噪聲后，由原來的n維增廣到n+n+m維，在高維目標跟蹤系統(tǒng)中的濾波精度有顯著提高；此外，仿真實驗顯示，在復雜的環(huán)境中，對于系統(tǒng)本身的突變干擾和噪聲，本發(fā)明提出的增廣卡爾曼濾波算法表現(xiàn)出了較強的魯棒性，在犧牲了一定量的計算復雜度的前提下能達到實際系統(tǒng)中對濾波器性能的要求，是一類較好的目標跟蹤算法。

附圖說明

圖1是本發(fā)明實施例提供的自由落體運動的幾何圖式示意圖。

圖2是本發(fā)明實施例提供的目標下落位置的均方誤差曲線示意圖。

圖3是本發(fā)明實施例提供的軌跡跟蹤示意圖。

圖4是本發(fā)明實施例提供的軌跡跟蹤位置均方誤差曲線示意圖。

具體實施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點更加清楚明白，以下結(jié)合實施例，對本發(fā)明進行進一步詳細說明。應當理解，此處所描述的具體實施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。

本發(fā)明提出了一類基于確定性采樣的增廣非線性卡爾曼濾波算法；該類算法將狀態(tài)與過程噪聲以及觀測噪聲融合，整個濾波過程中只需構(gòu)建一次數(shù)值積分點。這不僅能夠獲取更多的先驗信息，也避免了重復采樣造成的高階信息的流失。通過目標自由落體和協(xié)調(diào)轉(zhuǎn)彎運動兩個高維模型驗證了所提的增廣跟蹤算法能夠在實時目標跟蹤中有效地減小突變干擾和噪聲的影響，提高跟蹤效果。

下面結(jié)合附圖對本發(fā)明的應用原理作詳細的描述。

1基于確定性采樣的非線性卡爾曼濾波算法

1.1高斯近似濾波框架

一般的離散非線性動力學系統(tǒng)可以描述為：

x_k＝f(x_k-1)+q_k-1 (1)

y_k＝h(x_k)+r_k (2)

其中狀態(tài)變量x_k∈Rⁿ，觀測向量y_k∈R^m；f(·)和h(·)是給定的非線性函數(shù)；q_k-1和r_k分別是零均值的高斯白噪聲，相應的協(xié)方差矩陣分別是Q_k和R_k；

在高斯假設的前提下，結(jié)合貝葉斯理論，可以把貝葉斯濾波看作是對一系列形為非線性函數(shù)×高斯概率密度的多維高斯積分的求解；利用數(shù)值積分理論，當高斯概率密度函數(shù)為標準型分布N(x；0；I)時，積分可以近似為：

這里，g(x)表示任意的非線性函數(shù)，L代表點的總個數(shù)，w_i和ξ_i分別表示權(quán)重和相應的積分點。

通過線性轉(zhuǎn)換關(guān)于的積分可以近似成：

其中，表示P的平方根矩陣，即和P分別是多維高斯分布的均值和協(xié)方差矩陣。令則基于公式(1)和(2)描述的系統(tǒng)的高斯近似濾波框架可總結(jié)如下：

i)預測階段：

其中，表示積分點，S_k-1是P_k-1的平方根矩陣。

ii)糾錯階段：

其中：

這里，是P_k|k-1的平方根矩陣。

1.2基于不同積分準則的采樣點與權(quán)重

由公式(4)所示的數(shù)值近似積分表達式可知，不同積分準則的使用可以得到不同的數(shù)值積分點和相應的權(quán)重，其中，最常見的積分點和權(quán)重有如下幾種。

1.2.1 Sigma點

相應的權(quán)重大小為：

其中，表示矩陣(n+λ)P的平方根矩陣的i階分量；0.001≤α≤1是一個常數(shù)參數(shù)，決定了采樣點在均值附近的擴散程度；β是一個考慮狀態(tài)先驗信息的參數(shù)，在高斯假設下，一般取值為2；λ＝α²(n+k)-n是一個標量參數(shù)，參數(shù)k在狀態(tài)估計時通常設置為0；權(quán)重和分別用于計算均值和協(xié)方差。

1.2.2容積點

基于三階球面-徑向容積準則的L＝2n個容積點及其權(quán)重的計算如下：

這里的e_i表示一個單位向量，第i個元素值為1。

1.2.3單純形點

基于三階球面單純形-徑向容積準則的L＝2n+2個單純形點和相應的權(quán)重系數(shù)如下：

與球面-徑向容積準則不同，這里的向量a_i是由n單純形的n+1個頂點構(gòu)成的，即a_i＝[a_i,1,a_i,2,…,a_i,n]^T,i＝1,2,…,n+1計算如下：

將上述Sigma點，容積點，單純形點以及各自相應的權(quán)重分別代入1.1節(jié)所示的濾波框架，可獲得一類基于確定性采樣的非線性卡爾曼濾波器，即UKF，CKF和SSRCKF。

2一類基于確定性采樣的增廣非線性卡爾曼濾波器

在非線性模型已知下，UKF分別需要在預測階段和糾錯階段獨立取兩次Sigma點，但是在某些情況下，Sigma點的重復生成不僅增加了計算量還會引起某些高階統(tǒng)計信息的丟失，從而影響到濾波精度。為了解決這種問題，提高算法精度，增廣無跡卡爾曼濾波采用較為徹底的狀態(tài)重構(gòu)方法，即在預測階段將狀態(tài)與過程噪聲以及觀測噪聲三者融合起來，使得新的狀態(tài)由原來n維的x擴展到N＝n+n+m維的x^a。在糾錯階段，并沒有重新采樣，而是保留了第一次采樣取得的Sigma點。以UKF為例，增廣無跡卡爾曼濾波的具體算法步驟總結(jié)如下。

i)系統(tǒng)初始化:在給定了狀態(tài)估計值的初始值和協(xié)方差后，設置增廣狀態(tài)的初始值為協(xié)方差矩陣為：

ii)預測:獲取采樣點

得到的N維采樣點中，第1到n行為狀態(tài)采樣點第n+1到2n行為過程噪聲采樣點第2n+1到2N行為觀測噪聲采樣點權(quán)重大小的計算同式(14)所示。

把采樣點和代入狀態(tài)過程方程(1)中，得到一系列經(jīng)過轉(zhuǎn)換的采樣點：

計算增廣狀態(tài)的預測值和相應的誤差協(xié)方差矩陣：

iii)糾錯:計算經(jīng)過觀測方程轉(zhuǎn)換后的點：

分別計算觀測向量的預測值，新息的協(xié)方差，以及狀態(tài)與觀測向量之間的互協(xié)方差矩陣：

狀態(tài)估計值以及狀態(tài)估計誤差協(xié)方差矩陣的更新同公式(7)，(8)。

基于上述算法框架，可以得到一類基于確定性采樣的增廣非線性卡爾曼濾波器，如增廣容積卡爾曼濾波器(augmented cubature Kalman filter，ACKF)，增廣球面單純形-徑向容積卡爾曼濾波器(augmented simplex-spherical cubature Kalman filter，ASSRCKF)。與本發(fā)明提出的一類算法不同的是，這種增廣形式在預測階段僅把狀態(tài)與過程噪聲二者融合，即將n維的狀態(tài)增廣到n+n維，從而初始化時協(xié)方差矩陣為：

為了加以區(qū)分比較，仿真中把僅融合觀察噪聲的濾波器分別表示為AUKF1，ACKF1和ASSRCKF1。而把本發(fā)明中提出的一類新的基于確定性采樣的增廣非線性卡爾曼濾波器分別表示為AUKF2，ACKF2和ASSRCKF2。

3算法仿真與分析

為了驗證新的增廣卡爾曼濾波算法在跟蹤系統(tǒng)中的可行性及魯棒性，文中給出了目標自由落體運動和協(xié)調(diào)轉(zhuǎn)彎兩個模型，并采用蒙特卡洛方法進行仿真。仿真中主要的比較對象為非增廣卡爾曼濾波算法如UKF，CKF，SSRCKF和新的增廣卡爾曼濾波算法AUKF2，ACKF2，ASSRCKF2。同時，仿真部分也對兩類不同增廣形式下的濾波器的性能進行了比較。仿真實例中，UKF中的參數(shù)分別取值為:α＝0.1，β＝2，k＝0。

3.1模型一

本發(fā)明采用三維自由落體跟蹤模型來驗證本發(fā)明提出的一類新的基于確定性采樣的增廣非線性卡爾曼濾波算法在多維模型中的有效性及優(yōu)越性。該模型通過雷達探測實時估計目標下落時的高度，速度，以及彈道系數(shù)。連續(xù)狀態(tài)空間模型描述如下：

經(jīng)過離散化的測距在k時刻表達為：

這里采用四階龍格-庫塔方法對式(28)所示的連續(xù)時間系統(tǒng)進行離散化，離散時間步長為1/64；傳感器的采樣間隔為1s；為了更直觀地理解自由落體運動物理模型，給出了相應的幾何圖式，如圖1所示；變量x₁(t)，x₂(t)和x₃(t)分別表示目標下落時的位置高度，速度和彈道系數(shù)D₁表示目標與雷達探測器水平方向上的距離，D₂表示雷達的位置高度.這里的是已知的常值系數(shù)，r_k是方差大小為R的零均值噪聲；初始階段，目標下落位置較高，而速度的變化較慢，使得趨于0值，系統(tǒng)可近似為線性；當目標距離雷達較近時，雷達對于觀測噪聲越來越敏感，系統(tǒng)非線性程度也會因此突然增加，這就對濾波器的性能提出了較高的要求；系統(tǒng)初始化時的狀態(tài)真實值設置為x(0)＝[3×10⁵,2×10⁴,1×10^-3]^T，初始估計值為初始方差大小為：

其他參數(shù)的大小分別設置為:D₁＝D₂＝1×10⁵,Q＝0,R＝1×10⁴；為了對所有濾波器的性能加以分析比較，定義狀態(tài)的均方誤差(root mean square error，RMSE)為：

其中，M是蒙特卡洛總次數(shù)，仿真中取值100，和分別表示k時刻第j次蒙特卡洛試驗中第i維狀態(tài)的真實值和估計值；

為了使結(jié)果的顯示更為清晰，給出了目標下落過程中的位置均方誤差曲線，如圖2所示；并列出了各類濾波器的均方誤差的均值以及一次仿真實驗消耗的時間，如表1所示；由圖2可以看到，10s內(nèi)所有濾波器的均方誤差曲線幾乎重疊，沒有明顯的性能差異，這是由于初始階段系統(tǒng)接近線性所至；10s后，增廣卡爾曼濾波算法的均方誤差曲線均在其所對應的非增廣算法的下方。此外，表1更加直觀地顯示了增廣卡爾曼濾波算法雖然耗費了一定量的時間，但卻在精度上有了較大的提高。

表1基于100次蒙特卡洛仿真的目標位置的均值均方誤差

表2兩類增廣濾波器在目標位置的均值均方誤差中的比較

表2分別給出了兩類不同增廣形式的卡爾曼濾波器的估計誤差，從表中可以看出，在增加了較小計算量的前提下，融入了過程噪聲和觀測噪聲的增廣形式的卡爾曼濾波器AUKF2，ACKF2和ASSRCKF2分別比僅僅融入過程噪聲的增廣卡爾曼濾波器AUKF1，ACKF1和ASSRCKF1的狀態(tài)估計誤差小，更適于實時目標跟蹤。

3.2模型二

這部分運用著名的協(xié)調(diào)轉(zhuǎn)彎模型(coordinated turn，CT)來驗證增廣卡爾曼濾波算法在狀態(tài)突變中表現(xiàn)出的較好的魯棒性；模型描述如下所示：

其中x_k和y_k表示方向，和分別表示k時刻x方向和y方向上的速度；ω_k表示角速率，在仿真中設置為：

模型中的其他參數(shù)設置:傳感器位置(S_x,S_y)＝(0,0)，q_k-1和r_k均是零均值的高斯白噪聲，其方差大小分別為：

真實的狀態(tài)初始值為x₀＝[1000,300,1000,-3/180×π]^T，初始估計值從分布中N(x；x₀,P₀)隨機選取，狀態(tài)的初始方差設置為P₀＝diag([100,10,100,10,1×10⁴])；為了更好地檢測濾波算法的魯棒性能，除了公式(34)角速率的時變設置以外，在每次實驗的時刻k＝20，還引入了位置狀態(tài)的突變：

所示的角速率的時變設置及位置狀態(tài)的突變，均可看作實際中復雜環(huán)境帶來的噪聲干擾。圖3是各類濾波器對目標位置的跟蹤結(jié)果，可以看到所有濾波器均能對位置進行實時跟蹤，相比于非增廣卡爾曼濾波器，其相應的增廣形式的濾波算法表現(xiàn)了更強的魯棒性，跟蹤效果更好。

定義位置信息的均方誤差如下：

其中，M＝100表示蒙特卡洛總次數(shù)；和分別表示k時刻變量x_k在第j次蒙特卡洛仿真中的真實值和估計值；和分別表示k時刻變量y_k在第j次蒙特卡洛仿真中的真實值和估計值。

圖4表示位置的均方誤差曲線，從圖4可以看出增廣卡爾曼濾波算法能夠有效地克服時變擾動和狀態(tài)突變帶來的影響，從而降低估計誤差。

表3基于100次蒙特卡洛仿真的目標位置的均值均方誤差

表4兩類增廣濾波器在目標下落位置的均值均方誤差中的比較

表格3的定量分析比較也顯示了增廣卡爾曼濾波對于位置狀態(tài)的跟蹤效果優(yōu)于其相應的非增廣形式，即增廣卡爾曼濾波算法能夠減小系統(tǒng)本身及環(huán)境干擾產(chǎn)生的估計誤差，從而有效地提高濾波精度，達到實際中對濾波效果做出的要求.此外，在表格4中，兩類不同增廣形式的卡爾曼濾波器的比較顯示了本發(fā)明提出的同時融入過程噪聲和觀測噪聲的增廣形式的濾波器具有更好的魯棒性。

本發(fā)明將狀態(tài)與過程噪聲以及觀測噪聲融合，再結(jié)合傳統(tǒng)的非線性卡爾曼濾波算法框架，得到了一類新的基于確定性采樣的增廣非線性卡爾曼濾波算法。仿真結(jié)果顯示了在高維非線性系統(tǒng)中，該類濾波算法利用一定量的計算復雜度的增加換來了濾波精度的較大提高，同時，對于系統(tǒng)本身和環(huán)境帶來的時變干擾也表現(xiàn)出了較好的魯棒性；此外，仿真實驗還表明，與僅把狀態(tài)與過程噪聲融合的增廣卡爾曼濾波算法相比，本發(fā)明中提出的這類新的基于確定性采樣的增廣非線性卡爾曼濾波算法能夠更好地達到實際目標跟蹤中對濾波器性能的要求，更適合用于實時目標跟蹤。

以上所述僅為本發(fā)明的較佳實施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進等，均應包含在本發(fā)明的保護范圍之內(nèi)。