卡爾曼濾波算法與matlab實現(xiàn)

1、 一個應(yīng)用實例詳解卡爾曼濾波及其算法實現(xiàn)標簽： 算法filtermatlabalgorithm優(yōu)化工作2012-05-14 10:48 75511人閱讀 評論(25) 收藏 舉報 分類：數(shù)據(jù)結(jié)構(gòu)及其算法（4） 為了可以更加容易的理解卡爾曼濾波器，這里會應(yīng)用形象的描述方法來講解，而不是像大多數(shù)參考書那樣羅列一大堆的數(shù)學(xué)公式和數(shù)學(xué)符號。但是，他的5條公式是其核心內(nèi)容。結(jié)合現(xiàn)代的計算機，其實卡爾曼的程序相當?shù)暮唵?，只要你理解了他的?條公式。在介紹他的5條公式之前，先讓我們來根據(jù)下面的例子一步一步的探索。假設(shè)我們要研究的

2、對象是一個房間的溫度。根據(jù)你的經(jīng)驗判斷，這個房間的溫度是恒定的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設(shè)我們用一分鐘來做時間單位）。假設(shè)你對你的經(jīng)驗不是100%的相信，可能會有上下偏差幾度。我們把這些偏差看成是高斯白噪聲（White Gaussian Noise），也就是這些偏差跟前后時間是沒有關(guān)系的而且符合高斯分配（Gaussian Distribution）。另外，我們在房間里放一個溫度計，但是這個溫度計也不準確的，測量值會比實際值偏差。我們也把這些偏差看成是高斯白噪聲。好了，現(xiàn)在對于某一分鐘我們有兩個有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗的預(yù)測值（系統(tǒng)的預(yù)測值）和溫度計的值（測量值）。

3、下面我們要用這兩個值結(jié)合他們各自的噪聲來估算出房間的實際溫度值。假如我們要估算k時刻的是實際溫度值。首先你要根據(jù)k-1時刻的溫度值，來預(yù)測k時刻的溫度。因為你相信溫度是恒定的，所以你會得到k時刻的溫度預(yù)測值是跟k-1時刻一樣的，假設(shè)是23度，同時該值的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1時刻估算出的最優(yōu)溫度值的偏差是3，你對自己預(yù)測的不確定度是4度，他們平方相加再開方，就是5）。然后，你從溫度計那里得到了k時刻的溫度值，假設(shè)是25度，同時該值的偏差是4度。由于我們用于估算k時刻的實際溫度有兩個溫度值，分別是23 度和25度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰

4、多一點，我們可以用他們的covariance（協(xié)方差）來判斷。因為 Kg2=52/(52+42)，所以Kg=0.78，我們可以估算出k時刻的實際溫度值是：23+0.78*(25-23)=24.56度?？梢钥闯觯驗闇囟扔嫷腸ovariance比較?。ū容^相信溫度計），所以估算出的最優(yōu)溫度值偏向溫度計的值?，F(xiàn)在我們已經(jīng)得到k時刻的最優(yōu)溫度值了，下一步就是要進入 k+1時刻，進行新的最優(yōu)估算。到現(xiàn)在為止，好像還沒看到什么自回歸的東西出現(xiàn)。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優(yōu)值（24.56 度）的偏差。算法如下：(1-Kg)*52)0.5=2.35。這里的5就是上面的k時刻你預(yù)測的

5、那個23度溫度值的偏差，得出的2.35就是進入 k+1時刻以后k時刻估算出的最優(yōu)溫度值的偏差（對應(yīng)于上面的3）。就是這樣，卡爾曼濾波器就不斷的把 covariance遞歸，從而估算出最優(yōu)的溫度值。他運行的很快，而且它只保留了上一時刻的covariance。上面的Kg，就是卡爾曼增益（Kalman Gain）。他可以隨不同的時刻而改變他自己的值，是不是很神奇！下面就要言歸正傳，討論真正工程系統(tǒng)上的卡爾曼。3 卡爾曼濾波器算法（The Kalman Filter Algorithm）在這一部分，我們就來描述源于Dr Kalman 的卡爾曼濾波器。下面的描述，會涉及一些基本的概念知識，包括概率（Pr

