一個(gè)應(yīng)用實(shí)例詳解卡爾曼濾波及其算法實(shí)現(xiàn)標(biāo)簽：算法filtermatlabalgorithm優(yōu)化工作2012-05-1410:4875511人閱讀評(píng)論（25）收藏舉報(bào)==分類：數(shù)據(jù)結(jié)構(gòu)及其算法（4）為了可以更加容易的理解卡爾曼濾波器，這里會(huì)應(yīng)用形象的描述方法來講解，而不是像大多數(shù)參考書那樣羅列一大堆的數(shù)學(xué)公式和數(shù)學(xué)符號(hào)。但是，他的5條公式是其核心內(nèi)容。結(jié)合現(xiàn)代的計(jì)算機(jī)，其實(shí)卡爾曼的程序相當(dāng)?shù)暮唵危灰憷斫饬怂哪?條公式。在介紹他的5條公式之前，先讓我們來根據(jù)下面的例子一步一步的探索。假設(shè)我們要研究的對象是一個(gè)房間的溫度。根據(jù)你的經(jīng)驗(yàn)判斷，這個(gè)房間的溫度是恒定的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設(shè)我們用一分鐘來做時(shí)間單位）。假設(shè)你對你的經(jīng)驗(yàn)不是100%的相信，可能會(huì)有上下偏差幾度。我們把這些偏差看成是高斯白噪聲（WhiteGaussianNoise），也就是這些偏差跟前后時(shí)間是沒有關(guān)系的而且符合高斯分配（GaussianDistribution）。另外，我們在房間里放一個(gè)溫度計(jì)，但是這個(gè)溫度計(jì)也不準(zhǔn)確的，測量值會(huì)比實(shí)際值偏差。我們也把這些偏差看成是高斯白噪聲。好了，現(xiàn)在對于某一分鐘我們有兩個(gè)有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗(yàn)的預(yù)測值（系統(tǒng)的預(yù)測值）和溫度計(jì)的值（測量值）。下面我們要用這兩個(gè)值結(jié)合他們各自的噪聲來估算出房間的實(shí)際溫度值。假如我們要估算k時(shí)刻的是實(shí)際溫度值。首先你要根據(jù)k-1時(shí)刻的溫度值，來預(yù)測k時(shí)刻的溫度。因?yàn)槟阆嘈艤囟仁呛愣ǖ模阅銜?huì)得到k時(shí)刻的溫度預(yù)測值是跟k-1時(shí)刻一樣的，假設(shè)是23度，同時(shí)該值的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1時(shí)刻估算出的最優(yōu)溫度值的偏差是3,你對自己預(yù)測的不確定度是4度，他們平方相加再開方，就是5）。然后，你從溫度計(jì)那里得到了k時(shí)刻的溫度值，假設(shè)是25度，同時(shí)該值的偏差是4度。由于我們用于估算k時(shí)刻的實(shí)際溫度有兩個(gè)溫度值，分別是23度和25度。究竟實(shí)際溫度是多少呢？相信自己還是相信溫度計(jì)呢？究竟相信誰多一點(diǎn)，我們可以用他們的covarianee（協(xié)方差）來判斷。因?yàn)镵g八2=5八2/（5八2+4八2），所以Kg=0.78，我們可以估算出k時(shí)刻的實(shí)際溫度值是：23+0.78*（25-23）=24.56度?？梢钥闯?，因?yàn)闇囟扔?jì)的covarianee比較?。ū容^相信溫度計(jì)），所以估算出的最優(yōu)溫度值偏向溫度計(jì)的值?，F(xiàn)在我們已經(jīng)得到k時(shí)刻的最優(yōu)溫度值了，下一步就是要進(jìn)入k+1時(shí)刻，進(jìn)行新的最優(yōu)估算。到現(xiàn)在為止，好像還沒看到什么自回歸的東西出現(xiàn)。對了，在進(jìn)入k+1時(shí)刻之前，我們還要算出k時(shí)刻那個(gè)最優(yōu)值（24.56度）的偏差。算法如下：（（1-Kg）*5八2廠0.5=2.35。這里的5就是上面的k時(shí)刻你預(yù)測的那個(gè)23度溫度值的偏差，得出的2.35就是進(jìn)入k+1時(shí)刻以后k時(shí)刻估算出的最優(yōu)溫度值的偏差（對應(yīng)于上面的3）。就是這樣，卡爾曼濾波器就不斷的把covarianee遞歸，從而估算出最優(yōu)的溫度值。他運(yùn)行的很快，而且它只保留了上一時(shí)刻的covariance。上面的Kg,就是卡爾曼增益(KalmanGain)。他可以隨不同的時(shí)刻而改變他自己的值，是不是很神奇！下面就要言歸正傳，討論真正工程系統(tǒng)上的卡爾曼。3.卡爾曼濾波器算法(TheKalmanFilterAlgorithm)在這一部分，我們就來描述源于DrKalman的卡爾曼濾波器。下面的描述，會(huì)涉及一些基本的概念知識(shí)，包括概率(Probability)，隨即變量(RandomVariable)，高斯或正態(tài)分配(GaussianDistribution)還有State-spaceModel等等。但對于卡爾曼濾波器的詳細(xì)證明，這里不能一一描述。首先，我們先要引入一個(gè)離散控制過程的系統(tǒng)。該系統(tǒng)可用一個(gè)線性隨機(jī)微分方程(LinearStochasticDiffereneeequation)來扌苗述：X(k)二AX(k-1)+BU(k)+W(k)再加上系統(tǒng)的測量值：Z(k)=HX(k)+V(k)上兩式子中，X(k)是*時(shí)刻的系統(tǒng)狀態(tài)，U(k)是*時(shí)刻對系統(tǒng)的控制量。A和B是系統(tǒng)參數(shù)，對于多模型系統(tǒng)，他們?yōu)榫仃噊Z(k)是*時(shí)刻的測量值，H是測量系統(tǒng)的參數(shù)，對于多測量系統(tǒng)，H為矩陣°W(k)和V(k)分別表示過程和測量的噪聲。他們被假設(shè)成高斯白噪聲(WhiteGaussianNoise)，他們的covariance分別是Q，R(這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化)。對于滿足上面的條件(線性隨機(jī)微分系統(tǒng)，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最優(yōu)的信息處理器。下面我們來用他們結(jié)合他們的covariances來估算系統(tǒng)的最優(yōu)化輸出(類似上一節(jié)那個(gè)溫度的例子)。首先我們要利用系統(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(k|k-1)=AX(k-I|k-1)+BU(k) (1)式(1)中，X(k|k-1)是利用上一狀態(tài)預(yù)測的結(jié)果，X(k-I|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)=AP(k-I|k-1)A'+Q (2)式⑵中,P(k|k-1)是X(k|k-1)對應(yīng)的covariance，P(kT|kT)是X(kT|kT)對應(yīng)的covariance，A'表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過程的covariance。式子1,2就是卡爾曼濾波器5個(gè)公式當(dāng)中的前兩個(gè)，也就是對系統(tǒng)的預(yù)測?，F(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測結(jié)果，然后我們再收集現(xiàn)在狀態(tài)的測量值。結(jié)合預(yù)測值和測量值，我們可以得到現(xiàn)在狀態(tài)(k)的最優(yōu)化估算值X(k|k)：TOC\o"1-5"\h\zX(k|k)二X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1)) (3)其中Kg為卡爾曼增益(KalmanGain)：Kg(k)=P(k|k-1)H'/(HP(k|k-1)H'+R) (4)到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k|k)。但是為了要另卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的covarianee:P(k|k)=(I-Kg(k)H)P(k|k—1) (5)其中I為1的矩陣，對于單模型單測量，1=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，P(k|k)就是式子(2)的P(k-I|k-1)。這樣，算法就可以自回歸的運(yùn)算下去?？柭鼮V波器的原理基本描述了，式子1,2,3,4和5就是他的5個(gè)基本公式。根據(jù)這5個(gè)公式，可以很容易的實(shí)現(xiàn)計(jì)算機(jī)的程序。下面，用Matlab程序舉一個(gè)實(shí)際運(yùn)行的例子。4.簡單例子(ASimpleExample)這里我們結(jié)合第二第三節(jié)，舉一個(gè)非常簡單的例子來說明卡爾曼濾波器的工作過程。所舉的例子是進(jìn)一步描述第二節(jié)的例子，而且還會(huì)配以程序模擬結(jié)果。根據(jù)第二節(jié)的描述，把房間看成一個(gè)系統(tǒng)，然后對這個(gè)系統(tǒng)建模。當(dāng)然，我們見的模型不需要非常地精確。我們所知道的這個(gè)房間的溫度是跟前一時(shí)刻的溫度相同的，所以A=1。沒有控制量，所以U(k)=0。因此得出：TOC\o"1-5"\h\zX(k|k—1)=X(k—l|k—1) (6)式子(2)可以改成：P(k|k—1)=P(k—l|k—1)+Q (7)因?yàn)闇y量的值是溫度計(jì)的，跟溫度直接對應(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)在我們模擬一組測量值作為輸入。假設(shè)房間的真實(shí)溫度為25度，我模擬了200個(gè)測量值，這些測量值的平均值為25度，但是加入了標(biāo)準(zhǔn)偏差為幾度的高斯白噪聲(在圖中為藍(lán)線)。為了令卡爾曼濾波器開始工作，我們需要告訴卡爾曼兩個(gè)零時(shí)刻的初始值，是X(0|0)和P(0|0)。他們的值不用太在意，隨便給一個(gè)就可以了，因?yàn)殡S著卡爾曼的工作，X會(huì)逐漸的收斂。但是對于P,—般不要取0,因?yàn)檫@樣可能會(huì)令卡爾曼完全相信你給定的x(o|o)是系統(tǒng)最優(yōu)的，從而使算法不能收斂。我選了X(0|0)=1度，P(0|0)=10。該系統(tǒng)的真實(shí)溫度為25度，圖中用黑線表示。圖中紅線是卡爾曼濾波器輸出的最優(yōu)化結(jié)果(該結(jié)果在算法中設(shè)置了Q=1e-6,R=1eT)。clearN=200;w(1)=0;w=randn(1,N)x(1)=0;a=1;fork=2:N;x(k)二a*x(kT)+w(kT);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;丫二c*x+V;p(1)=0；s(1)=0;fort=2:N;p1(t)二a.八2*p(tT)+Rww;b(t)二c*p1(t)/(c.八2*p1(t)+Rvv);s(t)二a*s(tT)+b(t)*(Y(t)-a*c*s(tT));p(t)=p1(t)-c*b(t)*p1(t);endt=1:N;plot(t,s,'r',t,Y,'g',t,x,'b');用matlab做的kalman濾波程序，已通過測試還有下面一個(gè)Matlab源程序，顯示效果更好。clearclc;N=300;CON=25;%房間溫度，假定溫度是恒定的%%%%%%%%%%%%%%%kalmanfilter%%%%%%%%%%%%%%%%%%%%%%x=zeros(1,N);y=295*randn(1,N)+CON;%加過程噪聲的狀態(tài)輸出x(1)=1;p=10；Q=cov(randn(1,N));%過程噪聲協(xié)方差R=cov(randn(1,N));%觀測噪聲協(xié)方差fork=2:Nx(k)=x(k一1);%預(yù)估計(jì)k時(shí)刻狀態(tài)變量的值p=p+Q;%對應(yīng)于預(yù)估值的協(xié)方差kg=p/(p+R);%kalmangainx(k)=x(k)+kg*(y(k)一x(k));p=(1-kg)*p;end%%%%%%%%%%%SmoothnessFilter%%%%%%%%%%%%%%%%%%%%%%%%Filter_Wid=10;smooth_res=zeros(1,N);fori=Filter_Wid+1:Ntempsum=0;forj=i-Filter_Wid:i-1tempsum=tempsum+y(j);endsmooth_res(i)=tempsum/Filter_Wid;end%figure(1);%hist(y);t=1:N;figure(1);expValue=zeros(1,N);fori=1:NexpValue(i)=CON;endplot(t,expValue,'r',t,x,'g',t,y,'b',t,smooth_res,'k');legend('expected','estimate','measure','smoothresult');

