《目標跟蹤系統(tǒng)中的濾波方法》課件第4章

第4章等式狀態(tài)約束條件下的濾波方法4.1引言4.2線性狀態(tài)約束方法4.3非線性狀態(tài)約束方法4.4線性等式狀態(tài)約束條件下的粒子濾波算法4.5非線性等式狀態(tài)約束條件下的濾波算法4.6小結(jié)

4.1引言

在狀態(tài)估計理論的實際應用中，狀態(tài)向量可能受到某些線性或者非線性函數(shù)的約束。例如：地面目標跟蹤系統(tǒng)中，目標運動軌跡可能會受道路二維形狀的約束；船舶沿海岸線航行過程中會受海岸線物理位置的約束；鐘擺在擺動過程中遵循機械能守恒定律等。如果能夠?qū)⑦@些固有的狀態(tài)約束條件有效地施加到濾波過程中，則從理論上可以獲得更高的濾波精度。盡管卡爾曼濾波是最小方差意義下的最優(yōu)濾波器，然而，如果系統(tǒng)是非線性的，則由卡爾曼濾波衍生出來的算法并不能得到最優(yōu)解。此外，即便系統(tǒng)是線性的，通過施加約束，可以縮小算法中所使用的系統(tǒng)模型和實際系統(tǒng)之間的差異。因此，針對狀態(tài)約束下估計算法的研究是很有意義的[1-3]。

4.2線性狀態(tài)約束方法

考慮系統(tǒng)模型

xk+1=Fxk+ωk

(4-1)

yk=Hxk+υk

(4-2)

其中，xk是k時刻的狀態(tài)值，yk是k時刻的量測值，ωk和υk是均值為零協(xié)方差分別為Q和R的過程噪聲和量測噪聲，F(xiàn)和H是狀態(tài)轉(zhuǎn)移矩陣和量測矩陣。Kalman濾波器是上世紀中葉由幾個不同研究者分別獨立提出的[11]。

Kalman濾波方程如下:

Pk|k－1=FPk－1|k－1FT+Q

(4-3)

Kk=Pk|k－1HT(HPk|k－1HT+R)－1

(4-4)(4-5)(4-6)(4-7)當ωk和υk為不相關(guān)的高斯白噪聲序列時，Kalman濾波器是最小方差濾波器，而且每一時刻估計誤差協(xié)方差的跡也是最小的。但當ωk和υk是非高斯時，Kalman濾波器是最小方差線性濾波器，也存在一些其它的非線性濾波器會表現(xiàn)出的更好的性能，例如文獻[12]針對量測噪聲為均勻分布時所提出的方法。當ωk和υk是相關(guān)或有色的噪聲序列時，式(4-3)~式(4-7)可以被修正，并以此來獲得最小方差濾波器[2]?，F(xiàn)在假定系統(tǒng)滿足等式約束

Dxk=d

(4-8)或者不等式約束

Dxk≤d

(4-9)

其中D是已知的矩陣，d是已知的向量。在這種情況下想要獲得滿足約束的狀態(tài)估計xk，即狀態(tài)估計值應滿足

^(4-10)或(4-11)4.2.1模型降階

在式(4-8)中的等式約束可以用降低系統(tǒng)模型參數(shù)的階數(shù)來解決[13]。舉一個例子，考慮系統(tǒng)(4-12)(4-13)假設(shè)有約束(4-14)如果將xk(3)=－xk(1)代入到式(4-12)和式(4-13)，就能得到(4-15)(4-16)(4-17)式(4-15)~式(4-17)可以被寫為(4-18)(4-19)該例說明如何將一個等式約束濾波問題轉(zhuǎn)化為一個等價的但無約束的濾波問題。對于無約束系統(tǒng)，Kalman濾波器是最優(yōu)線性估計，同時它也是最初的受約束系統(tǒng)的最優(yōu)線性估計。降階后模型的維數(shù)低于最初模型，因此減少了Kalman濾波器的計算量。這個方法的一個缺點是狀態(tài)變量的物理意義會丟失，另一個缺點是它不能直接地應用于式(4-9)中的不等式約束。4.2.2最佳量測

狀態(tài)等式約束可以作為具有零量測噪聲的最佳量測來對待[14,15]。如果這個約束通過式(4-8)給定，此處D是一個s×n維矩陣，其中s<n，然后將約束分量擴維為式(4-2)的s個最佳量測，擴維后的方程為(4-20)狀態(tài)等式(4-1)不改變，但是量測等式會增大。實際上，量測等式的最后s個分量是沒有噪聲的，這意味著狀態(tài)的后驗濾波估計和s個量測值是一致的。也就是說Kalman濾波估計滿足。這個方法在數(shù)學上等價于模型降階方法。4.2.3估計投影

另一種約束濾波方法是將Kalman濾波的無約束估計投影到約束面[5]。約束估計為(4-21)使得

Dx=d

(4-22)其中W是一個正定權(quán)值矩陣。該問題的解為(4-23)如果過程和量測噪聲是高斯的且設(shè)W=(Pk|k)－1，則得到服從狀態(tài)約束的最大概率估計。如果設(shè)W=I，則得到服從狀態(tài)約束的最小二乘估計。該方法類似于參考文獻[16]中使用的輸入信號估計方法。可以證明式(4-23)的約束狀態(tài)估計值是無偏的，即(4-24)令W=(Pk|k)－1，可得到最小方差濾波。即如果W=(Pk|k)－1，那么(4-25)滿足所有的。設(shè)W=I會產(chǎn)生一個約束估計，它比非約束估計更接近于真實狀態(tài)。即：對于任意時刻k，若W=I，則(4-26)在文獻[17]中，根據(jù)一些附加的性質(zhì)用另一種途徑得到式(4-23)。假定約束先驗估計是基于無約束估計的，約束濾波方程為(4-27)(4-28)(4-29)如果約束先驗估計是基于約束估計的，那么約束濾波為(4-30)(4-31)(4-32)4.2.4具有不等式約束的估計投影

