卡爾曼濾波器及其簡(jiǎn)matlab仿真

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

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

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

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

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

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

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

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

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

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

11、矩陣V=randn(1,N); %產(chǎn)生隨機(jī)高斯分布q1=std(V);Rvv=q1.2; %Rq2=std(w);Qww=q2.2; %Qh=1; %觀測(cè)矩陣Y=25+V; %測(cè)量誤差值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('真實(shí)值','估計(jì)值','測(cè)量值&