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

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

2、Filtering and Prediction Problems （線性濾波與預測問題的新方法），在這篇文章里一種克服了維納濾波缺點的新方法被提出來，這就是我們今天稱之為卡爾曼濾波的方法?？柭鼮V波應用廣泛且功能強大，它可以估計信號的過去和當前狀態(tài)甚至能估計將來的狀態(tài)即使并不知道模型的確切性質。其基本思想是以最小均方誤差為最佳估計準則，采用信號與噪聲的狀態(tài)空間模型利用前一時刻的估計值和當前時刻的觀測值來更新對狀態(tài)變量的估計，求出當前時刻的估計值。算法根據建立的系統(tǒng)方程和觀測方程對需要處理的信號做出滿足最小均方誤差的估計。對于解決很大部分的問題，它是最優(yōu)，效率最高甚至是最有用的。它的廣泛應用已

3、經超過30年，包括機器人導航，控制，傳感器數據融合甚至在軍事方面的雷達系統(tǒng)以及導彈追蹤等等。近年來更被應用于計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等?？柭鼮V波不要求保存過去的測量數據，當新的數據到來時，根據新的數據和前一時刻的儲值的估計，借助于系統(tǒng)本身的狀態(tài)轉移方程，按照一套遞推公式，即可算出新的估值。卡爾曼遞推算法大大減少了濾波裝置的存儲量和計算量，并且突破了平穩(wěn)隨機過程的限制，使卡爾曼濾波器適用于對時變信號的實時處理。 二、 卡爾曼濾波的原理卡爾曼濾波思想的來源是在海圖作業(yè)中，航海長通常以前一時刻的船位為基準，根據航向、船速和海流等一系列因素推算下一個船位，但是他并不輕易

4、認為船位就一定在推算船位上，還要選擇適當的方法，通過儀器得到另一個推算船位。觀測和推算這兩個船位一般不重合，航海長需要通過分析和判斷選擇一個可靠的船位，作為船艦當前的位置。就是以現時刻的最佳估計為在前一時刻的最佳估計的基礎上根據現時刻的觀測值作線性修正?？柭鼮V波在數學上是一種線性最小方差統(tǒng)計估算方法，它是通過處理一系列帶有誤差的實際測量數據而得到物理參數的最佳估算。其實質要解決的問題是要尋找在最小均方誤差下K X 的估計值K X 。它的特點是可以用遞推的方法計算K X ，其所需數據存儲量較小，便于進行實時處理。具體來說，卡爾曼濾波就是要用預測方程和測量方程對系統(tǒng)狀態(tài)進行估計。設動態(tài)系統(tǒng)的狀態(tài)