6、obability），隨即變量（Random Variable），高斯或正態(tài)分配（Gaussian Distribution）還有State-space Model等等。但對于卡爾曼濾波器的詳細證明，這里不能一一描述。首先，我們先要引入一個離散控制過程的系統(tǒng)。該系統(tǒng)可用一個線性隨機微分方程（Linear Stochastic Difference equation）來描述：X(k)=A X(k-1)+B U(k)+W(k)再加上系統(tǒng)的測量值：Z(k)=H X(k)+V(k)上兩式子中，X(k)是k時刻的系統(tǒng)狀態(tài)，U(k)是k時刻對系統(tǒng)的控制量。A和B是系統(tǒng)參數(shù)，對于多模型系統(tǒng)，他們?yōu)榫仃?。Z(

7、k)是k時刻的測量值，H是測量系統(tǒng)的參數(shù)，對于多測量系統(tǒng)，H為矩陣。W(k)和V(k)分別表示過程和測量的噪聲。他們被假設(shè)成高斯白噪聲(White Gaussian Noise)，他們的covariance 分別是Q，R（這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化）。對于滿足上面的條件(線性隨機微分系統(tǒng)，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最優(yōu)的信息處理器。下面我們來用他們結(jié)合他們的covariances 來估算系統(tǒng)的最優(yōu)化輸出（類似上一節(jié)那個溫度的例子）。首先我們要利用系統(tǒng)的過程模型，來預(yù)測下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測出現(xiàn)在狀態(tài)：X(

8、k|k-1)=A X(k-1|k-1)+B U(k) . (1)式(1)中，X(k|k-1)是利用上一狀態(tài)預(yù)測的結(jié)果，X(k-1|k-1)是上一狀態(tài)最優(yōu)的結(jié)果，U(k)為現(xiàn)在狀態(tài)的控制量，如果沒有控制量，它可以為0。到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對應(yīng)于 X(k|k-1)的covariance(協(xié)方差)還沒更新。我們用P表示covariance：P(k|k-1)=A P(k-1|k-1) A+Q (2)式(2)中，P(k|k-1)是X(k|k-1)對應(yīng)的covariance，P(k-1|k-1)是X(k-1|k-1)對應(yīng)的 covariance，A表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過程的c

9、ovariance。式子1，2就是卡爾曼濾波器5個公式當中的前兩個，也就是對系統(tǒng)的預(yù)測。現(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測結(jié)果，然后我們再收集現(xiàn)在狀態(tài)的測量值。結(jié)合預(yù)測值和測量值，我們可以得到現(xiàn)在狀態(tài)(k)的最優(yōu)化估算值X(k|k)：X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1) (3)其中Kg為卡爾曼增益(Kalman Gain)：Kg(k)= P(k|k-1) H / (H P(k|k-1) H + R) (4)到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值 X(k|k)。但是為了要另卡爾曼濾波器不斷的運行下去直到系統(tǒng)過程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的co

10、variance：P(k|k)=（I-Kg(k) H）P(k|k-1) (5)其中I 為1的矩陣，對于單模型單測量，I=1。當系統(tǒng)進入k+1狀態(tài)時，P(k|k)就是式子(2)的P(k-1|k-1)。這樣，算法就可以自回歸的運算下去。卡爾曼濾波器的原理基本描述了，式子1，2，3，4和5就是他的5 個基本公式。根據(jù)這5個公式，可以很容易的實現(xiàn)計算機的程序。下面，用Matlab程序舉一個實際運行的例子。4 簡單例子（A Simple Example）這里我們結(jié)合第二第三節(jié)，舉一個非常簡單的例子來說明卡爾曼濾波器的工作過程。所舉的例子是進一步描述第二節(jié)的例子，而且還會配以程序模擬結(jié)果。根據(jù)第二節(jié)的描述

11、，把房間看成一個系統(tǒng)，然后對這個系統(tǒng)建模。當然，我們見的模型不需要非常地精確。我們所知道的這個房間的溫度是跟前一時刻的溫度相同的，所以A=1。沒有控制量，所以U(k)=0。因此得出：X(k|k-1)=X(k-1|k-1) . (6)式子（2）可以改成：P(k|k-1)=P(k-1|k-1) +Q (7)因為測量的值是溫度計的，跟溫度直接對應(yīng)，所以H=1。式子 3，4，5可以改成以下：X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1) (8)Kg(k)= P(k|k-1) / (P(k|k-1) + R) (9)P(k|k)=（1-Kg(k)）P(k|k-1) (10)現(xiàn)

12、在我們模擬一組測量值作為輸入。假設(shè)房間的真實溫度為25 度，我模擬了200個測量值，這些測量值的平均值為25度，但是加入了標準偏差為幾度的高斯白噪聲（在圖中為藍線）。為了令卡爾曼濾波器開始工作，我們需要告訴卡爾曼兩個零時刻的初始值，是X(0|0)和P(0|0)。他們的值不用太在意，隨便給一個就可以了，因為隨著卡爾曼的工作，X會逐漸的收斂。但是對于 P，一般不要取0，因為這樣可能會令卡爾曼完全相信你給定的X(0|0)是系統(tǒng)最優(yōu)的，從而使算法不能收斂。我選了X(0|0)=1 度，P(0|0)=10。該系統(tǒng)的真實溫度為25度，圖中用黑線表示。圖中紅線是卡爾曼濾波器輸出的最優(yōu)化結(jié)果（該結(jié)果在算法中設(shè)置

