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

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

考慮系統(tǒng)模型

xk+1=Fxk+ωk

(4-1)

yk=Hxk+υk

(4-2)

其中，xk是k時(shí)刻的狀態(tài)值，yk是k時(shí)刻的量測(cè)值，ωk和υk是均值為零協(xié)方差分別為Q和R的過(guò)程噪聲和量測(cè)噪聲，F(xiàn)和H是狀態(tài)轉(zhuǎn)移矩陣和量測(cè)矩陣。Kalman濾波器是上世紀(jì)中葉由幾個(gè)不同研究者分別獨(dú)立提出的[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)當(dāng)ωk和υk為不相關(guān)的高斯白噪聲序列時(shí)，Kalman濾波器是最小方差濾波器，而且每一時(shí)刻估計(jì)誤差協(xié)方差的跡也是最小的。但當(dāng)ωk和υk是非高斯時(shí)，Kalman濾波器是最小方差線性濾波器，也存在一些其它的非線性濾波器會(huì)表現(xiàn)出的更好的性能，例如文獻(xiàn)[12]針對(duì)量測(cè)噪聲為均勻分布時(shí)所提出的方法。當(dāng)ωk和υk是相關(guān)或有色的噪聲序列時(shí)，式(4-3)~式(4-7)可以被修正，并以此來(lái)獲得最小方差濾波器[2]?，F(xiàn)在假定系統(tǒng)滿足等式約束

Dxk=d

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

Dxk≤d

(4-9)

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

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

在式(4-8)中的等式約束可以用降低系統(tǒng)模型參數(shù)的階數(shù)來(lái)解決[13]。舉一個(gè)例子，考慮系統(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)可以被寫(xiě)為(4-18)(4-19)該例說(shuō)明如何將一個(gè)等式約束濾波問(wèn)題轉(zhuǎn)化為一個(gè)等價(jià)的但無(wú)約束的濾波問(wèn)題。對(duì)于無(wú)約束系統(tǒng)，Kalman濾波器是最優(yōu)線性估計(jì)，同時(shí)它也是最初的受約束系統(tǒng)的最優(yōu)線性估計(jì)。降階后模型的維數(shù)低于最初模型，因此減少了Kalman濾波器的計(jì)算量。這個(gè)方法的一個(gè)缺點(diǎn)是狀態(tài)變量的物理意義會(huì)丟失，另一個(gè)缺點(diǎn)是它不能直接地應(yīng)用于式(4-9)中的不等式約束。4.2.2最佳量測(cè)

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

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

Dx=d

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

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

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

在概率密度函數(shù)(ProbabilityDensityFunction，PDF)截?cái)喾椒ㄖ?，通過(guò)Kalman濾波計(jì)算狀態(tài)估計(jì)的PDF，假定它是高斯的，而且在約束邊界截?cái)郟DF，該約束狀態(tài)估計(jì)等價(jià)于截?cái)嗟腜DF的均值。設(shè)計(jì)該方法的最初目的是解決不等式約束，對(duì)其進(jìn)行稍加改變后可以應(yīng)用到等式約束問(wèn)題中。

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

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

xk+1=Fxk+ωk

(4-46)

Dxk=d

(4-47)

同樣可以假設(shè)無(wú)噪聲系統(tǒng)也滿足上式約束條件，即DFxk=0。但是這種結(jié)果意味著Dωk=0。如果沒(méi)有滿足上述方程，那么噪聲與狀態(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是行滿秩的。舉一個(gè)簡(jiǎn)單例子，考慮式(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)成立。

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

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

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

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

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

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

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

g(xk)=b

(4-60)

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

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

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

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

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

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

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

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

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

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

xk+1=f(xk)+ωk

(4-75)

yk=h(xk)+υk

(4-76)

g(xk)=0

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

g({xk})=0

(4-79)

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

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

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

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

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

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

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

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

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

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

4.4.1問(wèn)題描述

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

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

(4-87)

yk=h(xk)+nk

(4-88)

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

Dxk=dk

(4-89)

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

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

MethodA算法步驟如下：

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

輸出：xk，xik，wi，Pk；

Step1：Fors=1，…，N

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

(2)使用式(4-87)計(jì)算第s個(gè)粒子下一時(shí)刻的狀態(tài)預(yù)測(cè)值xsk+1|k；

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

EndFor

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

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

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

MethodB算法步驟如下：

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

輸出：xk，xik，wi，Pk；

Step1：Fors=1，…，N

(1)使用式(4-87)計(jì)算第s個(gè)粒子下一時(shí)刻的狀態(tài)預(yù)測(cè)值xsk+1|k；

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

EndFor

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

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

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

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

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

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

ξ

ζ

ζ]T=[100

0

0

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

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

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

12

52

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

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

(4-108)

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

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

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

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

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

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

4.5.1問(wèn)題描述

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

g(xk)=dk

(4-111)

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

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

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

(4-118)

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

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

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

dk=g(xk)+μ

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

使用鐘擺運(yùn)動(dòng)估計(jì)例子[1]。已知鐘擺的狀態(tài)向量xk=[θk

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

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

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

0.0072])，R=diag([0