約束濾波器的估計投影方法有這樣一個優(yōu)點，即：它能擴展到式(4-11)中的不等式約束。如果有約束條件，那么約束估計能夠通過修正式(4-21)和式(4-22)得到。即要解下列問題：(4-33)(4-34)使得如果主動約束的集合是一個已知的先驗值，那么式(4-33)和式(4-34)的解也是等式約束問題的一個解，即(4-35)使得(4-36)式(4-33)和式(4-34)的不等式約束問題等價于式(4-35)和式(4-36)的等式約束問題。因此等式約束狀態(tài)估計的所有性質(zhì)也適用于不等式約束狀態(tài)估計。4.2.5增益投影

標準Kalman濾波可以通過求解下列問題來推導[2]：(4-37)該問題的解給出了最優(yōu)Kalman增益(4-38)(4-39)狀態(tài)估計和量測更新方程為(4-40)(4-41)如果將約束增加到該問題中，那么式(4-37)的最小化問題可以寫為(4-42)使得(4-43)該約束問題的解為(4-44)當把的值代入式(4-41)中的Kk時，所得結(jié)果就是約束狀態(tài)估計(4-45)這個估計類似于式(4-23)在W=I的情況下的估計值。4.2.6概率密度函數(shù)截斷

在概率密度函數(shù)(ProbabilityDensityFunction，PDF)截斷方法中，通過Kalman濾波計算狀態(tài)估計的PDF，假定它是高斯的，而且在約束邊界截斷PDF，該約束狀態(tài)估計等價于截斷的PDF的均值。設(shè)計該方法的最初目的是解決不等式約束，對其進行稍加改變后可以應用到等式約束問題中。

當狀態(tài)向量維數(shù)大于一維時，該方法變得復雜。在這種情況下，對狀態(tài)估計進行歸一化，使得其分量相互獨立。然后，將歸一化的約束一次應用于一個分量。當所有的約束使用完之后，使用歸一化的逆過程來得到約束估計狀態(tài)。詳細過程可查看文獻[2,18]。4.2.7系統(tǒng)投影

狀態(tài)約束意味著過程噪聲也是受約束的。這種認識引導我們可以先修改初始估計誤差協(xié)方差和過程噪聲，然后再執(zhí)行標準的卡爾曼濾波方程[19]。假定受約束系統(tǒng)方程為

xk+1=Fxk+ωk

(4-46)

Dxk=d

(4-47)

同樣可以假設(shè)無噪聲系統(tǒng)也滿足上式約束條件，即DFxk=0。但是這種結(jié)果意味著Dωk=0。如果沒有滿足上述方程，那么噪聲與狀態(tài)就是相關(guān)的，這是與系統(tǒng)特性的典型假設(shè)相違背的。如果Dωk=0，則

DωkωTkDT=0

(4-48)

E(DωkωTkDT)=0

(4-49)

DQDT=0

(4-50)上述方程意味著Q必須是奇異矩陣，假設(shè)Q是行滿秩的。舉一個簡單例子，考慮式(4-12)和式(4-13)中的三態(tài)系統(tǒng)。由式(4-12)可得

x1,k+1+x3,k+1=5x1,k+5x3,k+ω1,k+ω3,k

(4-51)

將式(4-51)與式(4-14)結(jié)合，有

ω1,k+ω3,k=0

(4-52)

這意味著為了保證該約束系統(tǒng)的一致性，協(xié)方差矩陣Q必須是奇異的。要保證Dωk=0，就要使式(4-50)成立。

如果過程噪聲協(xié)方差Q沒有滿足式(4-50)，為了保證系統(tǒng)一致性，將Q投影到一個能夠滿足約束條件的修正協(xié)方差Q上，然后用Q代替卡爾曼濾波中的Q。為達此目的，首先計算DT的奇異值分解~~

這里，Sr是一個r×r的矩陣，r是D的秩。下一步我們計算N=U2UT2和Q=NQN。Q的取值滿足(4-53)~~(4-54)4.2.8軟約束

軟約束與硬約束相對，它的約束僅僅只需要近似滿足而不是嚴格滿足。通常，在約束是啟發(fā)式而不是嚴格的，或者在約束函數(shù)具有一定的不確定性和模糊性的情況下，我們希望實施軟約束。例如，假設(shè)在二維坐標系中有一個車輛導航系統(tǒng)，x(1)指示北方向，x(2)指示東方向。已知車輛在x(1)=mx(2)+b這樣的直線道路上行駛，其中m和b為已知的常量。但是由于道路也有一個未知的非零寬度，因此，狀態(tài)約束也可以被寫為x(1)≈mx(2)+b。在這種情況下獲得了一個近似等式約束，可稱之為軟約束。應該說，絕大多數(shù)實際工程系統(tǒng)中應該使用軟約束而不是硬約束。在卡爾曼濾波中軟約束可以用不同的方法來實現(xiàn)。首先，在最佳量測方法中通過加入少量的非零量測噪聲到最佳量測項中，將其擴展為軟約束。其次，軟約束可以通過在標準卡爾曼濾波中加入正則化項的方法來實現(xiàn)。第三，軟約束的執(zhí)行可以通過將無約束估計投影到約束方向而不是精確到約束表面來實現(xiàn)。4.2.9仿真實驗

