卡爾曼濾波器及其簡matlab仿真

1、卡爾曼濾波器及其簡matlab仿真一、 卡爾曼濾波的起源談到信號的分析與處理，就離不開濾波兩個字。通常，信號的頻譜處于有限的頻率范圍內(nèi)，而噪聲的頻譜那么散布在很廣的頻率范圍內(nèi)，為了消除噪聲，可以進行頻域濾波。但在許多應(yīng)用場合，需要直接進行時域濾波，從帶噪聲的信號中提取有用信號。雖然這樣的過程其實也算是對信號的濾波，但其所依據(jù)的理論，即針對隨機信號的估計理論，是自成體系的。人們對于隨機信號干擾下的有用信號不能“確知，只能“估計。為了“估計，要事先確定某種準那么以評定估計的好壞程度。1960年卡爾曼發(fā)表了用遞歸方法解決離散數(shù)據(jù)線性濾波問題的論文A New Approach

2、60;to Linear Filtering and Prediction Problems線性濾波與預(yù)測問題的新方法，在這篇文章里一種克服了維納濾波缺點的新方法被提出來，這就是我們今天稱之為卡爾曼濾波的方法。卡爾曼濾波應(yīng)用廣泛且功能強大，它可以估計信號的過去和當(dāng)前狀態(tài)甚至能估計將來的狀態(tài)即使并不知道模型確實切性質(zhì)。 其根本思想是以最小均方誤差為最正確估計準那么，采用信號與噪聲的狀態(tài)空間模型利用前一時刻的估計值和當(dāng)前時刻的觀測值來更新對狀態(tài)變量的估計，求出當(dāng)前時刻的估計值。算法根據(jù)建立的系統(tǒng)方程和觀測方程對需要處理的信號做出滿足最小均

3、方誤差的估計。 對于解決很大局部的問題，它是最優(yōu)，效率最高甚至是最有用的。它的廣泛應(yīng)用已經(jīng)超過30年，包括機器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達系統(tǒng)以及導(dǎo)彈追蹤等等。近年來更被應(yīng)用于計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。卡爾曼濾波不要求保存過去的測量數(shù)據(jù)，當(dāng)新的數(shù)據(jù)到來時，根據(jù)新的數(shù)據(jù)和前一時刻的儲值的估計，借助于系統(tǒng)本身的狀態(tài)轉(zhuǎn)移方程，按照一套遞推公式，即可算出新的估值?？柭f推算法大大減少了濾波裝置的存儲量和計算量，并且突破了平穩(wěn)隨機過程的限制，使卡爾曼濾波器適用于對時變信號的實時處理。二、 卡爾曼濾波的原理卡爾曼濾波思想的來源是在海圖作業(yè)中，

4、航海長通常以前一時刻的船位為基準，根據(jù)航向、船速和海流等一系列因素推算下一個船位，但是他并不輕易認為船位就一定在推算船位上，還要選擇適當(dāng)?shù)姆椒ǎㄟ^儀器得到另一個推算船位。觀測和推算這兩個船位一般不重合，航海長需要通過分析和判斷選擇一個可靠的船位，作為船艦當(dāng)前的位置。就是以現(xiàn)時刻的最正確估計為在前一時刻的最正確估計的根底上根據(jù)現(xiàn)時刻的觀測值作線性修正?？柭鼮V波在數(shù)學(xué)上是一種線性最小方差統(tǒng)計估算方法，它是通過處理一系列帶有誤差的實際測量數(shù)據(jù)而得到物理參數(shù)的最正確估算。其實質(zhì)要解決的問題是要尋找在最小均方誤差下的估計值。它的特點是可以用遞推的方法計算，其所需數(shù)據(jù)存儲量較小，便于進行實時處理。具體

5、來說，卡爾曼濾波就是要用預(yù)測方程和測量方程對系統(tǒng)狀態(tài)進行估計。設(shè)動態(tài)系統(tǒng)的狀態(tài)方程和測量方程分別為： 上兩式子中，是k時刻的系統(tǒng)狀態(tài)，和是k-1時刻到k時刻的狀態(tài)轉(zhuǎn)移矩陣，是k時刻的測量值，是測量系統(tǒng)的參數(shù)，和分別表示過程和測量的噪聲，他們被假設(shè)成高斯白噪聲。如果被估計狀態(tài)和觀測量是滿足上述第一式，系統(tǒng)過程噪聲和觀測噪聲滿足第二式的假設(shè)，k時刻的觀測的估計可按下述方程求解。狀態(tài)的一步預(yù)測： (1) 均方誤差進一步預(yù)測： (2) 濾波增益矩陣： (3) 濾波估計方程： (4) 均方誤差更新矩陣K時刻的最優(yōu)均方誤差： (5)上述就是卡爾曼濾波器的5條根本公式，只有給定初值和，根據(jù)k時刻的觀測值，就

6、可以遞推計算得k時刻的狀態(tài)估計。下面論述卡爾曼五個公式的推導(dǎo)過程：設(shè)系統(tǒng) 1 k>=1 2其中,動態(tài)噪聲與量測噪聲是互不相關(guān)的零均值白噪聲序列，對任意k,j其根本統(tǒng)計性質(zhì)為:E = 0 Cov(,)=E=E = 0 Cov(,)=E=Cov(,)=E=0其中是克羅內(nèi)克函數(shù)，即：又設(shè)初始狀態(tài)的統(tǒng)計特征為Ex0=0 Var x0 = E(x0-0)( x0-0)T=P0且x0與，都不相關(guān)，即Cov(,)=0Cov(,)=0在量測k-1次之后，已經(jīng)有一個的估計值，要推測k狀態(tài)的狀態(tài)值，因為E = 0，可定義為由k-1次量測值所估計值的一步預(yù)測合理數(shù)值，即 3同樣，考慮到E = 0，因而量測的期

