1、卡爾曼濾波器及mat I ab代碼卡爾曼濾波器及matlab代碼編輯整理:尊敬的讀者朋友們：這里是精品文檔編輯中心，本文檔內容是由我和我的同事精心編輯整理后發布的，發布之前我們對 文中內容進行仔細校對，但是難免會有疏漏的地方，但是任然希望（卡爾曼濾波器及matlab代碼） 的內容能夠給您的工作和學習帶來便利。同時也真誠的希望收到您的建議和反饋，這將是我們進步 的源泉，前進的動力。本文可編輯可修改，如果覺得對您有幫助請收藏以便隨時查閱,最后祝您生活愉快業績進步，以 下為卡爾曼濾波器及matlab代碼的全部內容。卡爾曼濾波器及mat I ab代碼信息融合大作業一維納最速下降法濾波器，卡爾曼濾波器設

2、計及Mat lab仿真時間:2010126 專業：信息工程 班級：09030702 學號：姓名：馬志強卡爾曼濾波器及mat I ab代碼1.濾波問題淺談估計器或濾波器這一術語通常用來稱呼一個系統，設計這樣的系統是為了從含有 噪聲的數據中提取人們感興趣的，接近規定質量的信息。由于這樣一個寬目標，估計 理論應用于諸如通信、雷達、聲納、導航、地震學、生物醫學工程、金融工程等眾 多不同的領域。例如，考慮一個數字通信系統，其基本形式由發射機、信道和接收 機連接組成發射機的作用是把數字源（例如計算機）產生的0、1符號序列組成的消 息信號變換成為適合于信道上傳送的波形而由于符號間干擾和噪聲的存在，信道輸 出

3、端收到的信號是含有噪聲的或失真的發送信號。接收機的作用是，操作接收信號 并把原消息信號的一個可靠估值傳遞給系統輸岀端的某個用戶。隨著通信系統復雜 度的提高，對原消息信號的還原成為通信系統中最為重要的壞節，而噪聲是接收端 需要排除的最主要的干擾，人們也設計出了針對各種不同條件應用的濾波器，其中 最速下降算法是一種古老的最優化技術，而卡爾曼濾波器隨著應用條件的精簡成為 了普適性的高效濾波器。2.維納最速下降算法濾波器2o 1最速下降算法的基本思想考慮一個代價函數/（叨，它是某個未知向量“的連續可微分函數。函數叨將的 元素映射為實數這里，我們要尋找一個最優解叭使它滿足如下條件/（W0） /（W）（2

4、. 1）這也是無約束最優化的數學表示。特別適合于自適應濾波的一類無約束最優化算法基于局部迭代下降的算法：從某一初始猜想譏0）出發，產生一系列權向量w（l）,w（2）,，使得代價函數J）在算法的每一次迭代都是下降的，即J(w(n + 1) ；(w(n)卡爾曼濾波器及mat I ab代碼其中、心)是權向量的過去值，而Mn + 1)是其更新值。我們希望算法最終收斂到最優值叫.迭代下降的一種簡單形式是最速下降法，該方法是沿最速下降方向連續調整權向量。為方便起見，我們將梯度向量表示為9 =號0)=引3)dw(2.2)因此，最速下降法可以表示為1w(n + 1) = w(n)-尹9(n)(2o 3)其中7