5、方程和測量方程分別為：11, 11, -+=K K K K K K K W X XK K K K V X H Z +=上兩式子中，K X 是k 時刻的系統(tǒng)狀態(tài)，1, -K K 和1, -K K 是k-1時刻到k 時刻的狀態(tài)轉移矩陣，K Z 是k 時刻的測量值，K H 是測量系統(tǒng)的參數，K W 和K V 分別表示過程和測量的噪聲，他們被假設成高斯白噪聲。如果被估計狀態(tài)和觀測量是滿足上述第一式，系統(tǒng)過程噪聲和觀測噪聲滿足第二式的假設，k 時刻的觀測K X 的估計X 可按下述方程求解。狀態(tài)的一步預測： (1均方誤差進一步預測： (2 濾波增益矩陣： (3 濾波估計方程： (4 均方誤差更新矩陣（K

6、時刻的最優(yōu)均方誤差）： (5 上述就是卡爾曼濾波器的5條基本公式，只有給定初值0X 和0P ，根據k 時刻的觀測值K Z ，就可以遞推計算得k 時刻的狀態(tài)估計K X 。/1, 11k k k k k x x -=*/1, 11, 1111TTk k k k k k k k k k P P Q -=*+1/1/1T T k k k k k k k k K P H H P H R -=*+/1/1k k k k k k k x x K Z H x-=+-*/1k k k k P I K H P -=-*下面論述卡爾曼五個公式的推導過程：設系統(tǒng)1111, -+=k k k k k k x x （1）

7、k k k k v x H z += k>=1 （2）其中, 動態(tài)噪聲k 與量測噪聲k v 是互不相關的零均值白噪聲序列，對任意k,j 其基本統(tǒng)計性質為:Ek = 0 Cov(k , j =Ek Tj =kj k Q Ek v = 0 Cov(k v , j v =Ek v T j v =kj k R Cov(k , j v =Ek Tj v =0其中kj 是克羅內克函數，即： 又設初始狀態(tài)的統(tǒng)計特征為 Ex 0=Var x0 = E(x 0-( x0- T =P0且x 0與k ，k v 都不相關，即 Cov(0x , k =0 Cov(0x , k v =0在量測（k-1）次之后，已經

8、有一個1/11-=k k k x x的估計值，要推測k 狀態(tài)的狀態(tài)值，因為E1-k = 0，可定義1/-k k x 為由k-1次量測值所估計值1-k x 的一步預測合理數值，即11, 1/-=k k k k k x x （3）同樣，考慮到Ek v = 0，因而量測的期望值為1-k k x H 也是合適的。考慮到這兩點以后利用第k 次的量測數據k z 來估計k x 的遞推形式，其應該為：(1/1/-+=k k k k k k k k k k x H z K x x x （4）這里的k K 是一個待定的增益矩陣，其應使誤差矩陣極小。 接下來推導誤差方差公式。定義k k k k k x xx -=-

9、1/1/ （5） k k k k k k x xx x -=/ （6） 其中“”表示誤差，式（5）表示先驗（沒有測量值）的誤差，式（6）表示后驗的誤差（經測量值校正）。則，根據式（6），將式（4），式（5）帶入，得k k k k k k k k k k k k k k k k x k k k k k k k k k k k k k k k k k k v K x H K I x x H K x H K v K x H K x x x x H v x H K x x xx +-=-+=-+=-=-1/1/1/1/1/ ( (估計誤差矩陣( ( (1/1/1/T k k T k k T k k k

10、 k T k T k T k k T k k k k k k T k k k K v H K I x v K K v H K I x x H K I E x x E P +-+-=-（7）定義1/1/1/T k k k k k k x x E P -= （8）這就是一步預測誤差矩陣。由模型的統(tǒng)計性質可知Ek v T k v =k R （9） 而且，預測誤差與測量噪聲不相關，即：01/1/=-T k k k T k k k x v E v x E （10） 將式（8），式（9），式（10）帶入式（7），得T kk k T k k k k k k k K R K H K I P H K I P +

11、-=- ( (1/ （11）現尋找一個k K 使式（11）最小。將式（11）右端展開后，加減同一項1/11/1/ (-+k k k k Tk k k k T k k k P H R H P H H P再把有關k K 的項歸在平方項里，即( ( ( (11/1/1/11/1/1/11/1/1/-+-+-+-=k T k k k k T k k k k k k k k k k T k k k k T k k k k k k k k Tk k k k T k k k k k k R H P H H P K R H P H R H P H H P K P H R H P H H P P P（12）在式

12、（12）中，欲使k P 極小，則11/1/ (-+=k T k k k k T k k k k R H P H H P K （13）此時1/1/11/1/1/ (-=+-=k k k k k k k k T k k k k T k k k k k k P H K I P H R H P H H P P P （14）這就是誤差迭代公式。由此可見，卡爾曼濾波的遞推公式即可得到濾波的估計值，又可得到誤差的方差陣。由式（3）兩邊同時減去k x 得：k k k k k k k x x x x -=-11, 1/- (15將式（1），式（5）帶入式（15）1111, 1/-=k k k k k k k x

13、 x 因此T k k k T k k k k k k k Q P P 1111, 11, 1/-+= （20）式（20）即一步預測估計誤差矩陣。 到此，卡爾曼濾波公式推導完畢。三、 卡爾曼濾波的實例以溫度檢測濾波為例假設我們要研究一個房間的溫度，這個房間的真實溫度是25度，以一分鐘為時間單位。根據我們的經驗判斷，這個房間的溫度是恒定的（A=1），但是對我們的經驗不是完全相信， 可能存在上下幾度的偏差，我們把該偏差看做是高斯白噪 聲（系統(tǒng)噪聲 W ） 。另外，我們在房間里放一個溫度計（H=1） ，溫度計也不準確， 測量值會與實際值存在偏差， 我們也把這偏差看做是高斯白噪聲 （測量噪聲 V ） 。

14、 現在我們用卡爾曼濾波，過濾掉噪聲，估算出房間的實際溫度。 系統(tǒng)參數名稱 解釋如下： xk 系統(tǒng)狀態(tài) 系統(tǒng)矩陣 狀態(tài)的控制量 觀測值 觀測矩陣 過程噪聲 測量噪聲 實際溫度 溫度不變，為 1 無控制量，為 0 溫度計讀數 直接讀出，為 1 溫度變化偏差，常量 1e-1 讀數誤差，常量 1e-6 f B、uk Zk H wk vk 假如我們要估算 2 時刻房間的實際溫度值。首先你要根據 1 時刻溫度的估 計值(就假設為 1） ，來算出 2 時刻溫度的估計值，即： 2,1 = f * x 1 x =1 （*公式 1） ，然后由給出的 1 時刻的均方差 P （假設為 10）進一步更新均方差， 1 =

15、 f * P * f T + G Q GT = 10.009（*公式 2） 有P 。 有方差 2,1 1 k -1 P 2,1 之后，根 -1 * H T éH * P *HT + R ù 據卡爾曼增益方程計算出增益： K = P 2 2,1 vv ë û 公式 3） 。 =0.925 （* 現在，我們可以通過增益和測量值（Z= 26.676，溫度計有測量誤差）來估 計 2 時刻的溫度了， 公式 4） 。 得到了 2 時刻的估計溫度，下一步就是對 3 時刻的溫度值進行最優(yōu)估算，需 要得到 K 時刻的最優(yōu)溫度（24.56）的偏差，算法如下： 2 = x 2

16、,1 + K é 2,1 ù x ëZ - H * x û = 24.763（* P2 = E - K * H * P 2,1 從而估算出最優(yōu)的溫度值，運行速度快。 = 0.751（*公式 5） 就這樣， 再進入 3 時刻的濾波循環(huán)， 卡爾曼濾波器就不斷的把均方誤差遞歸， 以下是 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; %產生隨機高斯分布 x(1=0; %賦值與否無所謂 a=1;

17、%系統(tǒng)狀態(tài)矩陣 %產生隨機高斯分布 V=randn(1,N; q1=std(V; Rvv=q1.2; q2=std(w; Qww=q2.2; h=1; Y=25+V; p(1=10; X(1=0; %Q %觀測矩陣 %測量誤差值 %R % 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; p1(k=a*p(k-1*a'+Qww; % X(k|k-1=A X(k-1|k-1+B U(k (1 % P(k|k-1=A P(k-1|k-1 A+Q (2 Kg(k=p1(k*h'/(h*p1(k*h'+Rvv; P(k|k-1 H + R (4 X(k=x(k+Kg(k*(Y(k-h*x(k; (Z(k-H X(k|k-1 (3 p(k=p1(k-Kg(k*h*p1(k; P(k|k-1 (5 end for i = 1: N expValue(i = 25; end k=1:N; plot(k,X,'r',k,Y,'g',k,expValue,'b' legend('真實值',&