7、望值為也是適宜的?？紤]到這兩點以后利用第k次的量測數(shù)據(jù)來估計的遞推形式，其應(yīng)該為： 4這里的是一個待定的增益矩陣，其應(yīng)使誤差矩陣極小。接下來推導(dǎo)誤差方差公式。定義 5 6其中“表示誤差，式5表示先驗沒有測量值的誤差，式6表示后驗的誤差經(jīng)測量值校正。那么，根據(jù)式6，將式4，式5帶入，得估計誤差矩陣 7定義 8這就是一步預(yù)測誤差矩陣。由模型的統(tǒng)計性質(zhì)可知E= 9而且，預(yù)測誤差與測量噪聲不相關(guān)，即： 10將式8，式9，式10帶入式7，得 11現(xiàn)尋找一個使式11最小。將式11右端展開后，加減同一項再把有關(guān)的項歸在平方項里，即 12在式12中，欲使極小，那么13此時14這就是誤差迭代公式。由此可見，卡爾

8、曼濾波的遞推公式即可得到濾波的估計值，又可得到誤差的方差陣。由式3兩邊同時減去得： (15)將式1，式5帶入式15因此 20式20即一步預(yù)測估計誤差矩陣。到此，卡爾曼濾波公式推導(dǎo)完畢。三、 卡爾曼濾波的實例以溫度檢測濾波為例假設(shè)我們要研究一個房間的溫度，這個房間的真實溫度是25度，以一分鐘為時間單位。根據(jù)我們的經(jīng)驗判斷，這個房間的溫度是恒定的A=1，但是對我們的經(jīng)驗不是完全相信，可能存在上下幾度的偏差，我們把該偏差看做是高斯白噪聲系統(tǒng)噪聲W 。另外，我們在房間里放一個溫度計H=1，溫度計也不準確，測量值會與實際值存在偏差，我們也把這偏差看做是高斯白噪聲測量噪聲V ?，F(xiàn)在我們用卡爾曼濾波，過濾掉

9、噪聲，估算出房間的實際溫度。 系統(tǒng)參數(shù)名稱解釋如下：xk系統(tǒng)狀態(tài)實際溫度系統(tǒng)矩陣溫度不變，為1B、uk狀態(tài)的控制量無控制量，為0Zk觀測值溫度計讀數(shù)H觀測矩陣直接讀出，為1wk過程噪聲溫度變化偏差，常量1e-1vk測量噪聲讀數(shù)誤差，常量1e-6假設(shè)我們要估算 2 時刻房間的實際溫度值。首先你要根據(jù) 1 時刻溫度的估計值(就假設(shè)為1，來算出2 時刻溫度的估計值，即： =1 *公式1，然后由給出的1時刻的均方差 假設(shè)為10進一步更新均方差，有 = 10.009*公式2。 有方差 之后，根據(jù)卡爾曼增益方程計算出增益： =0.925 *公式3。 現(xiàn)在，我們可以通過增益和測量值Z= 26.676，溫度計

10、有測量誤差來估計2時刻的溫度了， = 24.763*公式4。 得到了2時刻的估計溫度，下一步就是對3時刻的溫度值進行最優(yōu)估算，需要得到K時刻的最優(yōu)溫度24.56的偏差，算法如下： = 0.751*公式5 就這樣，再進入3時刻的濾波循環(huán)，卡爾曼濾波器就不斷的把均方誤差遞歸，從而估算出最優(yōu)的溫度值，運行速度快。以下是matlab程序：clear all;clc;close all;%系統(tǒng)方程 X(k)=AX(k-1)+BU(k)+W(k)%系統(tǒng)測量值 Z(k)=HX(k)+V(k)N=200;w=0.1*randn(1,N); %產(chǎn)生隨機高斯分布x(1)=0; %賦值與否無所謂a=1; %系統(tǒng)狀態(tài)

11、矩陣V=randn(1,N); %產(chǎn)生隨機高斯分布q1=std(V);Rvv=q1.2; %Rq2=std(w);Qww=q2.2; %Qh=1; %觀測矩陣Y=25+V; %測量誤差值p(1)=10;X(1)=0;% kalman filter %x(k)=x(k|k-1),X(k)=x(k|k),p1(k)=p(k|k-1),p(k)=p(k|k)for k=2:N;x(k)=a*X(k-1); % X(k|k-1)=A X(k-1|k-1)+B U(k) (1)p1(k)=a*p(k-1)*a'+Qww; % P(k|k-1)=A P(k-1|k-1) A+Q (2)Kg(k)=

12、p1(k)*h'/(h*p1(k)*h'+Rvv); % Kg(k)= P(k|k-1) H / (H P(k|k-1) H + R) (4)X(k)=x(k)+Kg(k)*(Y(k)-h*x(k); % X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1) (3)p(k)=p1(k)-Kg(k)*h*p1(k); % P(k|k)=I-Kg(k) HP(k|k-1) (5)endfor i = 1: N expValue(i) = 25; end k=1:N;plot(k,X,'r',k,Y,'g',k,expValue,'b');legend('真實值','估計值','測量值&