13、了Q=1e-6，R=1e-1）。clearN=200;w(1)=0;w=randn(1,N)x(1)=0;a=1;for k=2:N;x(k)=a*x(k-1)+w(k-1);endV=randn(1,N);q1=std(V);Rvv=q1.2;q2=std(x);Rxx=q2.2;q3=std(w);Rww=q3.2;c=0.2;Y=c*x+V;p(1)=0;s(1)=0;for t=2:N;p1(t)=a.2*p(t-1)+Rww;b(t)=c*p1(t)/(c.2*p1(t)+Rvv);s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1);p(t)=p1(t)-c*b(

14、t)*p1(t);endt=1:N;plot(t,s,'r',t,Y,'g',t,x,'b');用matlab做的kalman濾波程序,已通過測試還有下面一個 Matlab源程序，顯示效果更好。clearclc;N=300;CON = 25;%房間溫度，假定溫度是恒定的%kalman filter%x = zeros(1,N);y = 20.5 * randn(1,N) + CON;%加過程噪聲的狀態(tài)輸出x(1) = 1;p = 10;Q = cov(randn(1,N);%過程噪聲協(xié)方差R = cov(randn(1,N);%觀測噪聲協(xié)方差fo

15、r k = 2 : Nx(k) = x(k - 1);%預(yù)估計k時刻狀態(tài)變量的值p = p + Q;%對應(yīng)于預(yù)估值的協(xié)方差kg = p / (p + R);%kalman gainx(k) = x(k) + kg * (y(k) - x(k);p = (1 - kg) * p;end%Smoothness Filter%Filter_Wid = 10;smooth_res = zeros(1,N);for i = Filter_Wid + 1 : Ntempsum = 0;for j = i - Filter_Wid : i - 1tempsum = tempsum + y(j);endsmo

16、oth_res(i) = tempsum / Filter_Wid;end% figure(1);% hist(y);t=1:N;figure(1);expValue = zeros(1,N);for i = 1: NexpValue(i) = CON;endplot(t,expValue,'r',t,x,'g',t,y,'b',t,smooth_res,'k');legend('expected','estimate','measure','smooth result

17、9;);axis(0 N 20 30)xlabel('Sample time');ylabel('Room Temperature');title('Smooth filter VS kalman filter');卡爾曼濾波算法-核心公式推導(dǎo)導(dǎo)論再造紅旗寫在最前面：這是我第一篇專欄文章，感謝知乎提供這么一個平臺，讓自己能和大家分享知識。本人會不定期的開始更新文章，文章的內(nèi)容應(yīng)該集中在汽車動力學(xué)控制，整車軟件架構(gòu)，控制器等方面。作為一名在校碩士，很多理解都可能不全面，不正確，大家有不同意見歡迎討論。 謝謝！-卡爾曼濾波算法很牛逼，因為有一堆公式，

18、有一堆符號，看起來就很牛逼啊，乍一看不懂的都很牛逼??！本文針對卡爾曼濾波算法的核心公式進行推導(dǎo)，不讓大家被它華麗的外表嚇到。（之后計劃寫關(guān)于針對非線性情況的EKF和UKF，對卡爾曼濾波算法做一個全面一點的應(yīng)用介紹。感興趣的可以關(guān)注專欄。）-Okay，進入正題。這篇文章假設(shè)讀者已經(jīng)對卡爾曼濾波算法有初步的了解，知道它能做什么，知道它的優(yōu)點，知道它很牛逼，并且你已經(jīng)對它產(chǎn)生興趣，但不知道如何下手。首先給出一個控制理論中公式，別急著翻控制理論的書，沒那么復(fù)雜：兩個基本問題：1.卡爾曼濾波算法要做什么？對狀態(tài)進行估計。2. 卡爾曼濾波算法怎么對狀態(tài)進行估計？利用狀態(tài)過程噪聲和測量噪聲對狀態(tài)進行估計。一

19、個狀態(tài)在一個時刻點k的狀態(tài)進入下一個時刻點k+1狀態(tài)，會有很多外界因素的干擾，我們把干擾就叫做過程噪聲，（這個詞一看就是硬翻譯過來的，別在意為什么叫噪聲）用w表示。任何一個測量儀器，都會有誤差，我們把這個誤差叫做量測噪聲，用v表示。回到上面那個公式，狀態(tài)方程表示狀態(tài)在不斷的更新，從一個時刻點進入下一個時刻點，這個很好理解。關(guān)鍵是量測方程，它表示，我們不斷更新的狀態(tài)有幾個能用測量儀器測出來，比如，汽車運動狀態(tài)參數(shù)有很多，比如速度，輪速，滑移率等，但是我們只能測量出輪速，因此量測方程要做的就是把狀態(tài)參數(shù)中能量測的狀態(tài)拿出來。我們始終要記得我們要做的事：我們要得到的是優(yōu)化的狀態(tài)量Xk。理解了上面之后

20、就可以開始推導(dǎo)公式了。1.首先不考慮過程噪聲對狀態(tài)進行更新，很簡單：舉個例子，v(k)=v(k-1)+at，勻加速運動咯。2.不考慮測量噪聲取出能測量的狀態(tài)，也很簡單：3.用測量儀器測量出來的狀態(tài)值（大家可以考慮到：測量的值就是被各種噪聲干擾后的真實值）減去上面不考慮噪聲得到的測量值：這個值在數(shù)學(xué)上是一個定義值，叫做新息，有很多有趣的性質(zhì)，感興趣的可以自己谷歌。我們對步驟暫且停一停。這個叫新息的值有什么用？由上面的過程我們可以明顯看到，它反映了過程噪聲和測量噪聲綜合對測量狀態(tài)值的影響，也就是它包含了w和v的情況。回到數(shù)學(xué)層面，（不要害怕，很簡單的數(shù)學(xué)應(yīng)用和思考啦?。┮粋€數(shù)值c由兩部分內(nèi)容a和b

21、組成，那么怎樣用數(shù)學(xué)表達式來表達？一般有兩種做法：I.直接相加：c=a+b;II. 用比例的方法：a=n*c,b=(1-n)*c卡爾曼采用了方法II，用比例的方法來做（其實這也是為什么叫做濾波的原因，因為濾波就是給權(quán)值之類的操作）。也就是說，過程噪聲w=新息*一個比例。這樣得到的過程噪聲加上原來（第一步）不考慮過程噪聲的狀態(tài)值不就是優(yōu)化值了嗎？ 也就是：Okay，都寫到這里了，有必要做一下前提假設(shè)：a. 什么高斯噪聲，均值為零一堆；b.Ak,Ck,wk的協(xié)方差Q,vk的協(xié)方差R，系統(tǒng)協(xié)方差初始值P0,狀態(tài)初始值X0，都已知。為什么已知，你實際做項目就知道了。不過不懂的可以留言或者私信。那么到目

22、前為止我們的思路就是清楚了，找到一個合適的Hk值（卡爾曼增益），那么我們就能得到狀態(tài)的最優(yōu)值。（卡爾曼說的，不是我說的，所以你問為什么，你要問他，這么深層次的理論留給博士和學(xué)者們?nèi)プ鼍秃茫覀兙同F(xiàn)學(xué)現(xiàn)用就行，哈哈哈，站在巨人的肩膀?。﹩栴}來了：怎么得到合適的Hk？似乎不是隨便一個參數(shù)。這是誤差協(xié)方差矩陣。思路：使得誤差協(xié)方差矩陣Pk最小的Hk。為什么？這里我從感觀的角度說明自己的理解，歡迎討論。協(xié)方差表示什么，協(xié)方差表示兩者之間的聯(lián)系或者關(guān)系，關(guān)系越大，協(xié)方差越大。誤差協(xié)方差越小說明過程噪聲和量測噪聲的關(guān)系越小。關(guān)系越小能做什么，這要回到我們第3步討論的我們用比例的方法分開了w和v。用比例分開

23、，到底多少屬于w，多少是v，如果關(guān)系越小，分開的越精確，比如一堆白砂糖和鹽，如果兩種混合的很均勻，我們說它關(guān)系很大，也就越難用比例的方法將其分開。4.求的誤差協(xié)方差矩陣Pk自然是把里面的Xk先得到，然后公式運算，通過上面的步驟我們也容易得到：然后復(fù)雜的數(shù)學(xué)計算，和之前假設(shè)的高斯噪聲，新息的性質(zhì)之類（至于過程，個人覺得你如果只做應(yīng)用，不研究算法，就沒必要深入去看了），就能得到下面的卡爾曼濾波遞推公式：通過上面的解釋，我們也就不難知道這些公式都在干嘛，知道干嘛就可以了。在知道A,C,P0,Q,R的情況下，整個公式的運算流程也都很清晰了。過程方程：X(k+1) =  A

24、 X(k) + B U(k) + W(k)               >>>>式1量測方程：Z(k+1) =  H X（k+1）+ V(k+1)                  >>

25、;>>式2A和B是系統(tǒng)參數(shù)，對于多模型系統(tǒng)，他們?yōu)榫仃嚕籋是測量系統(tǒng)的參數(shù)，對于多測量系統(tǒng)，H為矩陣。W(k)和V(k)分別表示過程和測量的噪聲。他們被假設(shè)成高斯白噪聲，他們的協(xié)方差 分別是Q，R。為了不失一般性，下面的討論中將X,Z都視為矩陣，其中X是m行的單列矩陣，Z是n行的單列矩陣。 說明：下面的表達式中，不帶前綴的量都代表實際量，其小括號里面的“k”或“k+1”代表該量是第k或第k+1時刻的實際量，如“Z(k+1)”就代表第k+1時刻的實際測量值；帶前綴“”的量都代表預(yù)測量，如果小括號里面是“k+1|k”，就代表k+1時刻的先驗預(yù)測值，如果小括號里面是“k+1|k