考慮一個導航問題。首先，系統(tǒng)的兩個狀態(tài)分量分別表示地面車輛的向北方向和向東方向的位置，另外的兩個分量是向北方向和向東方向的角速度。車輛的角速度θ是順時針從東方向為起點測量得到的。使用某種測位置設(shè)備，獲得了車輛北方向和東方向的位置測量，這些測量值是含噪聲的。該系統(tǒng)方程表示如下:(4-55)(4-56)其中，T是離散的時間步長，uk是一個加速度輸入。過程噪聲協(xié)方差及量測噪聲協(xié)方差分別為Q=diag([4，4，1，1])和R=diag([900，900])。初始估計誤差協(xié)方差為P0=diag([900，900，4，4])。如果我們已知車輛在道路上是以航向θ在行駛，則有(4-57)將約束條件寫為Dixk=0，使用兩個矩陣，記為(4-58)(4-59)D1直接制約速度和位置。D2根據(jù)速度決定位置，即速度約束隱式地對位置進行約束。值得注意的是，此處我們不能使用約束D=[1－tanθ00]。如果我們使用該式，那么位置將受約束，而速度卻未受到約束。但是速度是通過系統(tǒng)方程來決定位置的。因此D的這種特性是不符合狀態(tài)方程的，特別是它違反了文獻[17]中的DF=d條件。此時，我們可以采取一些方法來實現(xiàn)狀態(tài)估計。例如，我們可以使用Q和P0來運行標準的無約束卡爾曼濾波，并且忽視那些約束條件，或運行最佳量測濾波算法，或?qū)o約束估計投影到約束面，或使用概率假設(shè)密度濾波截斷算法，或使用約束滑動水平估計(MHE)。另外，我們可以利用被預測的Q和P0|0，然后使用標準卡爾曼濾波。由于Q

和P0|0在約束方面具有一致性，如果初始估計x0|0滿足了約束，那么狀態(tài)估計對于所有的時間點都能滿足約束。這種方法便是系統(tǒng)投影法。我們注意到，在該情況下，不論是最佳量測濾波器、估計投影濾波器、PDF截斷濾波器，還是MHE，都不會改變估計值。因為無約束估計潛在地也受系統(tǒng)投影約束。除此之外，可以選擇使用式(4-58)或者式(4-59)中的D1、D2矩陣來約束系統(tǒng)。~~~^~使用MATLAB軟件對上述系統(tǒng)進行仿真，一共仿真150秒，每隔3秒觀測一次[1]。使用的狀態(tài)向量初始值為x0=[0010tanθ10]T，并在估計過程中將其作為初始估計。采用蒙特卡羅方法仿真100次。表4.1顯示的是兩個位置狀態(tài)分量的平均均方根誤差和約束誤差的均方根誤差。仿真結(jié)果表明所有約束濾波算法的約束誤差為0。當D1用來作為約束矩陣時，所有約束濾波器的執(zhí)行結(jié)果都是相同的。然而，當D2用來作為約束矩陣時，最佳量測與系統(tǒng)投影方法得到的結(jié)果是最好的。

4.3非線性狀態(tài)約束方法

若狀態(tài)約束是非線性的，則與Dxk=d相對應，非線性約束方程寫為

g(xk)=b

(4-60)

圍繞著約束方程中的xk|k－1進行泰勒級數(shù)展開，得^(4-61)其中，s是g(x)的維數(shù)，ei是自然數(shù)基向量的第i個分量。下面給出了n×n矩陣gi″(x)的第p行q列上的元素(4-62)忽略二階項，則有(4-63)若令(4-64)(4-65)式(4-63)與線性約束Dxk=d相同。因此，在約束線性化后，所有線性約束方法都可以用來解決非線性約束。為了進一步提高算法性能，下面介紹幾種其它的非線性約束濾波算法。4.3.1二階項展開

若在式(4-61)中保留二階項，那么約束估計可以近似為(4-66)使得(4-67)其中，W是一個加權(quán)矩陣，Mi，mi和μi都可以從式(4-61)獲得，即有(4-68)(4-69)(4-70)該思路和二階擴展卡爾曼濾波方法相似，依據(jù)系統(tǒng)方程和量測方程的線性化，為了提高性能而保留二階項[2]。式(4-66)和式(4-67)中的優(yōu)化問題可以通過數(shù)值化方法得到解決。文獻[9]中，假定s=1和M是正定時，采用拉格朗日乘子法來求解上述優(yōu)化問題。4.3.2平滑約束卡爾曼濾波

另外一種處理非線性等式約束的方法是平滑約束卡爾曼濾波(SCKF)[10]。這種方法首先通過線性化方法處理非線性約束，之后將其當作最佳量測來執(zhí)行。然而，該估計結(jié)果只能近似滿足非線性約束。如果在每一個量測時刻多次應用線性化約束，那么估計結(jié)果將隨著迭代次數(shù)的增加而不斷地接近于約束條件。該思路和無約束估計中的迭代卡爾曼濾波方法類似。在處理非線性系統(tǒng)的迭代卡爾曼濾波算法中，非線性化系統(tǒng)在每個測量時刻重復進行線性化。在平滑約束卡爾曼濾波中，非線性系統(tǒng)在每個時刻進行線性化，并將其作為測量值重復進行濾波，以提高精確性。舉例來說，在濾波過程中使用方差為1的量測值，相當于使用方差為10的量測值重復濾波10次。依據(jù)SCKF的方法，非線性約束g(x)=h通過下面的算法來實現(xiàn)，該算法在每次測量更新后被執(zhí)行。

Step1：初始化i為1，該值為約束的個數(shù)。

Step2：設(shè)R0′=αGPGT，其中G=g′(x)為1×n的雅可比矩陣。R0′是初始化方差，系統(tǒng)約束將其作為量測值納入到狀態(tài)估計中。注意到GPGT是g(x)的近似線性化方差，因此，R0′是這個方差的一部分，并作為量測值納入到系統(tǒng)約束中。α是一個調(diào)整參數(shù)，通常在0.01和0.1之間。