axis([0N2030])xlabel('Sampletime');ylabel('RoomTemperature');title('SmoothfilterVSkalmanfilter');3029SmooflifilterVSkalnnanfiSerexacted estima^ mRasirftB76543222222EB罟<|幕3029SmooflifilterVSkalnnanfiSerexacted estima^ mRasirftB76543222222EB罟<|幕1￡3^-22500D150|::|2::25初始值耳卡爾曼濾波算法一核心公式推導(dǎo)導(dǎo)論計(jì)算未考慮噪芮均方?jīng)Q差陣島寫在最前面：這是我第一篇專欄文章，感謝知乎提供這么一個(gè)平臺(tái)，讓自己能和大家分享知識(shí)。本人會(huì)不定期的開始更新文章，文章的內(nèi)容應(yīng)該集中在汽車動(dòng)力學(xué)控制，整車軟件架構(gòu)，控制器等方面。作為一名在校碩士，很多理解都可能不全面，不正確，大家有不同意見歡迎討論。謝謝！卡爾曼濾波算法很牛逼，因?yàn)橛幸欢压?，有一堆符?hào)，看起來就很牛逼啊，乍一看不懂的都很牛逼??！本文針對卡爾曼濾波算法的核心公式進(jìn)行推導(dǎo)，不讓大家被它華麗的外表嚇到。（之后計(jì)劃寫關(guān)于針對非線性情況的EKF和UKF，對卡爾曼濾波算法做一個(gè)全面一點(diǎn)的應(yīng)用介紹。感興趣的可以關(guān)注專欄。）Okay，進(jìn)入正題。這篇文章假設(shè)讀者已經(jīng)對卡爾曼濾波算法有初步的了解，知道它能做什么，知道它的優(yōu)點(diǎn)，知道它很牛逼，并且你已經(jīng)對它產(chǎn)生興趣，但不知道如何下手。首先給出一個(gè)控制理論中公式，別急著翻控制理論的書，沒那么復(fù)雜：狀態(tài)方程:=4竝-I+叫“量測方程：yk^Ckxk+uk兩個(gè)基本問題：1卡爾曼濾波算法要做什么？對狀態(tài)進(jìn)行估計(jì)。2.卡爾曼濾波算法怎么對狀態(tài)進(jìn)行估計(jì)？利用狀態(tài)過程噪聲和測量噪聲對狀態(tài)進(jìn)行估計(jì)。一個(gè)狀態(tài)在一個(gè)時(shí)刻點(diǎn)k的狀態(tài)進(jìn)入下一個(gè)時(shí)刻點(diǎn)k+1狀態(tài)，會(huì)有很多外界因素的干擾，我們把干擾就叫做過程噪聲，（這個(gè)詞一看就是硬翻譯過來的，別在意為什么叫噪聲）用w表示。任何一個(gè)測量儀器，都會(huì)有誤差，我們把這個(gè)誤差叫做量測噪聲，用v表示。