26、+1”，就代表k+1時刻的后驗預(yù)測值；（測量值可以通過測量得到，所以只有先驗預(yù)測，沒有后驗預(yù)測。而實際狀態(tài)值無法得知，既有先驗預(yù)測，又有后驗預(yù)測）帶前綴“”的量都代表與預(yù)測值對應(yīng)的偏差值。 實際狀態(tài)值與先驗預(yù)測狀態(tài)值的偏差 = 實際狀態(tài)值 先驗預(yù)測狀態(tài)值X(k+1|k)      =     X(k+1)    -      X(k+1|k)       &

27、#160;     >>>> 式3 實際測量值與先驗預(yù)測測量值的偏差 = 當前測量值 - 先驗預(yù)測測量值Z(k+1|k)  = Z(k+1)  -   Z(k+1|k)                         

28、0;         >>>>式4 并且先驗預(yù)測測量值  =  轉(zhuǎn)換矩陣H * 先驗預(yù)測狀態(tài)值Z(k+1|k) =  H X(k+1|k)                        

29、    >>>> 式5 得到測量值后，再對當前狀態(tài)值X(k+1) 進行后驗預(yù)測（設(shè)后驗預(yù)測值為 Z(k+1|k+1) ） ，則后驗預(yù)測值（同時也是最終預(yù)測值）的偏差為X(k+1|k+1)  =     X(k+1)    -      X(k+1|k+1)             &#