Step3：設(shè)R0′=R0′exp(－i)。該方程被用于逐步降低量測方差，而此量測方差是適用于約束的。

Step4：設(shè)。Si是一個與約束信息相關(guān)的歸一化形式。當Si超出閾值Smax時，迭代終止。Smax的一個典型值為100?；蛘哳A先設(shè)定一個迭代次數(shù)imax，當?shù)螖?shù)等于該值時，停止迭代。這樣做的原因是因為尚無法證明SCKF的收斂性。在迭代終止之后設(shè)xk|k=x及Pk|k=P。^^^

Step5：將約束合并入量測向量中，并應用下列公式：(4-71)(4-72)(4-73)這些方程用于量測更新，與標準的卡爾曼濾波方程相同。但是我們合并的量測分量并不是非常準確的約束量測。

Step6：計算更新后的雅可比矩陣G=g′(x)。將i值加1后，回到Step3繼續(xù)迭代。^4.3.3水平滑動估計

水平滑動估計(MovingHorizonEstimation，MHE)[4,20]用來在卡爾曼濾波算法的基礎(chǔ)上，解決下列優(yōu)化問題：(4-74)上述討論引出一種針對一般非線性約束估計的相似方法。給定如下非線性系統(tǒng)和非線性約束：

xk+1=f(xk)+ωk

(4-75)

yk=h(xk)+υk

(4-76)

g(xk)=0

(4-77)解優(yōu)化問題(4-78)使得

g({xk})=0

(4-79)

此處為了符號互用，用g({xk})代替g(xk)，k=1，…，N。這種受約束的非線性優(yōu)化問題可以通過多種方法解決，因此應用到特定優(yōu)化算法中的所有理論也可以應用到約束MHE中。該方法的缺點在于問題的維數(shù)隨著求解步數(shù)的增加而不斷增大。每得到一個量測值，獨立變量就會增加n個，其中n是狀態(tài)變量的個數(shù)。

為了減少計算量，將式(4-78)近似為(4-80)使得g({xk})=0

(4-81)其中{xk}是集合{xM，…，xN}，N－M+1是滑動的尺寸。該問題的維數(shù)是n(N－M+1)，滑動尺寸的選擇要在估計精度與計算工作量之間進行折中。估計誤差協(xié)方差PM|M是從標準的EKF遞歸中獲得的。計算公式如下：(4-82)(4-83)(4-84)(4-85)(4-86)文獻[21]給出了有關(guān)MHE穩(wěn)定性的一些結(jié)果。MHE在其公式的通用性方面有優(yōu)勢，但是即使選擇很小的滑動尺寸，與其它類型的具有約束的算法相比，其運算復雜度也很大。

在MHE中，另一個難點是式(4-74)與式(4-78)中對矩陣P0可逆的假設(shè)，以及式(4-80)中對矩陣PM|M可逆的假設(shè)。一個約束系統(tǒng)的估計誤差協(xié)方差通常是奇異的[5]，如同在式(4-86)中那樣，可以使用無約束濾波算法中的協(xié)方差矩陣繞開該問題，但是這種處理方法使得MHE算法變成次優(yōu)的，即使對于線性系統(tǒng)也是如此。解決該問題的另一種途徑是利用文獻[5]中的奇異約束協(xié)方差，且使其降低到一個對角陣形式。這將導致狀態(tài)估計進行相應轉(zhuǎn)換，一些轉(zhuǎn)換狀態(tài)估計的方差將會變成0，意味著式(4-80)中的這些估計值將不會隨著時刻的增加而改變。該途徑使MHE的結(jié)果更優(yōu)，但同樣額外增加了時間復雜度。

遞歸的非線性動態(tài)數(shù)據(jù)調(diào)和聯(lián)合預測更新最優(yōu)化是和MHE相似的約束狀態(tài)估計的另一種方法。這些方法本質(zhì)上都是一個滑動尺寸為1的水平滑動估計，然而該類方法的最終目標是數(shù)據(jù)調(diào)和(即為輸出估計)而不是狀態(tài)估計，并且該類方法也包括參數(shù)估計。4.3.4不敏卡爾曼濾波

不敏卡爾曼濾波(UnscentedKalmanFilter，UKF)適用于非線性系統(tǒng)，基于以下兩個基本原則。首先，雖然它很難實現(xiàn)一個概率密度函數(shù)的非線性變換，但是它對向量易于實現(xiàn)非線性變換。其次，不難在狀態(tài)空間中找到一組向量，其采樣概率密度函數(shù)近似于給定的概率密度函數(shù)。UKF通過產(chǎn)生一組稱為西格瑪點的向量進行運算。UKF使用的西格瑪點數(shù)目為n+1至2n+1，其中n為狀態(tài)向量的維數(shù)。西格瑪點以一種特殊的方式變換和合并，以得到一個狀態(tài)估計和狀態(tài)估計誤差協(xié)方差。約束可以作為量測向量的一部分被合并入量測向量中，將其看做是最佳量測方法進行處理。下面討論的多種方法都可以實現(xiàn)。一種可行的方法是在前一步無約束的UKF后驗估計的基礎(chǔ)上，進行先驗狀態(tài)估計。該方法相對于約束UKF，無約束UKF獨立運行，在每一個量測時刻，無約束UKF的狀態(tài)估計與被作為最佳量測的約束值相結(jié)合，以獲得一個約束的后驗UKF估計值。這種濾波器被稱為UKF投影，對于線性系統(tǒng)與約束條件來說，類似于式(4-27)~式(4-29)，注意非線性約束可以通過多種方式合并為最佳量測，例如線性化、二階擴張[9]、不敏變換或者是SCKF，這些途徑是一個開放的問題，可以作進一步研究。