5、1代表進程，是正常數，稱為步長參數，1/2因子的引入是為了數學上處理方 便在從到”+1的迭代中，權向量的調整量為1血何=w(n + 1) - w(n) =- -p.g(n)(2.4)為了證明最速下降算法滿足式(2.1)，在叭砒處進行一階泰勒展開，得到/(w(n + 1) /(w(n) + gH(7i)8w(n)(2o 5)此式對于較小時是成立的。在式(2.4)中設砂為負值向量，因而梯度向量&也為負值向量，所以使用埃爾米特轉置將式(2。4)用到式(2。5)中，得到/(w(zi + l)；(w(n) - ；|協(小|2此式表明當為正數時/3( + 1)丿3()。因此，隨著7啲增加，代價函數/(巧減

6、小, 當斤=00時,代價函數趨于最小值Js。2o 2最速下降算法應用于維納濾波器考慮一個橫向濾波器，其抽頭輸入為災)，啥-1)，皿M + 1),對應的抽頭權值 為w0(w1(n),-,u/M_1(n)o抽頭輸入是來自零均值、相關矩陣為尺的廣義平穩隨機過卡爾曼濾波器及mat I ab代碼程的抽樣值除了這些輸入外，濾波器還要一個期望響應“)，以便為最優濾波提供 一個參考在時刻與由頭輸入向量表示為以巧，濾波器輸出端期望響應的估計值為 通|必 其中幾是由抽頭輸u(n)Xn-l)/-Xn-M + 1)所張成的空間。空過比較期望 響應d)及其估計值，可以得到一個估計誤差*)，即e(n) = d(n) 3(

7、n | Un) = d(n) w(7i)u(n)(2o6)這里(n)u(n)是抽頭權向量叭巧與抽頭輸入向量班“)的內積。叭小可以進一步表示 為w(n) = vv0(n),w1(n),-,ivjV/_1(n)r同樣，抽頭輸入向量“)可表示為u(n) = u(n)u(n 一一 M + 1)卩如果抽頭輸入向量“)和期望響應“)是聯合平穩的，此時均方誤差或者在時刻71的代價函數/5)是抽頭權向量的二次函數,于是可以得到/(n)=制-wH(n)p 一 pHw(n) + wH(n)Rw(n)(2o 7)其中，為目標函數d何的方差，附由頭輸入向量2心)與期望響應心)的互相關向量, 及尺為抽頭輸入向量的相關矩

8、陣。從而梯度向量可以寫為I dJQn). 0J5) I加0何+尬oG)dJWdj(n)W(n)=麗而 + /麗而=-即 + 2Rw(n) 。/(小；引(加出1(宛)。切-1G)(2.8)刃)引)其中在列向量中而麗和喃分別是代價函數心)對應第上個抽頭權值叫何的實部似5) 和虛部如)的偏導數。對最速下降算法應用而言，假設式(2。8)中相關矩陣尺和互卡爾曼濾波器及mat I ab代碼相關向量P已知，則對于給定的抽頭權向量+1)為w(n + 1) =+ “p - Rw(ti)(2o 9)它描述了為那濾波中最速下降法的數學表達式.3. 卡爾曼濾波器3o 1卡爾曼濾波器的基本思想卡爾曼濾波器是用狀態空間概

9、念描述其數學公式的，另外新穎的特點是，他的解 遞歸運算，可以不加修改地應用于平穩和非平穩環境。尤其是，其狀態的每一次更 新估計都由前一次估計和新的輸入數據計算得到，因此只需存儲前一次估計除了不 需要存儲過去的所有觀測數據外，卡爾曼濾波計算比直接根據濾波過程中每一步所 有過去數據進行估值的方法都更加有效。1 仇)x(n +1)x(n)y(n)0=0 二1?2(72)圖3. 1線性動態離散時間系統的信號流圖表示“狀態”的概念是這種表示的基礎狀態向量，簡單地說狀態，定義為數據的最 小集合，這組數據足以唯一地描述系統的自然動態行為。換句話說，狀態由預測系統 未來特性時所素要的，與系統的過去行為有關的最

10、少的數據組成。典型地，比較有代 表性的情況是，狀態心)是未知的為了估計它，我們使用一組觀測數據，在途中用 向量y()表示。y(e成為觀測向量或者簡稱觀測值，并假設它是川維的。在數學上，圖3。1表示的信號流圖隱含著一下兩個方程：卡爾曼濾波器及mat I ab代碼x(n + 1) = F(n + l,n)x(n) + vx (n)(3O 1)式中，Mxl向量可表示噪聲過程，可建模為零均值的白噪聲過程，且其相關矩陣定義為(2)測量方程y(n) = C(n)x(n) + v2(n)(3. 2)其中“)是已知的N x M測量矩陣。N x響量巾)稱為測量噪聲，建模為零均值 的白噪聲過程，其相關矩陣為匝畑嶼