30、160;   >>>>式6 為了得到當前狀態(tài)值X(k+1)， 根據(jù)式3，需要：X(k+1) =  X(k+1|k)  + X(k+1|k)                        >>>> 式7上式中，我們可以通過卡爾曼公式1（見附注2）計算出X(k+1|

31、k)，但我們無法得知實際狀態(tài)值X(k+1)，因而X(k+1|k) 也無法得知。我們最終的目的是得出一個比較接近實際狀態(tài)值X(k+1)的濾波值X(k+1|k+1)，根據(jù)式7，只要能準確的估計出X(k+1|k)即可。X(k+1|k)本身雖無法得知，但Z(k+1|k) 卻可以通過測量得到，而且它們二者存在一定的相關(guān)性。不妨再設(shè)存在一個矩陣K（m行n列矩陣），能使得 X(k+1|k) = K * Z(k+1|k)                

32、                        >>>>式8那么最終的預(yù)測任務(wù)其實就是找到K。由于X(k+1|k)和Z(k+1|k)都是單列矩陣，因此不難看出，滿足式8的矩陣K應(yīng)有無窮多個。矩陣K中第i行第j列反映了量測變量偏差矩陣Z(k+1|k)的第j個元素對狀態(tài)變量偏差矩陣X(k+1|k)的第i個元素的貢獻。因此矩陣K的物理意義很明顯，K的

33、第i行第j列的元素表示：對于第i個待測的狀態(tài)量來說，第j個測量儀器測到的偏差的可信度。某個測量值對應(yīng)的可信度越高，濾波器越“相信”該測量值。 既然滿足條件的K有無窮多個，那應(yīng)該使用哪個K呢？實際上，我們并不知道X(k+1|k)的值，所以也就無法直接計算出K，而只能通過某種方法找到一個Kg，使得將Kg帶入式8后，等號兩邊的差（的平方）的期望盡可能小。我們最終的預(yù)測值或濾波值是后驗預(yù)測值X(k+1|k+1)，因此最后的預(yù)測也應(yīng)使 X(k+1|k+1) 的期望為0且方差最?。ㄟ@與讓8式兩端的差最小是一致的，下面的式9體現(xiàn)了這一點），這樣預(yù)測值才最可靠。下面詳細說明。 X(k+1|

34、k+1) =  X(k+1|k) + Kg * Z(k+1|k)        （后驗預(yù)測的狀態(tài)值）X(k+1|k+1)  =     X(k+1)    -      X(k+1|k+1)    （后驗預(yù)測的偏差） X(k+1|k+1)  =       &

35、#160;           X(k+1)                         -             X(k+1|k+1

36、)                       =     ( X(k+1|k)  +  X(k+1|k) ) -      (  X(k+1|k) + Kg * Z(k+1|k)  )   

37、;                  =                   X(k+1|k)             

38、       -             Kg * Z(k+1|k)                             &#

39、160;                                                 &#

40、160;                         >>>>式9 Z(k+1|k)       =             

41、60;     Z(k+1)                         -             Z(k+1|k)      

42、60;              =     (  H X（k+1）+ V(k+1)  )              -      (  H X(k+1|k)  )

43、60;                    =     H (  X（k+1）-X(k+1|k)  )  + V(k+1)               

44、60;     =     H  X(k+1|k)  + V(k+1)                                  &

45、#160;  >>>>式10 接下來的分析中，為了更直觀的說明卡爾曼濾波的原理，我們用幾何方法來解釋。這時，X和Z矩陣中的每個元素應(yīng)看做向量空間中的一個向量而不再是一個單純的數(shù)。這個向量空間（統(tǒng)計測試空間）可以看成無窮多維的，每一個維對應(yīng)一個可能的狀態(tài)。X和Z矩陣中的每個元素向量都是由所有可能的狀態(tài)按照各自出現(xiàn)的概率組合而成（在測量之前，X和Z 的實際值都是不可知的）。X和Z中的每個元素向量都應(yīng)是0均值的，他們與自己的內(nèi)積就是他們的協(xié)方差矩陣。我們無法給出X和Z中每個元素向量的具體表達，但我們通過協(xié)方差矩陣就可以知道所有元素向量的模長，以及相互之間的