另一種方法是在前一步約束的UKF后驗估計的基礎(chǔ)上，進行先驗狀態(tài)估計[17]，在每一個量測時刻，無約束UKF的狀態(tài)估計與被作為最佳量測的約束值相結(jié)合，以獲得一個約束的后驗UKF估計值。這種約束的后驗估計被作為下一時刻更新的初始條件，該濾波器被稱為等式約束UKF。該方法也等價于文獻[17]中的量測擴維UKF。對于線性系統(tǒng)和約束，等式約束UKF類似于式(4-30)~式(4-32)。文獻[22]提出了一種類似的濾波器，該文認為約束估計協(xié)方差將比無約束估計協(xié)方差大，因為無約束估計近似于最小方差估計。二步式UKF(2UKF)[22]將每一個后驗西格瑪點投影到約束面以獲得受約束的西格瑪點，狀態(tài)估計是通過以常規(guī)的方式對西格瑪點進行加權(quán)平均獲得的，由此產(chǎn)生的估計隨后投影到約束表面。注意到受約束的西格瑪點的均值并不要求一定滿足非線性約束。2UKF很特別，在應用約束之后，其估計誤差協(xié)方差將增大，其增大的原因是無約束估計是最小方差估計，所以想通過應用約束來改變估計就要增大協(xié)方差。此外，如果協(xié)方差隨著約束的應用而降低，那么協(xié)方差矩陣可能會變?yōu)槠娈惖?，這可能導致不敏變換中求矩陣平方根的數(shù)值計算問題。4.3.5內(nèi)點方法

一種具有不等式約束狀態(tài)估計的方法稱為內(nèi)點似然最大化算法

[23]。該算法基于內(nèi)點方法，但是在約束的實施方面，內(nèi)點方法從根本上不同于活動集方法。對于不等式約束，正如本章前面所討論的，活動集方法是通過解決等式約束的子問題進行處理的，然后檢查最初的約束是否滿足?；顒蛹囊粋€難點是隨著約束數(shù)量的增加，計算工作量呈指數(shù)增長。利用內(nèi)點法解決不等式約束問題時，使用牛頓法進行迭代。它也有缺陷，即隨著時步數(shù)的增長，問題呈線性增長。然而，與MHE類似，該問題可以通過限制滑動尺寸的大小來緩解。4.3.6粒子濾波方法

粒子濾波方法通過傳播許多狀態(tài)估計進行操作，之所以稱之為粒子，是因為這些估計值服從真實狀態(tài)的概率密度函數(shù)分布。廣義地說，一個UKF可以稱為一種粒子濾波器，但是UKF在幾個基本方面不同于粒子濾波器。首先，粒子濾波器的時間更新包括了噪聲的隨機產(chǎn)生，噪聲的產(chǎn)生是根據(jù)已知過程噪聲的概率密度函數(shù)產(chǎn)生的；而UKF的時間更新是確定的。其次，UKF中西格瑪點的數(shù)目是確定的，通常選擇n+1個、2n個或者2n+1個，其中n是狀態(tài)向量的維數(shù)；而一個粒子濾波器的粒子數(shù)目沒有上限，但通常隨著n的增加呈指數(shù)增加。第三，UKF中狀態(tài)向量的估計值和協(xié)方差的精度達到三階；粒子濾波器不直接估計均值和協(xié)方差，而是估計狀態(tài)的概率密度函數(shù)，當粒子數(shù)目接近無窮大時，概率密度估計值收斂于真實的概率密度。正如UKF可以被認為是一個推廣的卡爾曼濾波器一樣，粒子濾波器也可以被認為是一個推廣的UKF。給定足夠多的粒子，粒子濾波器總是表現(xiàn)得比UKF好，但是粒子濾波算法的計算復雜度很高。狀態(tài)約束粒子濾波已經(jīng)通過多種方法得以解決，例如根據(jù)粒子滿足約束的程度或者是否形成確保傳播粒子滿足約束條件的過程噪聲來修改粒子的似然函數(shù)[24]，而且已經(jīng)討論的許多方法也可能適用于約束粒子濾波，例如投影、概率密度函數(shù)截斷或者SCKF。這些方法可以應用到每個粒子或者只適用于每一次的狀態(tài)估計，從而可以獲得多種約束粒子濾波算法。4.4線性等式狀態(tài)約束條件下的粒子濾波算法

4.4.1問題描述

假定離散系統(tǒng)的動態(tài)方程和量測方程分別為

xk=f(xk－1)+vk－1

(4-87)

yk=h(xk)+nk

(4-88)

其中，f和h為狀態(tài)轉(zhuǎn)移函數(shù)和量測函數(shù)，vk和nk分別為過程噪聲和量測噪聲，它們都是零均值高斯噪聲，方差分別為Qk－1和Rk，并且過程噪聲和量測噪聲互不相關(guān)。假定狀態(tài)向量xk存在如下線性等式約束條件：

Dxk=dk

(4-89)

其中，D為已知狀態(tài)約束矩陣，dk為已知向量。從貝葉斯濾波角度來看，我們的目標是進行如下遞推計算并獲得各個時刻的狀態(tài)向量估計。首先根據(jù)0~k時刻觀測量y0:k計算狀態(tài)向量的概率密度分布，即(4-90)為了使p(xk|y0:k)滿足式(4-89)，通過對式(4-90)修正的方法實現(xiàn)。使用文獻[2]~[5]中的方法都可以實現(xiàn)對式(4-90)的修正，并能取得不錯的效果。其中文獻[5]的方法在狀態(tài)向量為高斯分布的條件下，計算過程簡單，運算量小，而且精確程度較高，因此本節(jié)選用文獻[5]中的方法對狀態(tài)向量進行修正。接下來根據(jù)狀態(tài)轉(zhuǎn)移方程(4-87)計算k+1時刻的一步預測分布(4-91)4.4.2算法描述及步驟