11、)=屏他”尙(3O 3)測量方程(3.2)確立了可測系統輸出y(“)與狀態心)之間的關系，如圖3.1 所示.3. 2新息過程為了求解卡爾曼濾波問題，我們將應用基于新息過程的方法根據之前所述，用 向量譏川兒)表示“ =1時亥倒T時刻所有觀測數據過去值給定的情況下，你時刻觀 測數據刃巧的最小均方估計。過去的值用觀測值y(i)(2)，,血-1)表示，他們張成的 向量空間用兒“表示。從而可以定義新息過程如下：a(n)=y(n)-y(ny1l.1)(3.4)其中x響量5)表示觀測數據刃巧的新息.3. 3應用新息過程進行狀態估計下面，我們根據信息過程導出狀態班。的最小均方估計。根據推導,這個估計可以卡爾曼

12、濾波器及mat I ab代碼表示成為新息過程班1)。(2),.皿(71)序列的線性組合，即 乞坊(S伙)k = l(3o 5)其中是一組待定的MxN矩陣。根據正交性原理，預測狀態誤差向量與新息 過程正交，即E(ifn)an(7n) =xiynaH(m = O(3o 6)將式(3。5)代入式(3.6),并利用新息過程的正交性質，即得Ex(t)aH (m) = B i(ni)Ea(m)aH(Tri) = Bi(m)R(m)(3.7)因此，式(3。刀兩邊同時右乘逆矩陣5),可得乞少)的表達式為E(m) = Ek(Z)a(m)R i(m)(3O 8)最后，將式(3。8)帶入式(3。5),可得最小軍方差

13、估計Xd yjH?l -1=Y Ex(i)aH(k)R J(k)a(k)=Ex(i)aH(k)R -】(k)a(k) + Ex(i)aH(n)R r(n)a k=lk=l(n)(3.9)故對于iz + 1,有71-1+ 1 |yn) = 丫 Ex(n + Ex(n + l)aH(n)A?_ 1(n)a(n)k = i(3O 10)然而，n + 1時刻的狀態2 + 1)與“時刻的狀態刑)的關系式由式可以推導出對于 0k1&q 2/Ryx 最大特征值hn=zeros (1, N);hn (:) =5;vg=0;Rxx=xcorr (1)；Ryx=min(min(corrcoef (1, 1+ran

14、dn)；echo offfor i=1 : N1 ；%vg=2*Rxx*hn(:, i)2*Ryx;%hn (:, i+1)二hn(:, i ) T/2*q*vg；vg二2*Rxx*hn ( i) -2*Ryx;hn(i+1)=hn (i) 1/2*q*vg;m(i)=1;endt=1:N-1 ；plot (t, hn (t), r-, tr m(t), b);卡爾曼濾波器及mat I ab代碼5. 3后聯平滑濾波器的卡爾曼濾波器clearclc；N=300;CON = 5；x = zeros (1, N);x (1)= 1;P = 10;Q = randnd.N) *0.2； %過程噪聲協方

15、差R = randn(1,N);%觀測噪聲協方差y = R + CON； %加過程噪聲的狀態輸出for k = 2 ： NQ1 = cov (Q (1: k-1) ;%過程噪聲協方差Q2 = cov (R (1:k-1)；x (k) = x (k 1 )； %預估計k時刻狀態變量的值P = P + Q1; %對應于預估值的協方差kg = p / (p + Q2) ； %ka Iman gainx(k) = x(k) + kg 決(y(k) x (k);p =(1 一 kg) * p;endFilter_Wid = 10;smooth_res = zeros(1, N)；kaI man_p =

16、zeros (1,N);卡爾曼濾波器及mat I ab代碼for i 二 Fi lter_Wid + 1 : Ntempsum 二 0；kalman_m 二 0；for j = i - Filter_Wid : i 1tempsum 二 tempsum + y(j)；kaI man_m 二 kalman_m+x(j);endkalman_p (i)二 kaIman_m/Fi Iter_Wid；smoothes (i)= tempsum / Fi lter_Wid;%平滑濾波end% figured)；% hist (y)；t=1： N;figure (1);expVaIue 二 zeros (1, N)；for i = 1 ： NexpVaIue (i)= CON；endplot(t, expVa I ue, ? r5y,