46、夾角（從內(nèi)積計算）。為了方便用幾何方法解釋，我們假設(shè)狀態(tài)變量X是一個1行1列的矩陣（即只有一個待測狀態(tài)量），而量測變量Z是一個2行1列的矩陣（即有兩個測量儀器，共同測量同一個狀態(tài)量X），也就是說，m=1，n=2。矩陣X中只有X1一項，矩陣Z中有Z1和Z2兩項。Kg此時應(yīng)是一個1行2列的矩陣，兩個元素分別記作Kg1 和 Kg2 。H和V此時應(yīng)是一個2行1列的矩陣。將矩陣表達式9和10按元素展開：X(k+1|k+1)1    =     X(k+1|k)1       

47、       -      (Kg1 * Z(k+1|k)1 + Kg2 * Z(k+1|k)2 )                                 &#

48、160;                    >>>> 式9iZ(k+1|k)i   =     Hi  X(k+1|k)     +     V(k+1)i      &#

49、160;                            >>>>式10i X(k+1|k) 中各個元素（向量）的線性組合可以產(chǎn)生一個m維或更低維的向量子空間Vx，這里，按照我們的假設(shè)，m=1，所以Vx應(yīng)是一維的； 同時V(k+1)中的各個元素（向量）的線性組合也可以產(chǎn)生一個n維或更低維的

50、向量子空間Vv，這里，按照我們的假設(shè)，n=2，所以Vv應(yīng)是二維的。由于V(k+1)中的每一項與X(k+1|k)中的每一項都不相關(guān)（見附注1），故這兩個子空間相互垂直。如下圖所示。式10i所體現(xiàn)的Z(k+1|k)i、Hi X(k+1|k)、V(k+1)i  三者之間的幾何關(guān)系，也在下圖中描繪了出來。   從上圖中可以看出，Z(k+1|k)中各個元素（向量）的線性組合也可以產(chǎn)生一個n維或更低維的向量子空間Vz，這里已假設(shè)n=2，所以Vz是一個二維的平面，就是上圖中兩條紅色的線所構(gòu)成的平面。   圖2中（注意此圖中的橢圓代表的是V

51、z空間，而圖1中則代表Vv空間，二者不一樣），粉色的向量就是Kg1 * Z(k+1|k)1 + Kg2 * Z(k+1|k)2 ， 記此粉色向量為 Y ，Y為Z(k+1|k)1和Z(k+1|k)2線性組合而成，它始終在子空間Vz中。根據(jù)式9i，X(k+1|k+1)1 等于X(k+1|k)1和Y的差向量，為使X(k+1|k+1)1長度最短（協(xié)方差最小），Kg的選取應(yīng)使得X(k+1|k+1)1垂直于Vz空間。通過先驗預(yù)測的協(xié)方差矩陣（見卡爾曼公式2），可以得到X(k+1|k)中各個元素的模長以及彼此間的夾角。這是因為協(xié)方差矩陣中的第i行第j列其實就代表了X(k+1|k)中第i個

52、元素向量與第j個元素向量的內(nèi)積。通過測量可以得到新息協(xié)方差（見卡爾曼公式3的分母部分），進而可以知道Z(k+1|k)中各個元素的模長以及彼此間的夾角。通過已知的量測噪聲協(xié)方差矩陣R，可以得出V(k+1) 中各個元素的模長以及彼此間的夾角。最后根據(jù)X(k+1|k+1)1與Y垂直以及圖1中所示的幾何關(guān)系，用高中學(xué)的立體幾何和向量知識就可以求得兩個Kg的值了。如果將向量的內(nèi)積都用協(xié)方差矩陣表示，就會發(fā)現(xiàn)，我們最后求得的Kg，其實就是卡爾曼公式3。 （上面討論的是較低次的卡爾曼濾波，只有一個待測量，兩個測量儀器。這種情況還是比較常見的，比如傾角測量系統(tǒng)中，我們用加速度計和陀螺儀共同測量傾角。

53、對于更高次的卡爾曼濾波，X和Z都是多行矩陣時，用幾何方法已經(jīng)無法直觀解釋，只能用矩陣分析的方法證明。求解Kg的詳細過程參考 卡爾曼濾波器及其應(yīng)用基礎(chǔ)國防工業(yè)出版社敬喜 編 ） 卡爾曼濾波的核心過程，就是求解能使得E  X(k+1|k+1)  *  X(k+1|k+1) 取最小值的Kg增益矩陣的過程，X(k+1|k+1)代表的是X(k+1|k+1)的轉(zhuǎn)置（這里X(k+1|k+1)中的元素代表數(shù)值，不是向量）。前面已經(jīng)提到過，卡爾曼增益矩陣Kg中的元素，都代表測量儀器測到的偏差的可信度，或者叫估計權(quán)重。 附注1： (a

54、).   v(k+1)中的每一項與X(k+1|k)中的每一項都不相關(guān) X(k+1|k)  =            X(k+1)        -         X(k+1|k)          =

55、0;          X(k+1)         -  ( A X(k|k)  +  B U(k)  )          = ( A X(k) + B U(k) + W(k) ) &#

56、160; -   (  A X(k)  +  B U(k)  -  A X(k|k) )         =         W(k)   +   A X(k|k)    

