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

,.一個應(yīng)用實例詳解卡爾曼濾波及其算法實現(xiàn)標(biāo)簽：算法filtermatlabalgorithm優(yōu)化工作謝謝閱讀2012-05-1410:4875511人閱讀評論(25)收藏舉報精品文檔放心下載分類：數(shù)據(jù)結(jié)構(gòu)及其算法（4）為了可以更加容易的理解卡爾曼濾波器，這里會應(yīng)用形象的描述方法來講解，而不是像大多數(shù)參考書那樣羅列一大堆的數(shù)學(xué)公式和數(shù)學(xué)符號。但是，他的5條公式是其核心內(nèi)容。結(jié)合現(xiàn)代的計算機，其實卡爾曼的程序相當(dāng)?shù)暮唵危灰憷斫饬怂哪?條公式。謝謝閱讀在介紹他的5條公式之前，先讓我們來根據(jù)下面的例子一步一步的探索。感謝閱讀假設(shè)我們要研究的對象是一個房間的溫度。根據(jù)你的經(jīng)驗判斷，這個房間的溫度是恒定的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設(shè)我們用一分鐘來做時間單位）。假設(shè)你對你的經(jīng)驗不是100%的相信，可能會有上下偏差幾度。我們把這些偏差看成是高斯白噪聲（WhiteGaussianNoise），也就是這些偏差跟前后時間是沒有關(guān)系的而且符合高斯分配（GaussianDistribution）。另外，我們在房間里放一個溫度計，但是這個溫度計也不準(zhǔn)確的，測量值會比實際值偏差。我們也把這些偏差看成是高斯白噪聲。感謝閱讀好了，現(xiàn)在對于某一分鐘我們有兩個有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗的預(yù)測值（系統(tǒng)的預(yù)測值）和溫度計的值（測量值）。下面我們要用這兩個值結(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度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的covariance（協(xié)方差）來判斷。因為Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我們可以估算出k時刻的實際溫度值是：23+0.78*(25-23)=24.56度。可以看出，因為溫度計的covariance比較?。ū容^相信溫度計），所以估算出的最優(yōu)溫度值偏向溫度計的值。感謝閱讀現(xiàn)在我們已經(jīng)得到k時刻的最優(yōu)溫度值了，下一步就是要進入k+1時刻，進行新的最優(yōu)估算。到現(xiàn)在為止，好像還沒看到什么自回歸的東西出現(xiàn)。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優(yōu)值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。這里的5就是上面的k時刻你預(yù)測的那個23度溫度值的偏差，得出的2.35就是進入k+1時刻以后k時刻估算出的最優(yōu)溫度值的偏差（對應(yīng)于上面的3）。謝謝閱讀就是這樣，卡爾曼濾波器就不斷的把covariance遞歸，從而估算出最優(yōu)的溫度值。他運行的很快，而且它只保留了上一時刻的covariance。上面的Kg，就是卡爾曼增益（KalmanGain）。他可以隨不同的時刻而改變他自己的值，是不是很神奇！精品文檔放心下載下面就要言歸正傳，討論真正工程系統(tǒng)上的卡爾曼。3．卡爾曼濾波器算法（TheKalmanFilterAlgorithm）謝謝閱讀在這一部分，我們就來描述源于DrKalman的卡爾曼濾波器。下面的描述，會涉及一些基本的概念知識，包括概率（Probability），隨即變量（RandomVariable），高斯或正態(tài)分配（GaussianDistribution）還有State-spaceModel等等。但對于卡爾曼濾波器的詳細(xì)證明，這里不能一一描述。謝謝閱讀首先，我們先要引入一個離散控制過程的系統(tǒng)。該系統(tǒng)可用一個線性隨機微分方程（LinearStochasticDifferenceequation）來描述：X(k)=AX(k-1)+BU(k)+W(k)感謝閱讀再加上系統(tǒng)的測量值：Z(k)=HX(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(k)是k時刻的測量值，H是測量謝謝閱讀系統(tǒng)的參數(shù)，對于多測量系統(tǒng)，H為矩陣。W(k)和V(k)分別表示過程和測量的噪聲。他們被假設(shè)成高斯白噪聲(WhiteGaussianNoise)，他們的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(k|k-1)=AX(k-1|k-1)+BU(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)=AP(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)過程的covariance。式子1，謝謝閱讀2就是卡爾曼濾波器5個公式當(dāng)中的前兩個，也就是對系統(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)-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)。但是為了要另卡爾曼濾波器不斷的運行下去直到系統(tǒng)過程結(jié)束，我們還要更新k狀態(tài)下X(k|k)謝謝閱讀covariance：P(k|k)=（I-Kg(k)H）P(k|k-1)………(5)精品文檔放心下載其中I為1的矩陣，對于單模型單測量，I=1。當(dāng)系統(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．簡單例子（ASimpleExample）這里我們結(jié)合第二第三節(jié)，舉一個非常簡單的例子來說明卡爾曼濾波器的工作過程。所舉的例子是進一步描述第二節(jié)的例子，而且還會配以程序模擬結(jié)果。精品文檔放心下載根據(jù)第二節(jié)的描述，把房間看成一個系統(tǒng)，然后對這個系統(tǒng)建模。當(dā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)在我們模擬一組測量值作為輸入。假設(shè)房間的真實溫度為25度，我模擬了200個測量值，這些測量值的平均值為25度，但是加入了標(biāo)準(zhǔn)偏差為幾度的高斯白噪聲（在圖中為藍線）。感謝閱讀為了令卡爾曼濾波器開始工作，我們需要告訴卡爾曼兩個零時刻的初始值，是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è)置了Q=1e-6，R=1e-1）。精品文檔放心下載clearN=200;w(1)=0;w=randn(1,N)x(1)=0;a=1;fork=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;fort=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);endt=1:N;plot(t,s,'r',t,Y,'g',t,x,'b');謝謝閱讀用matlab做的kalman濾波程序,已通過測試感謝閱讀－－－－－－－－－－－－－－－－－－－－－－－－－－謝謝閱讀還有下面一個Matlab源程序，顯示效果更好。clearclc;N=300;CON=25;%房間溫度，假定溫度是恒定的%%%%%%%%%%%%%%%kalmanfilter%%%%%%%%%%%%%%%%%%%%%%x=zeros(1,N);謝謝閱讀y=2^0.5*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ù)估計k時刻狀態(tài)變量的值精品文檔放心下載p=p+Q;%對應(yīng)于預(yù)估值的協(xié)方差kg=p/(p+R);%kalmangain謝謝閱讀x(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:N謝謝閱讀tempsum=0;forj=i-Filter_Wid:i-1感謝閱讀tempsum=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');謝謝閱讀,.卡爾曼濾波算法--核心公式推導(dǎo)導(dǎo)論再造紅旗寫在最前面：這是我第一篇專欄文章，感謝知乎提供這么一個平臺，讓自己能和大家分享知識。本人會不定期的開始更新文章，文章的內(nèi)容應(yīng)該集中在汽車動力學(xué)控制，整車軟件架構(gòu)，控制器等方面。作為一名在校碩士，很多理解都可能不全面，不正確，大家有不同意見歡迎討論。謝謝！精品文檔放心下載---------------------------------------------感謝閱讀卡爾曼濾波算法很牛逼，因為有一堆公式，有一堆符號，看起來就很牛逼啊，乍一看不懂的都很牛逼?。「兄x閱讀本文針對卡爾曼濾波算法的核心公式進行推導(dǎo)，不讓大家被它華麗的外表嚇到。（之后計劃寫關(guān)于針對非線性情況的EKF和UKF，對卡爾曼濾波算法做一個全面一點的應(yīng)用介紹。感興趣的可以關(guān)注專欄。）精品文檔放心下載-------------------------------------------------------------------------------感謝閱讀------------------------Okay，進入正題。這篇文章假設(shè)讀者已經(jīng)對卡爾曼濾波算法有初步的了解，知道它能做什么，知道它的優(yōu)點，知道它很牛逼，并且你已經(jīng)對它產(chǎn)生興趣，但不知道如何下手。精品文檔放心下載首先給出一個控制理論中公式，別急著翻控制理論的書，沒那么復(fù)雜：謝謝閱讀兩個基本問題：1.卡爾曼濾波算法要做什么？對狀態(tài)進行估計。卡爾曼濾波算法怎么對狀態(tài)進行估計？利用狀態(tài)過程噪聲和測量噪聲對狀態(tài)進行估計。一個狀態(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。精品文檔放心下載理解了上面之后就可以開始推導(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組成，那么怎樣用數(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，都已知。為什么已知，你實際做項目就知道了。不過不懂的可以留言或者私信。精品文檔放心下載那么到目前為止我們的思路就是清楚了，找到一個合適的Hk值（卡爾曼增益），那么我們就能得到狀態(tài)的最優(yōu)值。（卡爾曼說的，不是我說的，所以你問為什么，你要問他，這么深層次的理論留給博士和學(xué)者們?nèi)プ鼍秃?，我們就現(xiàn)學(xué)現(xiàn)用就行，哈哈哈，站在巨人的肩膀?。┚肺臋n放心下載問題來了：怎么得到合適的Hk？似乎不是隨便一個參數(shù)。謝謝閱讀這是誤差協(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先得到，然后公式運算，通過上面的步謝謝閱讀驟我們也容易得到：然后復(fù)雜的數(shù)學(xué)計算，和之前假設(shè)的高斯噪聲，新息的性質(zhì)之類（至于過程，個人覺得你如果只做應(yīng)用，不研究算法，就沒必要深入去看了），就能得到下面的卡爾曼濾波遞推公式：精品文檔放心下載通過上面的解釋，我們也就不難知道這些公式都在干嘛，知道干嘛就可以了。在知道A,C,P0,Q,R的情況下，整個公式的運算流程也都很清晰了。謝謝閱讀過程方程：X(k+1)=AX(k)+BU(k)+W(k) >>>>式1謝謝閱讀量測方程：,.Z(k+1)=HX（k+1）+V(k+1) >>>>式2精品文檔放心下載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行的單列矩陣。說明：下面的表達式中，不帶前綴的量都代表實際量，其小括號里面的“k”或“k+1”代精品文檔放心下載表該量是第k或第k+1時刻的實際量，如“Z(k+1)”就代表第k+1時刻的實際測量值；謝謝閱讀帶前綴“^”的量都代表預(yù)測量，如果小括號里面是“k+1|k”，就代表k+1時刻的先驗預(yù)精品文檔放心下載測值，如果小括號里面是“k+1|k+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) >>>>式3感謝閱讀實際測量值與先驗預(yù)測測量值的偏差=當(dāng)前測量值-先驗預(yù)測測量值感謝閱讀~Z(k+1|k)=Z(k+1)- ^Z(k+1|k) >>>>式4謝謝閱讀并且先驗預(yù)測測量值=轉(zhuǎn)換矩陣H*先驗預(yù)測狀態(tài)值感謝閱讀^Z(k+1|k)=H^X(k+1|k) >>>>式5感謝閱讀得到測量值后，再對當(dāng)前狀態(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) >>>>式6謝謝閱讀為了得到當(dāng)前狀態(tài)值X(k+1)，根據(jù)式3，需要：謝謝閱讀X(k+1)=^X(k+1|k)+~X(k+1|k) >>>>式7精品文檔放心下載上式中，我們可以通過卡爾曼公式1（見附注2）計算出^X(k+1|k)，但我們無法得知實際謝謝閱讀狀態(tài)值X(k+1)，因而~X(k+1|k)也無法得知。我們最終的目的是得出一個比較接近實際狀謝謝閱讀態(tài)值X(k+1)的濾波值^X(k+1|k+1)，根據(jù)式7，只要能準(zhǔn)確的估計出~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) >>>>式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的第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ì)說明。^X(k+1|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)= X(k+1) - ^X(k+1|k+1)精品文檔放心下載= (^X(k+1|k)+~X(k+1|k))- (^X(k+1|k)+Kg*謝謝閱讀~Z(k+1|k))= ~X(k+1|k) - Kg*~Z(k+1|k)謝謝閱讀>>>>式9~Z(k+1|k) = Z(k+1) - ^Z(k+1|k)感謝閱讀= (HX（k+1）+V(k+1)) - (H^X(k+1|k))謝謝閱讀= H(X（k+1）-^X(k+1|k))+V(k+1)感謝閱讀= H~X(k+1|k)+V(k+1) >>>>式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é)方差矩陣就可以知道所有元素向量的模長，以及相互之間的夾角（從內(nèi)積計謝謝閱讀算）。為了方便用幾何方法解釋，我們假設(shè)狀態(tài)變量X是一個1行1列的矩陣（即只有一個待測精品文檔放心下載狀態(tài)量），而量測變量Z是一個2行1列的矩陣（即有兩個測量儀器，共同測量同一個狀謝謝閱讀態(tài)量X），也就是說，m=1，n=2。矩陣X中只有X[1]一項，矩陣Z中有Z[1]和Z[2]兩項。感謝閱讀Kg此時應(yīng)是一個1行2列的矩陣，兩個元素分別記作Kg1和Kg2。H和V此時應(yīng)是一精品文檔放心下載個2行1列的矩陣。將矩陣表達式9和10按元素展開：~X(k+1|k+1)[1]~Z(k+1|k)[2])謝謝閱讀

=

~X(k+1|k)[1]

- (Kg1*~Z(k+1|k)[1]+Kg2*感謝閱讀>>>>式9i~Z(k+1|k)[i]

=

H[i]~X(k+1|k)

+

V(k+1)[i]

>>>>式10i~X(k+1|k)中各個元素（向量）的線性組合可以產(chǎn)生一個m維或更低維的向量子空間Vx，謝謝閱讀這里，按照我們的假設(shè)，m=1，所以Vx應(yīng)是一維的；同時V(k+1)中的各個元素（向量）謝謝閱讀的線性組合也可以產(chǎn)生一個n維或更低維的向量子空間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]、H[i]~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中（注意此圖中的橢圓代表的是Vz空間，而圖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個元素向量與第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)中，我們用加速度計和陀螺儀共同測量傾角。對于更高次的卡爾曼濾波，X和Z都是多行矩陣時，用幾何方法已經(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中的元素，都代表測量儀器測到的偏差的可信度，或者叫估計權(quán)重。感謝閱讀附注1：(a). v(k+1)中的每一項與~X(k+1|k)中的每一項都不相關(guān)精品文檔放心下載~X(k+1|k) = X(k+1) - ^X(k+1|k)精品文檔放心下載= X(k+1) -(A^X(k|k) +BU(k) )感謝閱讀=(AX(k)+BU(k)+W(k)) - (AX(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)，同時由于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) - ^X(k+1|k+1)精品文檔放心下載(^X(k+1|k)+~X(k+1|k))-(^X(k+1|k)+Kg(k+1)*~Z(k+1|k))謝謝閱讀= ~X(k+1|k) - Kg(k+1)*~Z(k+1|k)謝謝閱讀= ~X(k+1|k) - Kg(k+1)*(H~X(k+1|k)+v(k+1) )精品文檔放心下載,.= -Kg(k+1)*v(k+1)+ (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|k+1)不相關(guān)。謝謝閱讀正是因為(a)(b)中的兩個不相關(guān)，卡爾曼公式中的預(yù)測協(xié)方差矩（卡爾曼公式(2)）和新息協(xié)方差矩陣（卡爾曼公式(3)中的“分母”部分）才可以是簡單的加式。精品文檔放心下載附注2：卡爾曼濾波的五個公式先驗預(yù)測值與先驗預(yù)測協(xié)方差矩陣的計算。求解協(xié)方差時，都認(rèn)為預(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)=AX(k|k)+BU(k)(1)P(k+1|k)=AP(k|k)A’+Q(k)(2)卡爾曼增益矩陣的計算。量測預(yù)測值為,.Z(k+1|k)=HX(k+1|k)新息協(xié)方差見公式（3）中的“分母”部分。量測預(yù)測值的期望是實際量測值。謝謝閱讀因此，^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’/(HP(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)-HX(k+1|k))(4)感謝閱讀P(k+1|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(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;fort=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);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);運行程序，得到系統(tǒng)Kalman濾波器的增益矩陣L與估計誤差的協(xié)方感謝閱讀差為：L=1.06411.15662.0393P=0.0678 0.0664 0.10640.0664 0.0695 0.11570.1064 0.1157 0.2039卡爾曼濾波算法及MATLAB實現(xiàn)(2012-08-2921:39:56)轉(zhuǎn) 載 ▼這一段時間對現(xiàn)代濾波進行了學(xué)習(xí)，對自適應(yīng)濾波器和卡爾曼濾波器有了一定認(rèn)識，并謝謝閱讀對它們用MATLAB對語音信號進行了濾波，發(fā)現(xiàn)卡爾曼濾波器還是比較有用，能夠在較大謝謝閱讀的噪聲中還原原來的信號。新的學(xué)期馬上就開始了，由于TI的開發(fā)板一直在維修，所以學(xué)感謝閱讀習(xí)TI開發(fā)板的計劃擱置，但是對聲音信號的處理及濾波器的認(rèn)識有了進一步提高。新的學(xué)精品文檔放心下載期繼續(xù)努力！卡爾曼濾波的基本思想是：以最小均方誤差為最佳估計準(zhǔn)則，采用信號與噪聲的狀態(tài)空感謝閱讀間模型，利用前一時刻的估計值和當(dāng)前時刻的觀測值來更新對狀態(tài)變量的估計，求出當(dāng)前時精品文檔放心下載刻的估計值，算法根據(jù)建立的系統(tǒng)方程和觀測方程對需要處理的信號做出滿足最小均方誤差感謝閱讀的估計。語音信號在較長時間內(nèi)是非平穩(wěn)的，但在較短的時間內(nèi)的一階統(tǒng)計量和二階統(tǒng)計量近似為常精品文檔放心下載量，因此語音信號在相對較短的時間內(nèi)可以看成白噪聲激勵以線性時不變系統(tǒng)得到的穩(wěn)態(tài)輸精品文檔放心下載出。假定語音信號可看成由一AR模型產(chǎn)生：時間更新方程：測量更新方程：K(t)為卡爾曼增益，其計算公式為：其中、 分別為過程模型噪聲協(xié)方差和測量模型噪聲協(xié)方差，測量協(xié)方差可以通過觀測得到，感謝閱讀則較難確定，在本實驗中則通過與兩者比較得到。由于語音信號短時平穩(wěn)，因此在進行卡爾曼濾波之前對信號進行分幀加窗操作，在濾波精品文檔放心下載之后對處理得到的信號進行合幀，這里選取幀長為256，而幀重疊個數(shù)為128；謝謝閱讀下圖為原聲音信號與加噪聲后的信號以及聲音信號與經(jīng)卡爾曼濾波處理后的信號：精品文檔放心下載原聲音信號與加噪聲后的信號原聲音信號與經(jīng)卡爾曼濾波處理后的信號MATLAB程序?qū)崿F(xiàn)如下：%%%%%%%%%%%%%%%%%基于LPC全極點模型的最大后驗概率估計法,采用卡爾曼謝謝閱讀濾波%%%%%%%%%%%%%%clear;clc;%%%%%%%%%%%%%%%%%%%%%%%%%%%加載聲音數(shù)感謝閱讀據(jù)%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%謝謝閱讀loadvoice.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);plot(m1(1,:),x);xlabel('時間');ylabel('幅度');title('加噪聲后的信號');精品文檔放心下載%%%