粒子濾波算法采用大量的粒子及其權(quán)值來表示狀態(tài)向量的后驗分布，當粒子數(shù)目無限大時，可以無限逼近狀態(tài)向量的后驗分布。令{xi0:k，wik}Ni=1表示對后驗概率密度p(x0:k|y0:k)進行了N次隨機抽樣，其中{xi0:k，i=1，…，N}表示k時刻粒子集中具有的N個粒子，wik表示第i個粒子的權(quán)值。此時，k時刻的后驗概率密度可以近似為(4-92)其中，δ是DiracDelta函數(shù)，wik是經(jīng)過歸一化處理的權(quán)值。粒子權(quán)值要根據(jù)重要性抽樣原理計算獲得。下面給出粒子權(quán)值的計算過程。假定p(x)∝π(x)是一個難于采樣的概率密度，粒子可以通過一個易于采樣的提議函數(shù)(Proposalfunction)q產(chǎn)生，即xi~q(x)，i=1，…,N。概率密度函數(shù)p(x)通過下式加權(quán)近似：(4-93)其中，是第i個粒子的歸一化權(quán)值。式(4-92)中權(quán)值計算為(4-94)由于(4-95)將式(4-95)代入式(4-94)，得(4-96)如果只考慮k時刻的狀態(tài)估計和量測，并忽略掉k時刻以前所有的狀態(tài)估計和量測對k時刻粒子權(quán)值的影響，可得(4-97)最后還需要對權(quán)值進行歸一化處理，即(4-98)若狀態(tài)向量服從高斯分布，則k時刻的狀態(tài)估計及其協(xié)方差為(4-99)下面考慮約束條件式(4-89)對狀態(tài)向量的影響。如果x表示利用約束條件修正之后的狀態(tài)估計，W表示任意對稱的正定矩陣，x表示沒有考慮約束條件之前的狀態(tài)估計，則具有狀態(tài)約束的估計問題就轉(zhuǎn)化為下列優(yōu)化問題:~^(4-100)采用拉格朗日乘子法求解式(4-100)。首先構(gòu)建式子(4-101)對式(4-101)求偏導數(shù)，并令其對各變量的偏導數(shù)等于0，即(4-102)求解方程組(4-102)，得(4-103)這種修正狀態(tài)估計的方法通常稱為投影方法，也就是把當前狀態(tài)估計向約束子空間投影。在具有狀態(tài)約束的粒子濾波算法中使用投影方法修正狀態(tài)向量估計值，可以直接利用式(4-103)修正式(4-99)，也可以直接對每一個帶有權(quán)值的粒子利用式(4-103)進行修正。因此我們獲得了兩種基于投影的具有狀態(tài)約束的粒子濾波方法。如果狀態(tài)向量服從高斯分布，則在最大化后驗概率密度函數(shù)p(x|y)意義下有W=P－1，在最小化均方誤差意義下有W=I[5]。在估計過程中，可能會出現(xiàn)粒子退化(DegeneracyProblem)現(xiàn)象。粒子退化是指經(jīng)過多次遞推計算后，大部分粒子權(quán)值都小到可以忽略不計的程度，只有少數(shù)幾個粒子權(quán)值較大。由于那些權(quán)值很小(近似為0)的粒子在濾波過程中幾乎起不到作用，粒子多樣性特征得不到保證，會嚴重影響濾波精度。為了解決該問題，通常的做法是計算等效粒子數(shù)Neff，如果等效粒子數(shù)小于某個閾值，則采用重抽樣算法對粒子重抽樣形成新的粒子集[6]。其中等效粒子數(shù)的計算公式為(4-104)

MethodA算法步驟如下：

輸入：xik－1，wik－1，Pk－1，yk；

輸出：xk，xik，wi，Pk；

Step1：Fors=1，…，N

(1)使用式(4-103)計算第s個粒子修正之后的值xsk；

(2)使用式(4-87)計算第s個粒子下一時刻的狀態(tài)預測值xsk+1|k；

(3)使用式(4-97)計算粒子權(quán)值；

EndFor

Step2：使用式(4-98)將權(quán)值歸一化；

Step3：使用式(4-99)計算k時刻的狀態(tài)估計值xk及其方差Pk；~^

Step4：使用式(4-104)計算有效粒子數(shù)，如果有效粒子數(shù)小于某個閾值，則進行重抽樣。

MethodB算法步驟如下：

輸入：xik－1，wik－1，Pk－1，yk；

輸出：xk，xik，wi，Pk；

Step1：Fors=1，…，N

(1)使用式(4-87)計算第s個粒子下一時刻的狀態(tài)預測值xsk+1|k；

(2)使用式(4-97)計算粒子權(quán)值；

EndFor

Step2:使用式(4-98)將權(quán)值歸一化；

Step3:使用式(4-99)計算k時刻的狀態(tài)估計值xk及其方差Pk;~^

Step4：使用式(4-103)計算狀態(tài)估計值xk的修正值xk；

Step5：使用式(4-104)計算有效粒子數(shù)，如果有效粒子數(shù)小于某個閾值，則進行重抽樣。

在上述算法中只考慮了狀態(tài)約束為線性的情況，對于非線性狀態(tài)約束，可以采用泰勒級數(shù)展開[5,9]，從而將非線性問題轉(zhuǎn)化為線性問題，然后再利用上述算法加以解決。^~4.4.3仿真實驗及結(jié)果分析

假設(shè)目標在x－y平面內(nèi)以原點為圓心，r=100m為半徑做圓周運動。目標運動的角速度θ=5.7deg/s，目標初始狀態(tài)向量xk=0=[ξ

ξ

ζ

ζ]T=[100

0

0

10]T。有一個傳感器對目標進行量測跟蹤，傳感器的位置為(xs，ys)=(0，0)，采樣間隔T=1s，量測方程為(4-105)···(4-106)跟蹤采用的目標狀態(tài)轉(zhuǎn)移方程為(4-107)其中，過程噪聲nk和量測噪聲