57、0;    =         W(k)   +   A (  X(k|k-1)  -  Kg(k)* Z(k|k-1)  )  -這一步利用了式9                =    

58、            W(k)   +   A (  X(k|k-1)  -  Kg(k)* ( H X(k|k-1) + v(k) )  )             -這一步利用了式10     

59、           =                W(k)   -  A Kg(k) *v(k)  +  A ( I - Kg(k) * H )  X(k|k-1)上式最后一行出現(xiàn)了X(k|k-1)，可見X(k+1

60、|k)可以遞歸表示。而且遞歸式中的過程噪聲W(k)與v(k+1)不相關(guān)，同時由于 v本身是白噪聲，所以 v(k+1)與v(k)亦不相關(guān)（白噪聲的自相關(guān)是函數(shù)），因此通過遞推式可以判斷v(k+1)與X(k+1|k)不相關(guān)。 (b).  w(k+1)中的每一項與X(k+1|k+1)中的每一項都不相關(guān)，w(k+1)中的每一項與X(k+1|k)中的每一項都不相關(guān)。X(k+1|k+1)  =            X(k+1)   &#

61、160;    -         X(k+1|k+1)          = (  X(k+1|k) + X(k+1|k)  )   -   ( X(k+1|k) + Kg(k+1)*Z(k+1|k) )        

62、0;  =             X(k+1|k)       -       Kg(k+1)*Z(k+1|k)          =         

63、60;   X(k+1|k)       -     Kg(k+1)* (  H X(k+1|k) + v(k+1)   )           =            - Kg(k+1)* v(k+1)&#

64、160; +   ( I - Kg(k+1) * H ) X(k+1|k)我們已經(jīng)知道w(k+1)與v(k+1)不相關(guān)，因此只要X(k+1|k+1)與上式的第二項也不相關(guān)，就說明結(jié)論(b)成立。根據(jù)(a)中的結(jié)論，X(k+1|k)的遞歸展開式中出現(xiàn)的 v(k) ，w(k) ， v(k-1)，w(k-1)等等，顯然 w(k+1)與 v (m=k，k-1) 都不相關(guān)，另外，由于w(k+1)的自相關(guān)為函數(shù)，因此w(k+1)與 w(m=k，k-1) 也不相關(guān)，也就得出w(k+1)與X(k+1|k) 不相關(guān)。進而可知，w(k+1)與X(k+1|

65、k+1)不相關(guān)。 正是因為 (a) (b)中的兩個不相關(guān)，卡爾曼公式中的預(yù)測協(xié)方差矩（卡爾曼公式(2)）和新息協(xié)方差矩陣（卡爾曼公式(3)中的“分母”部分）才可以是簡單的加式。 附注2：卡爾曼濾波的五個公式 先驗預(yù)測值與先驗預(yù)測協(xié)方差矩陣的計算。求解協(xié)方差時，都認為預(yù)測值的期望是實際值。因此，X(k+1|k)的協(xié)方差矩陣同樣也是 X(k+1|k) 的協(xié)方差矩陣，又因為偏差X(k+1|k)的期望是0，因此協(xié)方差矩陣反映了X(k+1|k)在向量空間中的模長。注意，協(xié)方差矩陣都是對稱矩陣。X(k+1|k)=A X(k|k)+B U(k)   

66、0;                (1)P(k+1|k)=A P(k|k) A+Q(k)                   (2) 卡爾曼增益矩陣的計算。量測預(yù)測值為Z(k+1|k) = H X(k+1|k)新息協(xié)方差見公式（3）中的“分母”部分。量測預(yù)測值的期望是