回到上面那個(gè)公式，狀態(tài)方程表示狀態(tài)在不斷的更新，從一個(gè)時(shí)刻點(diǎn)進(jìn)入下一個(gè)時(shí)刻點(diǎn)，這個(gè)很好理解。關(guān)鍵是量測方程，它表示，我們不斷更新的狀態(tài)有幾個(gè)能用測量儀器測出來，比如，汽車運(yùn)動(dòng)狀態(tài)參數(shù)有很多，比如速度，輪速，滑移率等，但是我們只能測量出輪速，因此量測方程要做的就是把狀態(tài)參數(shù)中能量測的狀態(tài)拿出來。我們始終要記得我們要做的事：我們要得到的是優(yōu)化的狀態(tài)量Xk。理解了上面之后就可以開始推導(dǎo)公式了。1首先不考慮過程噪聲對狀態(tài)進(jìn)行更新，很簡單：心心―舉個(gè)例子，v(k)=v(k-1)+at，勻加速運(yùn)動(dòng)咯。2?不考慮測量噪聲取出能測量的狀態(tài)，也很簡單:-13?用測量儀器測量出來的狀態(tài)值(大家可以考慮-13?用測量儀器測量出來的狀態(tài)值(大家可以考慮到：測量的值就是被各種噪聲干擾后的真實(shí)值)減去上面不考慮噪聲得到的測量值：這個(gè)值在數(shù)學(xué)上是一個(gè)定義值，叫做新息，有很多有趣的性質(zhì)，感興趣的可以自己谷歌。我們對步驟暫且停一停。這個(gè)叫新息的值有什么用？由上面的過程我們可以明顯看到，它反映了過程噪聲和測量噪聲綜合對測量狀態(tài)值的影響，也就是它包含了w和v的情況?；氐綌?shù)學(xué)層面，(不要害怕，很簡單的數(shù)學(xué)應(yīng)用和思考啦！)一個(gè)數(shù)值c由兩部分內(nèi)容a和b組成，那么怎樣用數(shù)學(xué)表達(dá)式來表達(dá)？一般有兩種做法：直接相加：c=a+b;用比例的方法：a=n*c,b=(1-n)*c卡爾曼采用了方法II，用比例的方法來做(其實(shí)這也是為什么叫做濾波的原因,

因?yàn)闉V波就是給權(quán)值之類的操作）。也就是說，過程噪聲w二新息*一個(gè)比例。這樣得到的過程噪聲加上原來（第一步）不考慮過程噪聲的狀態(tài)值不就是優(yōu)化值了嗎？也就是：耳=4^-1+H仇=Ahxk_l+H,（yk-（\.Afcxk_l）Okay，都寫到這里了，有必要做一下前提假設(shè)：什么高斯噪聲，均值為零一堆；Ak,Ck,wk的協(xié)方差Q,vk的協(xié)方差R,系統(tǒng)協(xié)方差初始值P0,狀態(tài)初始值X0,都已知。為什么已知，你實(shí)際做項(xiàng)目就知道了。不過不懂的可以留言或者私信。那么到目前為止我們的思路就是清楚了，找到一個(gè)合適的Hk值（卡爾曼增益），那么我們就能得到狀態(tài)的最優(yōu)值。（卡爾曼說的，不是我說的，所以你問為什么,你要問他，這么深層次的理論留給博士和學(xué)者們?nèi)プ鼍秃茫覀兙同F(xiàn)學(xué)現(xiàn)用就行,哈哈哈，站在巨人的肩膀?。﹩栴}來了：怎么得到合適的Hk?似乎不是隨便一個(gè)參數(shù)。這是誤差協(xié)方差矩陣。這是誤差協(xié)方差矩陣。思路：使得誤差協(xié)方差矩陣Pk最小的Hk。為什么？這里我從感觀的角度說明自己的理解，歡迎討論。協(xié)方差表示什么，協(xié)方差表示兩者之間的聯(lián)系或者關(guān)系，關(guān)系越大，協(xié)方差越大。誤差協(xié)方差越小說明過程噪聲和量測噪聲的關(guān)系越小。關(guān)系越小能做什么，這要回到我們第3步討論的我們用比例的方法分開了w和v。用比例分開，到底多少屬于w,多少是v,如果關(guān)系越小，分開的越精確，比如一堆白砂糖和鹽，如果兩種混合的很均勻，我們說它關(guān)系很大，也就越難用比例的方法將其分開。驟我們也容易得到:4.求的誤差協(xié)方差矩陣Pk驟我們也容易得到:自然是把里面的Xk先得到，然后公式運(yùn)算，通過上面的步