都服從零均值高斯分布，方差分別為Q=diag([2

2])和R=diag([720.012])，并且兩者不相關(guān)。狀態(tài)向量初始值x0=x0，相應協(xié)方差為P0=diag([52

12

52

12])。假定在跟蹤過程中有如下位置約束：

g(xk)=ξ2k+ζ2k=1002

(4-108)

仿真步數(shù)為120步，目標繞原點旋轉(zhuǎn)兩周。

由于式(4-108)是非線性約束，需要對其進行線性化處理。也就是首先計算g(xk)的雅可比矩陣，使其符合式(4-89)的形式，然后再使用式(4-103)計算狀態(tài)估計值的修正值。^實驗一誤差性能分析

分別采用MethodA、MethodB、GeneralPF以及文獻[5]中的EstimateProjectionEKF方法實現(xiàn)目標跟蹤。其中，參與粒子濾波的粒子數(shù)為1000，MonteCarlo仿真50次。計算不同時刻的均方根誤差(RMS)。其中位置分量的RMS計算公式為

(4-109)圖4.1(a)和圖4.1(b)表明，算法的誤差性能由高到低依次是MethodA、MethodB、EstimateProjectionEKF、GeneralPF。本節(jié)算法與普通粒子濾波算法的誤差性能相比有較大幅度提高，這是因為在濾波過程中加入了狀態(tài)約束條件，縮小了狀態(tài)估計的空間，從而提高了濾波精度；本節(jié)算法與EstimateProjectionEKF算法的誤差性能相比也有不同程度提高，這是因為本節(jié)算法本質(zhì)上還是粒子濾波算法，而粒子濾波是一種比EKF精度更高的估計方法，在使用投影方法對粒子濾波狀態(tài)向量施加有效約束后，能夠取得較好的濾波精度。由量測模型易知，量測誤差具有周期性；另一方面，跟蹤所使用的約束條件的位置分量具有周期性，從而使圖4.1中濾波誤差變化呈現(xiàn)周期性。針對這種周期性和濾波結(jié)果誤差大小的互補性，可以融合MethodA和MethodB以獲得更好的誤差性能，然而這種融合策略需要視具體問題而定，不具有一般性。此外，圖4.1表明，幾種方法的誤差變化周期有所不同，原因如下：由于狀態(tài)約束只和目標位置相關(guān)，因此各種方法的速度分量的誤差周期相同，并且也和常規(guī)PF算法的位置誤差的周期相同；而在位置分量上施加狀態(tài)約束后，由于約束分量的周期性，使位置分量的誤差周期發(fā)生改變。MethodB和EstimateProjectionEKF方法對狀態(tài)向量估計值的修正時機相同，因此其位置分量的濾波誤差周期相同；而MethodA針對每個粒子進行修正，因此該方法與其它方法的位置分量誤差周期不相同。圖4.1不同時刻的均方根誤差實驗二算法性能受粒子數(shù)影響分析

