常規(guī)方位角算法理論基礎(chǔ)綜述 1 11.2卡爾曼濾波算法 2 2 2 4 7 81.3.1UT變換的基本原理 8 91.4粒子濾波算法 1 11.4.2非線性系統(tǒng)的貝葉斯估計(jì) 1.4.5濾波過程 1960年Kalman提出卡爾曼濾波算法，在線性的高斯過程系統(tǒng)中，即目標(biāo)狀態(tài)和觀測方程均呈線性時(shí)，卡爾曼濾波算法提供了最優(yōu)解。但是在現(xiàn)實(shí)的工程實(shí)踐中，大多數(shù)的觀測方程是非線性的，而卡爾曼濾波適用于線性高斯過程，將它用在非線性系統(tǒng)中會(huì)產(chǎn)生較大的誤差甚至?xí)?dǎo)致發(fā)散，這在工程上是不希望發(fā)生的。后來，學(xué)者們就一直在關(guān)注卡爾曼濾波在非線性系統(tǒng)應(yīng)用的問題，Y.Sunahara、K.Yamashita和R.S.Bucy、K.D.Senne等人創(chuàng)造了擴(kuò)展卡爾曼濾波(EKF)算法，EKF解決的KF在非線性系統(tǒng)中應(yīng)用的問題，但EKF也僅局限于應(yīng)用在高斯噪聲過程的系統(tǒng)，EKF將非線性系統(tǒng)一階線性化并忽略其高階項(xiàng)再利用KF進(jìn)行濾波。在2000年Julier等人提出的無跡卡爾曼濾波(UKF)算法，UKF主要利用無跡變換來處理均值和協(xié)方差，從而解決系統(tǒng)非線性所帶來的問題。線性的高斯系統(tǒng)都有人研究了，但非高斯過程的系統(tǒng)還是很難處理，直到Gordon等人提出粒子濾波(PF)算法，粒子濾波算法應(yīng)用的范圍不再局限于高斯過程的系1.2卡爾曼濾波算法應(yīng)用KF的系統(tǒng)噪聲是高斯分布的，且它的狀態(tài)方程和觀測方程必須滿足線用數(shù)學(xué)模型表達(dá)如下輸出的狀態(tài)；H(k+1)是一個(gè)p×n階的觀測矩陣；W(k),V(k+1)表示具有均值為零的獨(dú)立隨機(jī)過程，即高斯白噪聲，且對(duì)Vk,j滿足式中，Q為過噪聲方差；R為觀測噪聲方差；δ是狄拉克函數(shù)；從式(2-5)(1)狀態(tài)一步預(yù)測(2)預(yù)測的協(xié)方差矩陣P(k+1|k)=F(k)P(k|k)F(k)+TQrT(3)卡爾曼增益(4)狀態(tài)更新X(k+1|k+1)=X(k+1|k)+K(k+1)[Z((5)誤差協(xié)方差矩陣更新P(k+1|k+1)=[In-K(k+1H]P(kK(k+1)為最優(yōu)濾波增益，X(k+1|k+1)為最佳濾波值，P(k+1|k+1)計(jì)協(xié)方差矩陣，In是一個(gè)n×m的單位矩陣。上述遞推過程的具體過程可用圖錯(cuò)差X(k+1|k+1)=X(k+1|k)+K(k+1)[Z(k+1-HX(k+1|k))]結(jié)束將X(k+1)代入圖錯(cuò)誤!文檔中沒有指定樣式的文字。.1KF濾波算法流程框圖其觀測模型為速度為s(k),其加速度為a(k),則有在忽略目標(biāo)自身的動(dòng)力因素的情況下，a(k)即隨機(jī)加速度w(k),w(k)是由對(duì)應(yīng)系統(tǒng)狀態(tài)模型式(2-1)和(2-2)可得假設(shè)目標(biāo)在二維的坐標(biāo)系的海面上運(yùn)動(dòng)，目標(biāo)的初始位置為(10m,-100m),初始速度為(5m/s,-10m/s),采樣周期為T=1s,采樣頻率為f=80Hz,觀測噪聲為均值為零，方差為200;過程噪聲方差取wl=0.001,w2=0.1,w3=1三種情況。式中，目標(biāo)真實(shí)位置為(x,y),目標(biāo)觀測位置為(x,y),N為采樣次數(shù)。為w=0.001時(shí)目標(biāo)跟蹤的效果圖，從圖中可看出卡爾曼濾波的軌跡接近真實(shí)軌跡，其觀測的點(diǎn)也大多分散在真實(shí)軌跡那條直線的周圍，驗(yàn)證了KF算法的估計(jì)值與真實(shí)值的正確性。圖錯(cuò)誤!文檔中沒有指定樣式的文字實(shí)軌跡漸漸彎曲；圖錯(cuò)誤!文檔中沒有指定樣式的文字。.7是過程噪聲方差取wl=0.001,w2=0.1,w3=1時(shí)，其目標(biāo)跟蹤的誤差對(duì)比圖，從圖中可看出過程噪1.2.4擴(kuò)展卡爾曼濾波算法上一節(jié)對(duì)KF的目標(biāo)跟蹤和影響其濾波的因素，這為EKF奠定了基礎(chǔ)。線性系統(tǒng)是KF的作用環(huán)境，對(duì)于非線性系統(tǒng)的濾波問題，后來學(xué)者們提出了擴(kuò)展卡爾曼算法，EKF將非線性系統(tǒng)問題轉(zhuǎn)化為近似線性的問題。EKF濾波主要的余項(xiàng)忽略掉，那么剩下的一階的部分就可看似一個(gè)線性函數(shù)，然后用KF進(jìn)行濾波。EKF和KF的濾波過程及其相似。EKF濾波主要處理的是過程噪聲為高斯白噪聲的非線性系統(tǒng)，假設(shè)離散非線性系統(tǒng)的動(dòng)態(tài)方程表示為式(2-24)為狀態(tài)方程，(2-25)為觀測方程，X(k+1)表示目標(biāo)的第k+1時(shí)V(k)是均值為零，方差為R(k)的觀測噪聲，這里Q(k)和R(k)相互獨(dú)立。將式(2-24)和(2-25)中的非線性函數(shù)f()和h()對(duì)X(k)進(jìn)行一階Tayor展開得其中，是對(duì)非線性函數(shù)f()求偏導(dǎo)數(shù)后所得到的雅克比擴(kuò)展Kalman濾波遞推過程和Kalman濾波基本過程大體上差不多，不同的是，擴(kuò)展卡爾曼濾波在線性化后的系統(tǒng)中的F(k+1|k)和H(k+1)是對(duì)非線性函數(shù)f和h求偏導(dǎo)得到的雅克比矩陣表示。1.3無跡卡爾曼濾波算法值、協(xié)方差相等的特點(diǎn)，這些點(diǎn)可通過一定的運(yùn)算求得變換后的均值和協(xié)方差。UT變換使其均值和協(xié)方差的精度有了一定提高，其精度最高可達(dá)3階泰勒級(jí)數(shù)。1.3.1UT變換的基本原理為了說明UT變換的基本過程，假設(shè)在均值x和方差P都已知的情況下，非(2)采樣點(diǎn)權(quán)值的計(jì)算為均值；λ為伸縮因子，且λ=α2(n+k)-n,α確定X周圍Sigma點(diǎn)的分布，一般是一個(gè)較小的正數(shù)；β是非負(fù)的權(quán)系數(shù)，在高斯分布的情況下，β=2是最優(yōu)的；k為待選參數(shù)，可以是任意值，但要使(UKF也是對(duì)非線性高斯過程濾波的算法，對(duì)于UKF的動(dòng)態(tài)方程可表示為是方差為Q(x)的過程噪聲；V(k)是方差為R(k)的觀測噪聲，Q(k)和R(k)是互不(1)狀態(tài)初始化(2)利用(2-28)和(2-29)進(jìn)行UT變換從而獲得采樣點(diǎn)和采樣的權(quán)值。ξ⑥ξ⑥(k|k)=[X(k|k)X(k|k)+√(n+λ)P(k|k(3)利用獲得的采樣點(diǎn)集進(jìn)行進(jìn)一步預(yù)測(4)預(yù)測均值和協(xié)方差矩陣(5)將一步預(yù)測值，再次UT變換，產(chǎn)生新的采樣點(diǎn)集Z?(k+1|k)=h[ξ⑦(k+1|k)](7)計(jì)算預(yù)測均值和協(xié)方差(8)計(jì)算增益矩陣(9)進(jìn)行濾波更新，即狀態(tài)更新和協(xié)方差更新X(k+1|k+1)=X(K+1|k)+K||1.4粒子濾波算法出，該算法是針對(duì)非線性非高斯系統(tǒng)所提出的一種濾波方法，PF常用非線性非用粒子集代表概率，其分布情況是用其后{x:i=1,2…,N},δ(dXk)狄拉克函數(shù)。其狀態(tài)序列函數(shù)8k為此時(shí)，其粒子集X是獨(dú)立分布的，由大數(shù)定律可知，當(dāng)樣本數(shù)N趨于無窮由于，式(2-44)中的后驗(yàn)概率不易獲取，一般從已知的重要性分布函數(shù)E[8(Xak]=?8(Xak)p(XokI式中，X2是從參考分布q(XokIZ:)采樣得到的樣本集，w.(X?2)歸一化權(quán)值。其中為其中，X(k)表示系統(tǒng)狀態(tài)，W(k),V(k)為過程是相互獨(dú)立的，對(duì)于映射函數(shù)fh其可表現(xiàn)為線性關(guān)系，也可表現(xiàn)為非線性關(guān)令系統(tǒng)狀態(tài)變量為Xok={x?,x?,…xk},觀測值為Zk={20,z,…zk};其過程噪聲W(k)和觀測噪聲V(k)是互不相關(guān)的，且它們都知其概率密度分布情況，則將式(2-52)代入式(2-48)得式中，X?是從重要性分布函數(shù)q(X?k-1|Zk-1)取得的樣本，X是從重要性分布函數(shù)q(X?:kIX?:k-1,Z1.)中獲得的樣本點(diǎn)，由此可推出根據(jù)參考文獻(xiàn)錯(cuò)誤!未找到引用源?？芍话銓⑾闰?yàn)密度作為其重要性分布函數(shù)，即q(X:kIX?.k-1,Z:)=p(XkIXk-則其重要性權(quán)值可表示為權(quán)重計(jì)算是PF算法過程中最為關(guān)鍵的部分，粒子濾波根據(jù)權(quán)重的大小將貢獻(xiàn)度大的粒子進(jìn)行大量的復(fù)制，將貢獻(xiàn)度小的粒子給過濾掉，假設(shè)粒子濾波的粒子集合為Xse={x?,x?,…xn},其粒子的做加權(quán)平均值，其濾波的結(jié)果為：在濾波過程中，其權(quán)值的計(jì)算步驟為：(1)將運(yùn)動(dòng)目標(biāo)的第k-1時(shí)刻狀態(tài)X'(k)的粒子代入式(2-41),求其一步預(yù)測值Xpre(k),其中i=1,2,…,N.(2)將所求得的預(yù)測值Xpme(k),它是一個(gè)集合，將該集合中的值代入式(2-42)中，從而獲得預(yù)測的觀測值Zpne(k).(3)衡量每個(gè)粒子的權(quán)重(4)高斯分布的標(biāo)準(zhǔn)類型為(5)歸一化重要性權(quán)值近于0,這些粒子權(quán)重小，對(duì)后驗(yàn)估計(jì)的貢獻(xiàn)度是很小