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