為了考察粒子數(shù)對各種粒子濾波算法誤差性能的影響，將粒子數(shù)作為粒子濾波算法的參數(shù)，粒子數(shù)依次選取為{50，100，150，200，300，…，1000}。使用不同的粒子數(shù)作為參數(shù)值參與運算，仿真120步，采用MonteCarlo方法重復執(zhí)行50次，計算使用不同粒子數(shù)后不同方法估計結(jié)果的均方根誤差(RMS)。此時，位置RMS定義為(4-110)其中，N表示仿真步數(shù)，其它參數(shù)含義與式(4-109)中的參數(shù)含義相同。實驗結(jié)果如圖4.2所示。由于EstimateProjectionEKF方法的濾波過程與粒子數(shù)的多少無關(guān)，所以圖中顯示的誤差結(jié)果近似為一條直線。圖4.2(a)和圖4.2(b)表明，盡管粒子數(shù)不相同，但是各種算法的誤差性能的優(yōu)劣和實驗一結(jié)果一致。并且各種粒子濾波算法隨著粒子數(shù)的不斷增大，其誤差不斷減小。但是當粒子數(shù)增加到200以上時，各種方法本身的誤差大小基本保持一致，也就是說當粒子數(shù)大到一定程度后算法本身的誤差性能改善很微弱，使用較多的粒子已經(jīng)很難有效改善濾波性能了。在粒子數(shù)小于200的情況下，注意到MethodA比其它方法表現(xiàn)更出色，尤其是在粒子數(shù)選取特別少(小于50)的情況下，此時其它算法的狀態(tài)估計結(jié)果可能已經(jīng)發(fā)散，而MethodA仍然能夠?qū)⒄`差保持在較小的范圍內(nèi)。這是因為MethodA在使用狀態(tài)轉(zhuǎn)移方程對粒子進行傳播之前，使用狀態(tài)約束函數(shù)將各個粒子投影到約束子空間中，粒子集在較小的空間中表征向量分布，因此在狀態(tài)轉(zhuǎn)移之后能夠更好地逼近狀態(tài)分布的后驗概率。圖4.2不同粒子數(shù)下各種粒子濾波算法的均方根誤差為了考察新算法的時間復雜度，對實驗執(zhí)行時間進行統(tǒng)計。結(jié)果表明。采用1000個粒子，MethodA執(zhí)行120步需要37秒，MethodB完成同樣的過程需要32秒，而常規(guī)粒子濾波算法需要31秒。其中仿真使用的計算機CPU(雙核)主頻為1.6GHz，內(nèi)存為1GB。可以看出本節(jié)中的兩種算法與常規(guī)粒子濾波算法相比，時間復雜度增加幅度不大。對比兩種新算法的時間復雜度，結(jié)果表明，MethodA較高的濾波精度需要較高的時間復雜度作為代價。

4.5非線性等式狀態(tài)約束條件下的濾波算法

4.5.1問題描述

離散系統(tǒng)的動態(tài)方程和量測方程分別如式(4-87)和式(4-88)所示。假定狀態(tài)向量xk存在如下非線性約束條件：

g(xk)=dk

(4-111)

其中，g為已知非線性狀態(tài)約束函數(shù)，dk是已知向量。具有狀態(tài)約束的估計問題就是在式(4-111)條件下求解式(4-87)和式(4-88)所示系統(tǒng)中各個時刻的狀態(tài)估計。對于非線性狀態(tài)約束問題，采用基于泰勒級數(shù)展開的線性化的方法存在如下問題[25,26]：①如果非線性方程的局部線性特征不滿足，則會出現(xiàn)較大的線性化誤差，會對估計結(jié)果產(chǎn)生較大影響；②線性化的過程需要計算非線性約束方程的雅可比矩陣，然而并不是所有非線性約束函數(shù)的雅可比矩陣都存在；③某些非線性系統(tǒng)的雅可比矩陣求解困難，容易引入計算誤差。在非線性濾波問題中，能夠有效避免上述問題的算法就是不敏卡爾曼濾波(UKF)。不敏卡爾曼濾波是一種基于UT變換的濾波算法。本節(jié)就是通過使用UT變換來計算狀態(tài)向量在非線性約束條件下的狀態(tài)估計及協(xié)方差，從而避免求解非線性狀態(tài)約束函數(shù)的雅可比矩陣，也就避免了上述問題。4.5.2基于UT變換的最佳量測值方法

UT變換在計算過程中使用狀態(tài)向量的二階統(tǒng)計量，是一種更為直接的非線性估計方法。一般來說，對概率密度函數(shù)做近似比對非線性函數(shù)或者變換做近似容易。UT變換采用一系列點來近似非線性函數(shù)的概率密度分布，并通過點的非線性變換來近似概率密度函數(shù)的變換。UT變換的過程如下[25-27]：首先確定若干個點(Sigmapoints，西格瑪點)來近似狀態(tài)向量的均值和方差；然后計算每一個Sigma點的非線性變換；最后根據(jù)變換后的Sigma點計算變換后的均值和方差。假定一個n維的狀態(tài)向量xk，其方差為Pk，現(xiàn)在要計算該狀態(tài)向量在非線性方程(4-111)作用下的變換均值及其協(xié)方差。首先使用下式計算狀態(tài)向量的Sigma點及其權(quán)值：^其中，上標m表示w為狀態(tài)向量的權(quán)值，上標c表示w為協(xié)方差陣的權(quán)值，i=1，…，n，λ=α2(n+κ)－n，參數(shù)α，β，κ是事先指定的參數(shù)，它們通常取值為0.5，2，3－n；(P)i表示求矩陣P第i列的值，并將其作為列向量。然后計算每個Sigma點的非線性變換，即

ζi,k=g(χi，k)，i=0，1,…，2n

(4-118)

最后計算非線性變換后的均值及相關(guān)的協(xié)方差矩陣，即本方法將非線性約束作為量測噪聲為0的最佳量測值(PerfectMeasurement，PM)看待[2,8]，并將其應用到基于UT變換的估計過程中。即若表示量測噪聲方差，則=0。根據(jù)線性最小方差估計準則[28]將式(4-119)、式(4-121)、式(4-122)代入式(4-123)和式(4-124)，得到非線性狀態(tài)約束條件下的估計值xk及其協(xié)方差Pk。~~4.5.3基點誤差降低方法

由于UT變換的誤差受狀態(tài)向量誤差的影響，其狀態(tài)估計誤差性能下降。具體表現(xiàn)為：如果基點誤差大，則狀態(tài)估計誤差大；如果基點誤差小，則狀態(tài)估計誤差小。為了降低基點誤差對估計結(jié)果的影響，在SCKF算法[10]思想的啟發(fā)下，給狀態(tài)約束施加一系列量測噪聲方差，以此來表示不同強度的非線性狀態(tài)約束。然后依次針對不同強度的非線性狀態(tài)約束下的狀態(tài)向量實施UT變換，并采用線性最小方差估計準則計算狀態(tài)估計值及其協(xié)方差矩陣。

由式(4-111)可知，如果狀態(tài)向量xk存在誤差，則計算出的非線性約束與dk之間會存在差異，為了表示這種約束計算過程中的差異，將約束值dk表達為

dk=g(xk)+μ

(4-125)為了確定弱約束方差序列的大小，本節(jié)采用和SCKF算法類似的方法。首先用UT變換所得的非線性約束值的方差乘以一個常數(shù)a，并將其作為方差初值，其中a>0，在使用中取經(jīng)驗值。在以后的迭代過程中，方差以指數(shù)形式遞減。4.5.4仿真實驗及結(jié)果分析

使用鐘擺運動估計例子[1]。已知鐘擺的狀態(tài)向量xk=[θk

ωk]T，θk表示k時刻鐘擺所處的角度，ωk表示k時刻鐘擺的角速度。則系統(tǒng)的動態(tài)方程和量測方程為其中，T表示離散化的步長，L表示鐘擺的長度，g表示重力加速度，并假定過程噪聲[vθ，k

vω,k]T和量測噪聲nk服從均值為0、方差分別為Q和R的高斯分布，并且過程噪聲和量測噪聲相互獨立。此外，由機械能守恒定律可知系統(tǒng)中存在如下非線性狀態(tài)約束:

(4-129)其中C為鐘擺系統(tǒng)具有的機械能能量常數(shù)。實驗中參數(shù)設(shè)置如下：L=1，T=0.05，g=9.81，Q=diag([0.0072

0.0072])，R=diag([0