67、實際量測值。因此，Z(k+1|k)的協(xié)方差矩陣同樣也是 Z(k+1|k) 的協(xié)方差矩陣，又因為偏差Z(k+1|k)的期望是0，因此協(xié)方差矩陣反映了Z(k+1|k)在向量空間中的模長。Kg(k+1)= P(k+1|k) H/ (H P(k+1|k) H + R(k+1) (3) 后驗預(yù)測值與后驗預(yù)測協(xié)方差矩陣的計算X(k+1|k+1)= X(k+1|k)+Kg(k+1) (Z(k+1)-H X(k+1|k) (4)P(k+1|k+1)=（I-Kg(k+1) H）P(k+1|k)         

68、(5)clearN=200;w(1)=0; w=randn(1,N) x(1)=0; a=1; for k=2:N; x(k)=a*x(k-1)+w(k-1); end   V=randn(1,N); q1=std(V); Rvv=q1.2; q2=std(x); Rxx=q2.2;  q3=std(w); Rww=q3.2; c=0.2; Y=c*x+V;  p(1)=0; 

69、s(1)=0; for t=2:N; p1(t)=a.2*p(t-1)+Rww; b(t)=c*p1(t)/(c.2*p1(t)+Rvv); s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1); p(t)=p1(t)-c*b(t)*p1(t); end  t=1:N; plot(t,s,'r',t,Y,'g',t,x,'b'); % Kalman濾波器 A=-1,0,1;1,0,0;-4,9,-2; B=6,1,1'C=

70、0,0,1;D=0; S=ss(A,B,C,D); Q=0.001;R=0.1;kest,L,P=kalman(S,Q,R); 運行程序，得到系統(tǒng)Kalman濾波器的增益矩陣L與估計誤差的協(xié)方差P為： L = 1.0641 1.1566 2.0393 P = 0.0678 0.0664 0.1064 0.0664 0.0695 0.1157 0.1064 0.1157 0.2039卡爾曼濾波算法及MATLAB實現(xiàn) (2012-08-29 21:39:56)轉(zhuǎn)載    這一段時間對現(xiàn)代濾波進行了學(xué)習(xí)，對自適應(yīng)濾波器和卡爾曼濾波器有了一定認識，并對

71、它們用MATLAB對語音信號進行了濾波，發(fā)現(xiàn)卡爾曼濾波器還是比較有用，能夠在較大的噪聲中還原原來的信號。新的學(xué)期馬上就開始了，由于TI的開發(fā)板一直在維修，所以學(xué)習(xí)TI開發(fā)板的計劃擱置，但是對聲音信號的處理及濾波器的認識有了進一步提高。新的學(xué)期繼續(xù)努力！      卡爾曼濾波的基本思想是：以最小均方誤差為最佳估計準則，采用信號與噪聲的狀態(tài)空間模型，利用前一時刻的估計值和當前時刻的觀測值來更新對狀態(tài)變量的估計，求出當前時刻的估計值，算法根據(jù)建立的系統(tǒng)方程和觀測方程對需要處理的信號做出滿足最小均方誤差的估計。語音信號在較長時間內(nèi)是非平穩(wěn)的，

72、但在較短的時間內(nèi)的一階統(tǒng)計量和二階統(tǒng)計量近似為常量，因此語音信號在相對較短的時間內(nèi)可以看成白噪聲激勵以線性時不變系統(tǒng)得到的穩(wěn)態(tài)輸出。假定語音信號可看成由一AR模型產(chǎn)生：                  時間更新方程：               測量更新方程：

73、0;            K(t)為卡爾曼增益，其計算公式為：           其中                、分別為過程模型噪聲協(xié)方差和測量模型噪聲協(xié)方差，測量協(xié)方差可以通過觀測得到，則較難確定，

74、在本實驗中則通過與兩者比較得到。     由于語音信號短時平穩(wěn)，因此在進行卡爾曼濾波之前對信號進行分幀加窗操作，在濾波之后對處理得到的信號進行合幀，這里選取幀長為256，而幀重疊個數(shù)為128；     下圖為原聲音信號與加噪聲后的信號以及聲音信號與經(jīng)卡爾曼濾波處理后的信號：            原聲音信號與加噪聲后的信號    

75、0;     原聲音信號與經(jīng)卡爾曼濾波處理后的信號MATLAB程序?qū)崿F(xiàn)如下：%基于LPC全極點模型的最大后驗概率估計法,采用卡爾曼濾波%clear;clc;%加載聲音數(shù)據(jù)%load voice.maty=m1(2,:);x=y+0.08*randn(1,length(y);%原聲音信號和加噪聲后的信號%figure(1);subplot(211);plot(m1(1,:),m1(2,:);xlabel('時間');ylabel('幅度');title('原聲音信號');subplot(212);p

76、lot(m1(1,:),x);xlabel('時間');ylabel('幅度');title('加噪聲后的信號');%輸入?yún)?shù)%Fs=44100;                      %信號采樣的頻率bits=16;         &

77、#160;       %信號采樣的位數(shù)N=256;                      %幀長m=N/2;                &#

78、160;     %每幀移動的距離lenth=length(x);            %輸入信號的長度count=floor(lenth/m)-1;     %處理整個信號需要移動的幀數(shù)%先不考慮補零的問題p=11;              &

79、#160;              %AR模型的階數(shù)a=zeros(1,p);w=hamming(N);                   %加漢明窗函數(shù)y_temp=0;F=zeros(11,11);       

80、    %轉(zhuǎn)移矩陣F(1,2)=1;F(2,3)=1;F(3,4)=1;F(4,5)=1;F(5,6)=1;F(6,7)=1;F(7,8)=1;F(8,9)=1;F(9,10)=1;F(10,11)=1;H=zeros(1,p);                        %S0=zeros(p,1);P0=zeros(p)

81、;S=zeros(p);H(11)=1;s=zeros(N,1);G=H'P=zeros(p);%測試噪聲協(xié)方差%y_temp=cov(x(1:7680);x_frame=zeros(256,1);x_frame1=zeros(256,1);T=zeros(lenth,1);for r=1:count%5%分幀處理%        x_frame=x(r-1)*m+1:(r+1)*m);%采用LPC模型求轉(zhuǎn)移矩陣參數(shù)%       

82、60;     if r=1           a,VS=lpc(x_frame(:),p);           else            a,VS=lpc(T(r-2)*m+1:(r-2)*m+256),p);        end%幀長內(nèi)過程噪聲協(xié)方差%        if (VS-y_temp>0)               VS=VS-y_temp;        els