專皿+叫f 猛」+比US殆+叫l(wèi))+片戰(zhàn)｝=4UiO+ffA4氐％J+叫用點(diǎn)叫叩叫G)4tl*i￡.J-(八 ■盡q"也G)［汕(柏知)+叫』刃心然后復(fù)雜的數(shù)學(xué)計(jì)算，和之前假設(shè)的高斯噪聲，新息的性質(zhì)之類(至于過程，個(gè)人覺得你如果只做應(yīng)用，不研究算法，就沒必要深入去看了)，就能得到下面的卡爾曼濾波遞推公式：h嚴(yán)p"(cWR$

片=人％￡十Qg

瑰=Q?HGW通過上面的解釋，我們也就不難知道這些公式都在干嘛，知道干嘛就可以了。在知道A,C,PO,Q,R的情況下，整個(gè)公式的運(yùn)算流程也都很清晰了。過程方程:>>>>式1>>>>式2X(k+1)二AX(k)+BU(k)+W(k)>>>>式1>>>>式2量測方程：Z(k+1)二HX(k+1)+V(k+1)A和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行的單列矩陣。說明：下面的表達(dá)式中，不帶前綴的量都代表實(shí)際量，其小括號(hào)里面的k”或“k+1”代表該量是第k或第k+1時(shí)刻的實(shí)際量，如“Z(k+1)”就代表第k+1時(shí)刻的實(shí)際測量值；帶前綴““的量都代表預(yù)測量，如果小括號(hào)里面是'k+1|k”，就代表k+1時(shí)刻的先驗(yàn)預(yù)測值，如果小括號(hào)里面是“k+1|k+1”，就代表k+1時(shí)刻的后驗(yàn)預(yù)測值；(測量值可以通過測量得到，所以只有先驗(yàn)預(yù)測，沒有后驗(yàn)預(yù)測。而實(shí)際狀態(tài)值無法得知，既有先驗(yàn)預(yù)測，又有后驗(yàn)預(yù)測)帶前綴“~”的量都代表與預(yù)測值對應(yīng)的偏差值。實(shí)際狀態(tài)值與先驗(yàn)預(yù)測狀態(tài)值的偏差二實(shí)際狀態(tài)值-先驗(yàn)預(yù)測狀態(tài)值~X(k+l|k) 二 X(k+1) — 八X(k+I|k)>>>>式3實(shí)際測量值與先驗(yàn)預(yù)測測量值的偏差二當(dāng)前測量值-先驗(yàn)預(yù)測測量值~Z(k+l|k)二Z(k+1) — 八Z(k+I|k)>>>>式4并且先驗(yàn)預(yù)測測量值二轉(zhuǎn)換矩陣H*先驗(yàn)預(yù)測狀態(tài)值八Z(k+I|k)二 H八X(k+I|k)>>>>式5得到測量值后，再對當(dāng)前狀態(tài)值X(k+1)進(jìn)行后驗(yàn)預(yù)測(設(shè)后驗(yàn)預(yù)測值為八Z(k+I|k+1)),則后驗(yàn)預(yù)測值(同時(shí)也是最終預(yù)測值)的偏差為~X(k+l|k+1)二 X(k+1) — 八X(k+I|k+1)>>>>式6為了得到當(dāng)前狀態(tài)值X(k+1),根據(jù)式3,需要：X(k+1)二 八X(k+I|k) +~X(k+l|k)>>>>式7上式中，我們可以通過卡爾曼公式1(見附注2)計(jì)算出八X(k+I|k),但我們無法得知實(shí)際狀態(tài)值X(k+1),因而~X(k+l|k)也無法得知。我們最終的目的是得出一個(gè)比較接近實(shí)際狀態(tài)值X(k+1)的濾波值八X(k+I|k+1),根據(jù)式7,只要能準(zhǔn)確的估計(jì)出~X(k+l|k)即可。~X(k+l|k)本身雖無法得知，但~Z(k+l|k)卻可以通過測量得到，而且它們二者存在一定的相關(guān)性。不妨再設(shè)存在一個(gè)矩陣K(m行n列矩陣)，能使得~X(k+l|k)二K*~Z(k+l|k)>>>>式8那么最終的預(yù)測任務(wù)其實(shí)就是找到Ko由于~X(k+l|k)和~Z(k+l|k)都是單列矩陣，因此不難看出，滿足式8的矩陣K應(yīng)有無窮多個(gè)。矩陣K中第i行第j列反映了量測變量偏差矩陣~Z(k+l|k)的第j個(gè)元素對狀態(tài)變量偏差矩陣~X(k+l|k)的第i個(gè)元素的貢獻(xiàn)。因此矩陣K

的物理意義很明顯，K的第i行第j列的元素表示：對于第i個(gè)待測的狀態(tài)量來說，第j個(gè)測量儀器測到的偏差的可信度。某個(gè)測量值對應(yīng)的可信度越高，濾波器越“相信”該測量值既然滿足條件的K有無窮多個(gè)，那應(yīng)該使用哪個(gè)K呢？實(shí)際上，我們并不知道~X（k+1|k）的值，所以也就無法直接計(jì)算出K,而只能通過某種方法找到一個(gè)Kg,使得將Kg帶入式8后，等號(hào)兩邊的差（的平方）的期望盡可能小。我們最終的預(yù)測值或?yàn)V波值是后驗(yàn)預(yù)測值八X（k+I|k+1）,因此最后的預(yù)測也應(yīng)使~X（k+l|k+1）的期望為0且方差最小（這與讓8式兩端的差最小是一致的，下面的式9體現(xiàn)了這一點(diǎn)），這樣預(yù)測值才最可靠。下面詳細(xì)說明。八X(k+I|k+1)二八X(k+I|k)+Kg*~Z(k+l|k)八X(k+I|k+1)二八X(k+I|k)+Kg*~Z(k+l|k)（后驗(yàn)預(yù)測的狀態(tài)值）~X(k+l|k+1)二偏差)X(k+1)八X(k+I|k+1)（后驗(yàn)預(yù)測的~X(k+l|k+1)X(k+1)八~X(k+l|k+1)X(k+1)八X(k+I|k+1)八X(k+I|k) + ~X(k+l|k)八X(k+I|k) +八X(k+I|k) +Kg*~Z(k+l|k)Kg*~Z(k+l|k) 二Kg*~Z(k+l|k) 二V(k+1))「X(k+I|k) ) +V(k+1)X(k+I|k)+V(k+1)>>>>式10>>>>式9Z(k+1)八Z(k+I|k)(HX(k+1)+( H八X(k+I|k))H(X(k+1)~X(k+l|k)~Z(k+l|k)接下來的分析中，為了更直觀的說明卡爾曼濾波的原理，我們用幾何方法來解釋。這時(shí),~X和~z矩陣中的每個(gè)元素應(yīng)看做向量空間中的一個(gè)向量而不再是一個(gè)單純的數(shù)。這個(gè)向量空間（統(tǒng)計(jì)測試空間）可以看成無窮多維的，每一個(gè)維對應(yīng)一個(gè)可能的狀態(tài)。~滅和~乙矩陣中的每個(gè)元素向量都是由所有可能的狀態(tài)按照各自出現(xiàn)的概率組合而成(在測量之前，~x和~乙的實(shí)際值都是不可知的)°~X和~乙中的每個(gè)元素向量都應(yīng)是0均值的，他們與自己的內(nèi)積就是他們的協(xié)方差矩陣。我們無法給出~X和-z中每個(gè)元素向量的具體表達(dá)，但我們通過協(xié)方差矩陣就可以知道所有元素向量的模長，以及相互之間的夾角(從內(nèi)積計(jì)算)。為了方便用幾何方法解釋，我們假設(shè)狀態(tài)變量X是一個(gè)1行1列的矩陣(即只有一個(gè)待測狀態(tài)量)，而量測變量Z是一個(gè)2行1列的矩陣(即有兩個(gè)測量儀器，共同測量同一個(gè)狀態(tài)量X),也就是說，m=1,n=2。矩陣X中只有X[1]-項(xiàng)，矩陣Z中有Z[1]和Z[2]兩項(xiàng)。Kg此時(shí)應(yīng)是一個(gè)1行2列的矩陣，兩個(gè)元素分別記作Kg1和Kg2。H和V此時(shí)應(yīng)是一個(gè)2行1列的矩陣。將矩陣表達(dá)式9和10按元素展開：~X(k+1|k+1)[1]二 ~X(k+1|k)[1] —(Kg1*~Z(k+1|k)[1]+Kg2*~Z(k+1|k)[2])>>>>式9i~Z(k+1|k)[i]二 H[i]~X(k+1|k) + V(k+1)[i]>>>>式10i~X(k+1|k)中各個(gè)元素(向量)的線性組合可以產(chǎn)生一個(gè)m維或更低維的向量子空間Vx,這里，按照我們的假設(shè)，m=1，所以Vx應(yīng)是一維的；同時(shí)V(k+1)中的各個(gè)元素(向量)的線性組合也可以產(chǎn)生一個(gè)n維或更低維的向量子空間Vv,這里，按照我們的假設(shè)，n=2，所以Vv應(yīng)是二維的。由于V(k+1)中的每一項(xiàng)與~X(k+1|k)中的每一項(xiàng)都不相關(guān)(見附注1),故這兩個(gè)子空間相互垂直。如下圖所示。式10i所體現(xiàn)的~Z(k+1|k)[i]、H[i]~X(k+1|k)、V(k+1)[i]三者之間的幾何關(guān)系，也在下圖中描繪了出來。

Vx空閶L餌華觀I從上圖中可以看出，~z(k+l|k)中各個(gè)元素(向量)的線性組合也可以產(chǎn)生一個(gè)n維或更低維的向量子空間Vz,這里已假設(shè)n=2，所以Vz是一個(gè)二維的平面，就是上圖中兩條紅色的線所構(gòu)成的平面。Vx空KJ圖2中(注意此圖中的橢圓代表的是Vz空間，而圖1中則代表Vv空間，二者不一樣)，粉色的向量就是Kg1*~Z(k+1|k)[1]+Kg2*~Z(k+1|k)[2]，記此粉色向量為Y，Y為~Z(k+1|k)[1]和~乙低+1比)[2]線性組合而成，它始終在子空間Vz中。根據(jù)式9i，~X(k+1|k+1)[1]等于~X(k+1|k)[1]和Y的差向量,為使~X(k+l|k+1)[1]長度最短(協(xié)方差最小)，Kg的選取應(yīng)使得~X(k+l|k+1)[1]垂直于V空間。通過先驗(yàn)預(yù)測的協(xié)方差矩陣(見卡爾曼公式2),可以得到~X(k+1|k)中各個(gè)元素的模長以及彼此間的夾角。這是因?yàn)閰f(xié)方差矩陣中的第i行第j列其實(shí)就代表T~X(k+1|k)中第i個(gè)元素向量與第j個(gè)元素向量的內(nèi)積。通過測量可以得到新息協(xié)方差(見卡爾曼公式3的分母部分)，進(jìn)而可以知道~Z(k+1|k)中各個(gè)元素的模長以及彼此間的夾角。通過已知的量測噪聲協(xié)方差矩陣R，可以得出V(k+1)中各個(gè)元素的模長以及彼此間的夾角。最后根據(jù)~X(k+1|k+1)[1]與Y垂直以及圖1中所示的幾何關(guān)系，用高中學(xué)的立體幾何和向量知識(shí)就可以求得兩個(gè)Kg的值了。如果將向量的內(nèi)積都用協(xié)方差矩陣表示，就會(huì)發(fā)現(xiàn)，我們最后求得的Kg，其實(shí)就是卡爾曼公式3。(上面討論的是較低次的卡爾曼濾波，只有一個(gè)待測量，兩個(gè)測量儀器。這種情況還是比較常見的，比如傾角測量系統(tǒng)中，我們用加速度計(jì)和陀螺儀共同測量傾角。對于更高次的卡爾曼濾波，X和Z都是多行矩陣時(shí)，用幾何方法已經(jīng)無法直觀解釋，只能用矩陣分析的方法證明。求解Kg的詳細(xì)過程參考《卡爾曼濾波器及其應(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中的元素，都代表測量儀器測到的偏差的可信度，或者叫估計(jì)權(quán)重。附注1:(a). v(k+1)中的每一項(xiàng)與~X(k+1|k)中的每一項(xiàng)都不相關(guān)~X(k+1|k)=八x(k+l|k)X(k+1)(A八X(k|k) +BU(k))X(k+1)W(k))-((AX(k)+AX(k)+BU(k)+BU(k)-A~X(k|k))=W(k) +A~X(k|k)= W(k)+A( ~x(k|k-1)-Kg(k)*~Z(k|k-1)) ——這一步利用了式9= W(k)+A( ~X(k|k-1)一Kg(k)*(H~X(k|k-1)+v(k))) 這一步利用了式10= W(k)一AKg(k)*v(k) +A(I-Kg(k)*H) ~X(k|k-1)上式最后一行出現(xiàn)了~X(k|k-1),可見~X(k+1|k)可以遞歸表示。而且遞歸式中的過程噪聲W(k)與v(k+1)不相關(guān)，同時(shí)由于v本身是白噪聲，所以v(k+1)與v(k)亦不相關(guān)(白噪聲的自相關(guān)是&函數(shù))，因此通過遞推式可以判斷v(k+1)與~X(k+l|k)不相關(guān)。(b). w(k+1)中的每一項(xiàng)與~X(k+l|k+1)中的每一項(xiàng)都不相關(guān),w(k+1)中的每一項(xiàng)與~X(k+l|k)中的每一項(xiàng)都不相關(guān)。TOC\o"1-5"\h\z~X(k+l|k+1) = X(k+1) -八X(k+I|k+1)=(八X(k+I|k)+~X(k+l|k))一 (八X(k+I|k)+Kg(k+1)*~Z(k+l|k))= ~X(k+l|k) -Kg(k+1)*~Z(k+l|k)= ~X(k+l|k) -Kg(k+1)*(H~X(k+l|k)+v(k+1) )= -Kg(k+1)*v(k+1) + (I-Kg(k+1)*H)~X(k+l|k)我們已經(jīng)知道w(k+1)與v(k+1)不相關(guān)，因此只要~X(k+l|k+1)與上式的第二項(xiàng)也不相關(guān)，就說明結(jié)論(b)成立。根據(jù)(a)中的結(jié)論，~X(k+l|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)與~乂広+1|小不相關(guān)。進(jìn)而可知，w(k+1)與~X(k+l|k+1)不相關(guān)。正是因?yàn)?a)(b)中的兩個(gè)不相關(guān)，卡爾曼公式中的預(yù)測協(xié)方差矩(卡爾曼公式(2))和新息協(xié)方差矩陣(卡爾曼公式(3)中的“分母”部分)才可以是簡單的加式。附注2：卡爾曼濾波的五個(gè)公式先驗(yàn)預(yù)測值與先驗(yàn)預(yù)測協(xié)方差矩陣的計(jì)算。求解協(xié)方差時(shí)，都認(rèn)為預(yù)測值的期望是實(shí)際值。因此,X(k+l|k)的協(xié)方差矩陣同樣也是~X(k+l|k)的協(xié)方差矩陣,又因?yàn)槠額X(k+l|k)的期望是0,因此協(xié)方差矩陣反映了~X(k+l|k)在向量空間中的模長。注意，協(xié)方差矩陣都是對稱矩陣。X(k+l|k)=AX(k|k)+BU(k) (1)P(k+l|k)=AP(k|k)A'+Q(k) (2)卡爾曼增益矩陣的計(jì)算。量測預(yù)測值為Z(k+I|k)=HX(k+I|k)新息協(xié)方差見公式(3)中的“分母”部分。量測預(yù)測值的期望是實(shí)際量測值。因此，八Z(k+I|k)的協(xié)方差矩陣同樣也是~Z(k+l|k)的協(xié)方差矩陣，又因?yàn)槠額Z(k+l|k)的期望是0,因此協(xié)方差矩陣反映了~Z(k+l|k)在向量空間中的模長。Kg(k+1)=P(k+I|k)H'/(HP(k+I|k)H'+R(k+1))(3)后驗(yàn)預(yù)測值與后驗(yàn)預(yù)測協(xié)方差矩陣的計(jì)算X(k+I|k+1)=X(k+I|k)+Kg(k+1)(Z(k+1)-HX(k+l|k))⑷P(k+I|k+1)=(I-Kg(k+1)H)P(k+1|k) (5)clearN二200;w(1)=0;w=randn(1,N)x(1)=0;a=1;fork=2:N;x(k)二a*x(kT)+w(kT);endV=randn(1,N);q1二std(V);Rvv二q「2;q2=std(x);Rxx=q2."2;q3=std(w);Rww二q3.八2;

c=0.2;丫二c*x+V;P(1)=0；s(1)=0;fort=2:N;p1(t)二a.八2*p(tT)+Rww;b(t)二c*p1(t)/(c.八2*p1(t)+Rvv);s(t)二a*s(tT)+b(t)*(Y(t)-a*c*s(tT));p(t)=p1(t)-c*b(t)*p1(t);endt=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=[0,0,1];D=0;S二ss(A,B,C,D);Q=0.001;R=0.1;[kest,L,P]二kalman(S,Q,R);運(yùn)行程序，得到系統(tǒng)Kalman濾波器的增益矩陣L與估計(jì)誤差的協(xié)方差P為：L=1.06411.15662.0393P=0.06780.06640.10640.06640.06950.11570.10640.11570.2039卡爾曼濾波算法及MATLAB實(shí)現(xiàn)(2012-08-2921:39:56)轉(zhuǎn)載▼這一段時(shí)間對現(xiàn)代濾波進(jìn)行了學(xué)習(xí)，對自適應(yīng)濾波器和卡爾曼濾波器有了一定認(rèn)識(shí)，并對它們用MATLAB對語音信號(hào)進(jìn)行了濾波，發(fā)現(xiàn)卡爾曼濾波器還是比較有用，能夠在較大的噪聲中還原原來的信號(hào)。新的學(xué)期馬上就開始了，由于TI的開發(fā)板一直在維修，所以學(xué)習(xí)TI開發(fā)板的計(jì)劃擱置，但是對聲音信號(hào)的處理及濾波器的認(rèn)識(shí)有了進(jìn)一步提高。新的學(xué)期繼續(xù)努力！

卡爾曼濾波的基本思想是：以最小均方誤差為最佳估計(jì)準(zhǔn)則，采用信號(hào)與噪聲的狀態(tài)空間模型，利用前一時(shí)刻的估計(jì)值和當(dāng)前時(shí)刻的觀測值來更新對狀態(tài)變量的估計(jì)，求出當(dāng)前時(shí)刻的估計(jì)值，算法根據(jù)建立的系統(tǒng)方程和觀測方程對需要處理的信號(hào)做出滿足最小均方誤差的估計(jì)。語音信號(hào)在較長時(shí)間內(nèi)是非平穩(wěn)的，但在較短的時(shí)間內(nèi)的一階統(tǒng)計(jì)量和二階統(tǒng)計(jì)量近似為常量，因此語音信號(hào)在相對較短的時(shí)間內(nèi)可以看成白噪聲激勵(lì)以線性時(shí)不變系統(tǒng)得到的穩(wěn)態(tài)輸出。假定語音信號(hào)可看成由一AR模型產(chǎn)生：時(shí)間更新方程：P{t|t-l)=FP(t-L|t-l)FT+ohg^測量更新方程：x(t|t}=x(t|t-l}i-k(t)(z(t)-gTM(-t\t-l)yEfcW=p(-t|t-iW)gTPM"”K(t)為卡爾曼增益，其計(jì)算公式為：其中冷n)二［心-p+t)x(n-p+2)…x{n)pG討00...0討卩TOC\o"1-5"\h\z/0 ... 10 0\0 …01 0\0 ... 00 11、 分別為過程模型噪聲協(xié)方差和測量模型噪聲協(xié)方差，測量協(xié)方差可以通過觀測得到,則較難確定，在本實(shí)驗(yàn)中則通過與兩者比較得到。由于語音信號(hào)短時(shí)平穩(wěn)，因此在進(jìn)行卡爾曼濾波之前對信號(hào)進(jìn)行分幀加窗操作,在濾波之后對處理得到的信號(hào)進(jìn)行合幀，這里選取幀長為256,而幀重疊個(gè)數(shù)為128；下圖為原聲音信號(hào)與加噪聲后的信號(hào)以及聲音信號(hào)與經(jīng)卡爾曼濾波處理后的信號(hào):

廊崗晉哮號(hào)原聲音信號(hào)與加噪聲后的信號(hào)敢?*飾號(hào)廊崗晉哮號(hào)原聲音信號(hào)與加噪聲后的信號(hào)敢?*飾號(hào)原聲音信號(hào)與經(jīng)卡爾曼濾波處理后的信號(hào)MATLAB程序?qū)崿F(xiàn)如下：%%%%%%%%%%%%%%%%%基于LPC全極點(diǎn)模型的最大后驗(yàn)概率估計(jì)法，采用卡爾曼濾波%%%%%%%%%%%%%%clear;clc;%%%%%%%%%%%%%%%%%%%%%%%%%%%加載聲音數(shù)據(jù)%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%loadvoice.maty二m1(2,:);

x二y+0.08*randn(1,length(y));%%%%%%%%%%%%%%%原聲音信號(hào)和加噪聲后的信號(hào)%%%%%%%%%%%%%%%figured);subplot(211);plot(m1(1,:),m1(2,:));xlabel('時(shí)間');ylabel('幅度');title('原聲音信號(hào)')；subplot(212);plot(m1(1,:),x);xlabel('時(shí)間');ylabel('幅度');title('加噪聲后的信號(hào)')；%%%%%%%%%%%%%%%%%%%%%%%%瀚入?yún)?shù)%%%%%%%%%%%%%%%%%%%%%%%%%%%