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

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

2、分鐘的溫度（假設(shè)我們用一分鐘來(lái)做時(shí)間單位）。假設(shè)你對(duì)你的經(jīng)驗(yàn)不是100%的相信，可能會(huì)有上下偏差幾度。我們把這些偏差看成是高斯白噪聲（White Gaussian Noise），也就是這些偏差跟前后時(shí)間是沒(méi)有關(guān)系的而且符合高斯分配（Gaussian Distribution）。另外，我們?cè)诜块g里放一個(gè)溫度計(jì)，但是這個(gè)溫度計(jì)也不準(zhǔn)確的，測(cè)量值會(huì)比實(shí)際值偏差。我們也把這些偏差看成是高斯白噪聲。好了，現(xiàn)在對(duì)于某一分鐘我們有兩個(gè)有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗(yàn)的預(yù)測(cè)值（系統(tǒng)的預(yù)測(cè)值）和溫度計(jì)的值（測(cè)量值）。下面我們要用這兩個(gè)值結(jié)合他們各自的噪聲來(lái)估算出房間的實(shí)際溫度值。假如我們要估算k時(shí)刻的是實(shí)際溫度

3、值。首先你要根據(jù)k-1時(shí)刻的溫度值，來(lái)預(yù)測(cè)k時(shí)刻的溫度。因?yàn)槟阆嘈艤囟仁呛愣ǖ?，所以你?huì)得到k時(shí)刻的溫度預(yù)測(cè)值是跟k-1時(shí)刻一樣的，假設(shè)是23度，同時(shí)該值的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1時(shí)刻估算出的最優(yōu)溫度值的偏差是3，你對(duì)自己預(yù)測(cè)的不確定度是4度，他們平方相加再開(kāi)方，就是5）。然后，你從溫度計(jì)那里得到了k時(shí)刻的溫度值，假設(shè)是25度，同時(shí)該值的偏差是4度。由于我們用于估算k時(shí)刻的實(shí)際溫度有兩個(gè)溫度值，分別是23 度和25度。究竟實(shí)際溫度是多少呢？相信自己還是相信溫度計(jì)呢？究竟相信誰(shuí)多一點(diǎn)，我們可以用他們的covariance（協(xié)方差）來(lái)判斷。因?yàn)?Kg2=52/(52+42)

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

5、應(yīng)于上面的3）。就是這樣，卡爾曼濾波器就不斷的把 covariance遞歸，從而估算出最優(yōu)的溫度值。他運(yùn)行的很快，而且它只保留了上一時(shí)刻的covariance。上面的Kg，就是卡爾曼增益（Kalman Gain）。他可以隨不同的時(shí)刻而改變他自己的值，是不是很神奇！下面就要言歸正傳，討論真正工程系統(tǒng)上的卡爾曼。3 卡爾曼濾波器算法（The Kalman Filter Algorithm）在這一部分，我們就來(lái)描述源于Dr Kalman 的卡爾曼濾波器。下面的描述，會(huì)涉及一些基本的概念知識(shí)，包括概率（Probability），隨即變量（Random Variable），高斯或正態(tài)分配（Gaussia

6、n Distribution）還有State-space Model等等。但對(duì)于卡爾曼濾波器的詳細(xì)證明，這里不能一一描述。首先，我們先要引入一個(gè)離散控制過(guò)程的系統(tǒng)。該系統(tǒng)可用一個(gè)線性隨機(jī)微分方程（Linear Stochastic Difference equation）來(lái)描述：X(k)=A X(k-1)+B U(k)+W(k)再加上系統(tǒng)的測(cè)量值：Z(k)=H X(k)+V(k)上兩式子中，X(k)是k時(shí)刻的系統(tǒng)狀態(tài)，U(k)是k時(shí)刻對(duì)系統(tǒng)的控制量。A和B是系統(tǒng)參數(shù)，對(duì)于多模型系統(tǒng)，他們?yōu)榫仃?。Z(k)是k時(shí)刻的測(cè)量值，H是測(cè)量系統(tǒng)的參數(shù)，對(duì)于多測(cè)量系統(tǒng)，H為矩陣。W(k)和V(k)分別表示過(guò)

7、程和測(cè)量的噪聲。他們被假設(shè)成高斯白噪聲(White Gaussian Noise)，他們的covariance 分別是Q，R（這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化）。對(duì)于滿足上面的條件(線性隨機(jī)微分系統(tǒng)，過(guò)程和測(cè)量都是高斯白噪聲)，卡爾曼濾波器是最優(yōu)的信息處理器。下面我們來(lái)用他們結(jié)合他們的covariances 來(lái)估算系統(tǒng)的最優(yōu)化輸出（類似上一節(jié)那個(gè)溫度的例子）。首先我們要利用系統(tǒng)的過(guò)程模型，來(lái)預(yù)測(cè)下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測(cè)出現(xiàn)在狀態(tài)：X(k|k-1)=A X(k-1|k-1)+B U(k) . (1)式(1)中，X(k|k-1)是利

8、用上一狀態(tài)預(yù)測(cè)的結(jié)果，X(k-1|k-1)是上一狀態(tài)最優(yōu)的結(jié)果，U(k)為現(xiàn)在狀態(tài)的控制量，如果沒(méi)有控制量，它可以為0。到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對(duì)應(yīng)于 X(k|k-1)的covariance(協(xié)方差)還沒(méi)更新。我們用P表示covariance：P(k|k-1)=A P(k-1|k-1) A+Q (2)式(2)中，P(k|k-1)是X(k|k-1)對(duì)應(yīng)的covariance，P(k-1|k-1)是X(k-1|k-1)對(duì)應(yīng)的 covariance，A表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過(guò)程的covariance。式子1，2就是卡爾曼濾波器5個(gè)公式當(dāng)中的前兩個(gè)，也就是對(duì)系統(tǒng)的預(yù)測(cè)。現(xiàn)在我們

9、有了現(xiàn)在狀態(tài)的預(yù)測(cè)結(jié)果，然后我們?cè)偈占F(xiàn)在狀態(tài)的測(cè)量值。結(jié)合預(yù)測(cè)值和測(cè)量值，我們可以得到現(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)。但是為了要另卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過(guò)程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的covariance：P(k|k)=（I-Kg(k) H）P(k|k-1) (5)其中I 為1的矩陣

10、，對(duì)于單模型單測(cè)量，I=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，P(k|k)就是式子(2)的P(k-1|k-1)。這樣，算法就可以自回歸的運(yùn)算下去。卡爾曼濾波器的原理基本描述了，式子1，2，3，4和5就是他的5 個(gè)基本公式。根據(jù)這5個(gè)公式，可以很容易的實(shí)現(xiàn)計(jì)算機(jī)的程序。下面，用Matlab程序舉一個(gè)實(shí)際運(yùn)行的例子。4 簡(jiǎn)單例子（A Simple Example）這里我們結(jié)合第二第三節(jié)，舉一個(gè)非常簡(jiǎn)單的例子來(lái)說(shuō)明卡爾曼濾波器的工作過(guò)程。所舉的例子是進(jìn)一步描述第二節(jié)的例子，而且還會(huì)配以程序模擬結(jié)果。根據(jù)第二節(jié)的描述，把房間看成一個(gè)系統(tǒng)，然后對(duì)這個(gè)系統(tǒng)建模。當(dāng)然，我們見(jiàn)的模型不需要非常地精確。我們所知道的這個(gè)房

11、間的溫度是跟前一時(shí)刻的溫度相同的，所以A=1。沒(méi)有控制量，所以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àn)闇y(cè)量的值是溫度計(jì)的，跟溫度直接對(duì)應(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)在我們模擬一組測(cè)量值作為輸入。假設(shè)房間的真實(shí)溫度為25 度，我模擬了200個(gè)測(cè)量值，這些測(cè)量值的

12、平均值為25度，但是加入了標(biāo)準(zhǔn)偏差為幾度的高斯白噪聲（在圖中為藍(lán)線）。為了令卡爾曼濾波器開(kāi)始工作，我們需要告訴卡爾曼兩個(gè)零時(shí)刻的初始值，是X(0|0)和P(0|0)。他們的值不用太在意，隨便給一個(gè)就可以了，因?yàn)殡S著卡爾曼的工作，X會(huì)逐漸的收斂。但是對(duì)于 P，一般不要取0，因?yàn)檫@樣可能會(huì)令卡爾曼完全相信你給定的X(0|0)是系統(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=1e-1）。clearN=200;w(1)=0;w=randn(1,N)x(

13、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(t)*p1(t);endt=1:N;plot(t,s,r,t,Y,g,t,x,b);用matla

14、b做的kalman濾波程序,已通過(guò)測(cè)試還有下面一個(gè) Matlab源程序，顯示效果更好。clearclc;N=300;CON = 25;%房間溫度，假定溫度是恒定的%kalman filter%x = zeros(1,N);y = 20.5 * randn(1,N) + CON;%加過(guò)程噪聲的狀態(tài)輸出x(1) = 1;p = 10;Q = cov(randn(1,N);%過(guò)程噪聲協(xié)方差R = cov(randn(1,N);%觀測(cè)噪聲協(xié)方差for k = 2 : Nx(k) = x(k - 1);%預(yù)估計(jì)k時(shí)刻狀態(tài)變量的值p = p + Q;%對(duì)應(yīng)于預(yù)估值的協(xié)方差kg = p / (p + R);

15、%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);endsmooth_res(i) = tempsum / Filter_Wid;end% figure(1);% hist(y);t=1:N;figure(1);exp

16、Value = 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);axis(0 N 20 30)xlabel(Sample time);ylabel(Room Temperature);title(Smooth filter VS kalman filter);卡爾曼濾波算法-核心公式推導(dǎo)導(dǎo)論再造紅旗寫在最前面：這是我第一篇專欄文章，感謝知乎提供這么一個(gè)平臺(tái)，讓自己能和大

17、家分享知識(shí)。本人會(huì)不定期的開(kāi)始更新文章，文章的內(nèi)容應(yīng)該集中在汽車動(dòng)力學(xué)控制，整車軟件架構(gòu)，控制器等方面。作為一名在校碩士，很多理解都可能不全面，不正確，大家有不同意見(jiàn)歡迎討論。 謝謝！-卡爾曼濾波算法很牛逼，因?yàn)橛幸欢压?，有一堆符?hào)，看起來(lái)就很牛逼啊，乍一看不懂的都很牛逼??！本文針對(duì)卡爾曼濾波算法的核心公式進(jìn)行推導(dǎo)，不讓大家被它華麗的外表嚇到。（之后計(jì)劃寫關(guān)于針對(duì)非線性情況的EKF和UKF，對(duì)卡爾曼濾波算法做一個(gè)全面一點(diǎn)的應(yīng)用介紹。感興趣的可以關(guān)注專欄。）-Okay，進(jìn)入正題。這篇文章假設(shè)讀者已經(jīng)對(duì)卡爾曼濾波算法有初步的了解，知道它能做什么，知道它的優(yōu)點(diǎn)，知道它很牛逼，并且你已經(jīng)對(duì)它產(chǎn)生興趣

18、，但不知道如何下手。首先給出一個(gè)控制理論中公式，別急著翻控制理論的書，沒(méi)那么復(fù)雜：兩個(gè)基本問(wèn)題：1.卡爾曼濾波算法要做什么？對(duì)狀態(tài)進(jìn)行估計(jì)。2. 卡爾曼濾波算法怎么對(duì)狀態(tài)進(jìn)行估計(jì)？利用狀態(tài)過(guò)程噪聲和測(cè)量噪聲對(duì)狀態(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ì)有很多外界因素的干擾，我們把干擾就叫做過(guò)程噪聲，（這個(gè)詞一看就是硬翻譯過(guò)來(lái)的，別在意為什么叫噪聲）用w表示。任何一個(gè)測(cè)量?jī)x器，都會(huì)有誤差，我們把這個(gè)誤差叫做量測(cè)噪聲，用v表示?；氐缴厦婺莻€(gè)公式，狀態(tài)方程表示狀態(tài)在不斷的更新，從一個(gè)時(shí)刻點(diǎn)進(jìn)入下一個(gè)時(shí)刻點(diǎn)，這個(gè)很好理解。關(guān)鍵是量測(cè)方程，它表示，我們不斷更新的狀態(tài)有幾個(gè)能用

19、測(cè)量?jī)x器測(cè)出來(lái)，比如，汽車運(yùn)動(dòng)狀態(tài)參數(shù)有很多，比如速度，輪速，滑移率等，但是我們只能測(cè)量出輪速，因此量測(cè)方程要做的就是把狀態(tài)參數(shù)中能量測(cè)的狀態(tài)拿出來(lái)。我們始終要記得我們要做的事：我們要得到的是優(yōu)化的狀態(tài)量Xk。理解了上面之后就可以開(kāi)始推導(dǎo)公式了。1.首先不考慮過(guò)程噪聲對(duì)狀態(tài)進(jìn)行更新，很簡(jiǎn)單：舉個(gè)例子，v(k)=v(k-1)+at，勻加速運(yùn)動(dòng)咯。2.不考慮測(cè)量噪聲取出能測(cè)量的狀態(tài)，也很簡(jiǎn)單：3.用測(cè)量?jī)x器測(cè)量出來(lái)的狀態(tài)值（大家可以考慮到：測(cè)量的值就是被各種噪聲干擾后的真實(shí)值）減去上面不考慮噪聲得到的測(cè)量值：這個(gè)值在數(shù)學(xué)上是一個(gè)定義值，叫做新息，有很多有趣的性質(zhì)，感興趣的可以自己谷歌。我們對(duì)步驟暫

20、且停一停。這個(gè)叫新息的值有什么用？由上面的過(guò)程我們可以明顯看到，它反映了過(guò)程噪聲和測(cè)量噪聲綜合對(duì)測(cè)量狀態(tài)值的影響，也就是它包含了w和v的情況?；氐綌?shù)學(xué)層面，（不要害怕，很簡(jiǎn)單的數(shù)學(xué)應(yīng)用和思考啦?。┮粋€(gè)數(shù)值c由兩部分內(nèi)容a和b組成，那么怎樣用數(shù)學(xué)表達(dá)式來(lái)表達(dá)？一般有兩種做法：I.直接相加：c=a+b;II. 用比例的方法：a=n*c,b=(1-n)*c卡爾曼采用了方法II，用比例的方法來(lái)做（其實(shí)這也是為什么叫做濾波的原因，因?yàn)闉V波就是給權(quán)值之類的操作）。也就是說(shuō)，過(guò)程噪聲w=新息*一個(gè)比例。這樣得到的過(guò)程噪聲加上原來(lái)（第一步）不考慮過(guò)程噪聲的狀態(tài)值不就是優(yōu)化值了嗎？ 也就是：Okay，都寫到這里

21、了，有必要做一下前提假設(shè)：a. 什么高斯噪聲，均值為零一堆；b.Ak,Ck,wk的協(xié)方差Q,vk的協(xié)方差R，系統(tǒng)協(xié)方差初始值P0,狀態(tài)初始值X0，都已知。為什么已知，你實(shí)際做項(xiàng)目就知道了。不過(guò)不懂的可以留言或者私信。那么到目前為止我們的思路就是清楚了，找到一個(gè)合適的Hk值（卡爾曼增益），那么我們就能得到狀態(tài)的最優(yōu)值。（卡爾曼說(shuō)的，不是我說(shuō)的，所以你問(wèn)為什么，你要問(wèn)他，這么深層次的理論留給博士和學(xué)者們?nèi)プ鼍秃茫覀兙同F(xiàn)學(xué)現(xiàn)用就行，哈哈哈，站在巨人的肩膀?。﹩?wèn)題來(lái)了：怎么得到合適的Hk？似乎不是隨便一個(gè)參數(shù)。這是誤差協(xié)方差矩陣。思路：使得誤差協(xié)方差矩陣Pk最小的Hk。為什么？這里我從感觀的角度說(shuō)明

22、自己的理解，歡迎討論。協(xié)方差表示什么，協(xié)方差表示兩者之間的聯(lián)系或者關(guān)系，關(guān)系越大，協(xié)方差越大。誤差協(xié)方差越小說(shuō)明過(guò)程噪聲和量測(cè)噪聲的關(guān)系越小。關(guān)系越小能做什么，這要回到我們第3步討論的我們用比例的方法分開(kāi)了w和v。用比例分開(kāi)，到底多少屬于w，多少是v，如果關(guān)系越小，分開(kāi)的越精確，比如一堆白砂糖和鹽，如果兩種混合的很均勻，我們說(shuō)它關(guān)系很大，也就越難用比例的方法將其分開(kāi)。4.求的誤差協(xié)方差矩陣Pk自然是把里面的Xk先得到，然后公式運(yùn)算，通過(guò)上面的步驟我們也容易得到：然后復(fù)雜的數(shù)學(xué)計(jì)算，和之前假設(shè)的高斯噪聲，新息的性質(zhì)之類（至于過(guò)程，個(gè)人覺(jué)得你如果只做應(yīng)用，不研究算法，就沒(méi)必要深入去看了），就能得到

23、下面的卡爾曼濾波遞推公式：通過(guò)上面的解釋，我們也就不難知道這些公式都在干嘛，知道干嘛就可以了。在知道A,C,P0,Q,R的情況下，整個(gè)公式的運(yùn)算流程也都很清晰了。過(guò)程方程：X(k+1)=AX(k) +BU(k) + W(k) 式1量測(cè)方程：Z(k+1)=HX（k+1）+ V(k+1) 式2A和B是系統(tǒng)參數(shù)，對(duì)于多模型系統(tǒng)，他們?yōu)榫仃嚕籋是測(cè)量系統(tǒng)的參數(shù)，對(duì)于多測(cè)量系統(tǒng)，H為矩陣。W(k)和V(k)分別表示過(guò)程和測(cè)量的噪聲。他們被假設(shè)成高斯白噪聲，他們的協(xié)方差 分別是Q，R。為了不失一般性，下面的討論中將X,Z都視為矩陣，其中X是m行的單列矩陣，Z是n行的單列矩陣。說(shuō)明：下面的表達(dá)式中，不帶前綴

24、的量都代表實(shí)際量，其小括號(hào)里面的“k”或“k+1”代表該量是第k或第k+1時(shí)刻的實(shí)際量，如“Z(k+1)”就代表第k+1時(shí)刻的實(shí)際測(cè)量值；帶前綴“”的量都代表預(yù)測(cè)量，如果小括號(hào)里面是“k+1|k”，就代表k+1時(shí)刻的先驗(yàn)預(yù)測(cè)值，如果小括號(hào)里面是“k+1|k+1”，就代表k+1時(shí)刻的后驗(yàn)預(yù)測(cè)值；（測(cè)量值可以通過(guò)測(cè)量得到，所以只有先驗(yàn)預(yù)測(cè)，沒(méi)有后驗(yàn)預(yù)測(cè)。而實(shí)際狀態(tài)值無(wú)法得知，既有先驗(yàn)預(yù)測(cè)，又有后驗(yàn)預(yù)測(cè)）帶前綴“”的量都代表與預(yù)測(cè)值對(duì)應(yīng)的偏差值。實(shí)際狀態(tài)值與先驗(yàn)預(yù)測(cè)狀態(tài)值的偏差 = 實(shí)際狀態(tài)值 先驗(yàn)預(yù)測(cè)狀態(tài)值X(k+1|k) = X(k+1) - X(k+1|k) 式3實(shí)際測(cè)量值與先驗(yàn)預(yù)測(cè)測(cè)量值的偏

25、差 = 當(dāng)前測(cè)量值 - 先驗(yàn)預(yù)測(cè)測(cè)量值Z(k+1|k) =Z(k+1) - Z(k+1|k) 式4并且先驗(yàn)預(yù)測(cè)測(cè)量值 = 轉(zhuǎn)換矩陣H* 先驗(yàn)預(yù)測(cè)狀態(tài)值Z(k+1|k)=HX(k+1|k) 式5得到測(cè)量值后，再對(duì)當(dāng)前狀態(tài)值X(k+1) 進(jìn)行后驗(yàn)預(yù)測(cè)（設(shè)后驗(yàn)預(yù)測(cè)值為 Z(k+1|k+1) ） ，則后驗(yàn)預(yù)測(cè)值（同時(shí)也是最終預(yù)測(cè)值）的偏差為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上式中，我們可以通過(guò)卡爾曼公式1（見(jiàn)附注2）計(jì)算出X(k+1|k)，但我們無(wú)法得知實(shí)際狀

26、態(tài)值X(k+1)，因而X(k+1|k) 也無(wú)法得知。我們最終的目的是得出一個(gè)比較接近實(shí)際狀態(tài)值X(k+1)的濾波值X(k+1|k+1)，根據(jù)式7，只要能準(zhǔn)確的估計(jì)出X(k+1|k)即可。X(k+1|k)本身雖無(wú)法得知，但Z(k+1|k) 卻可以通過(guò)測(cè)量得到，而且它們二者存在一定的相關(guān)性。不妨再設(shè)存在一個(gè)矩陣K（m行n列矩陣），能使得X(k+1|k) = K * Z(k+1|k) 式8那么最終的預(yù)測(cè)任務(wù)其實(shí)就是找到K。由于X(k+1|k)和Z(k+1|k)都是單列矩陣，因此不難看出，滿足式8的矩陣K應(yīng)有無(wú)窮多個(gè)。矩陣K中第i行第j列反映了量測(cè)變量偏差矩陣Z(k+1|k)的第j個(gè)元素對(duì)狀態(tài)變量偏差

27、矩陣X(k+1|k)的第i個(gè)元素的貢獻(xiàn)。因此矩陣K的物理意義很明顯，K的第i行第j列的元素表示：對(duì)于第i個(gè)待測(cè)的狀態(tài)量來(lái)說(shuō)，第j個(gè)測(cè)量?jī)x器測(cè)到的偏差的可信度。某個(gè)測(cè)量值對(duì)應(yīng)的可信度越高，濾波器越“相信”該測(cè)量值。既然滿足條件的K有無(wú)窮多個(gè)，那應(yīng)該使用哪個(gè)K呢？實(shí)際上，我們并不知道X(k+1|k)的值，所以也就無(wú)法直接計(jì)算出K，而只能通過(guò)某種方法找到一個(gè)Kg，使得將Kg帶入式8后，等號(hào)兩邊的差（的平方）的期望盡可能小。我們最終的預(yù)測(cè)值或?yàn)V波值是后驗(yàn)預(yù)測(cè)值X(k+1|k+1)，因此最后的預(yù)測(cè)也應(yīng)使 X(k+1|k+1) 的期望為0且方差最?。ㄟ@與讓8式兩端的差最小是一致的，下面的式9體現(xiàn)了這一點(diǎn)）

28、，這樣預(yù)測(cè)值才最可靠。下面詳細(xì)說(shuō)明。X(k+1|k+1) = X(k+1|k) +Kg * Z(k+1|k) （后驗(yàn)預(yù)測(cè)的狀態(tài)值）X(k+1|k+1) = X(k+1) - X(k+1|k+1) （后驗(yàn)預(yù)測(cè)的偏差）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) 式9Z(k+1|k) = Z(k+1) - Z(k+1|k) = (HX（k+1）+ V(k+1) ) - (HX(k+1|k) ) =H( X（k+1）-X(

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

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

31、式10iX(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)（見(jiàn)附注1），故這兩個(gè)子空間相互垂直。如下圖所示。式10i所體現(xiàn)的Z(k+1|k)i、Hi X(k+1|k)、V(k+1)i 三者之間的幾何關(guān)系，也在下圖中描繪了出來(lái)。從上圖中可以看出，Z(k+1|k)中各個(gè)元素（向量）的線性組合也可以產(chǎn)生一個(gè)n維或

32、更低維的向量子空間Vz，這里已假設(shè)n=2，所以Vz是一個(gè)二維的平面，就是上圖中兩條紅色的線所構(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長(zhǎng)度最短（協(xié)方差最?。?，Kg的選取應(yīng)使得X(k+1|k+1)1垂直于Vz空間。通過(guò)先驗(yàn)預(yù)測(cè)的協(xié)方差矩陣（見(jiàn)卡爾曼公式2），可以得到X(k

33、+1|k)中各個(gè)元素的模長(zhǎng)以及彼此間的夾角。這是因?yàn)閰f(xié)方差矩陣中的第i行第j列其實(shí)就代表了X(k+1|k)中第i個(gè)元素向量與第j個(gè)元素向量的內(nèi)積。通過(guò)測(cè)量可以得到新息協(xié)方差（見(jiàn)卡爾曼公式3的分母部分），進(jìn)而可以知道Z(k+1|k)中各個(gè)元素的模長(zhǎng)以及彼此間的夾角。通過(guò)已知的量測(cè)噪聲協(xié)方差矩陣R，可以得出V(k+1) 中各個(gè)元素的模長(zhǎng)以及彼此間的夾角。最后根據(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è)

34、待測(cè)量，兩個(gè)測(cè)量?jī)x器。這種情況還是比較常見(jiàn)的，比如傾角測(cè)量系統(tǒng)中，我們用加速度計(jì)和陀螺儀共同測(cè)量?jī)A角。對(duì)于更高次的卡爾曼濾波，X和Z都是多行矩陣時(shí)，用幾何方法已經(jīng)無(wú)法直觀解釋，只能用矩陣分析的方法證明。求解Kg的詳細(xì)過(guò)程參考 卡爾曼濾波器及其應(yīng)用基礎(chǔ)國(guó)防工業(yè)出版社敬喜 編 ）卡爾曼濾波的核心過(guò)程，就是求解能使得E X(k+1|k+1)* X(k+1|k+1)取最小值的Kg增益矩陣的過(guò)程，X(k+1|k+1)代表的是X(k+1|k+1)的轉(zhuǎn)置（這里X(k+1|k+1)中的元素代表數(shù)值，不是向量）。前面已經(jīng)提到過(guò)，卡爾曼增益矩陣Kg中的元素，都代表測(cè)量?jī)x器測(cè)到的偏差的可信度，或者叫估計(jì)權(quán)重。附注1

35、：(a). v(k+1)中的每一項(xiàng)與X(k+1|k)中的每一項(xiàng)都不相關(guān)X(k+1|k) = X(k+1) - X(k+1|k) =X(k+1) -(AX(k|k) +BU(k) ) =(AX(k) +BU(k) + W(k) ) - (AX(k) +BU(k) -AX(k|k)= W(k) +AX(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)* (HX(k|k-1) + v(k) ) ) -這一步利用了式10 = W(k) -AKg(k) *v(k) +A(I- Kg(k) *

36、H) X(k|k-1)上式最后一行出現(xiàn)了X(k|k-1)，可見(jiàn)X(k+1|k)可以遞歸表示。而且遞歸式中的過(guò)程噪聲W(k)與v(k+1)不相關(guān)，同時(shí)由于 v本身是白噪聲，所以 v(k+1)與v(k)亦不相關(guān)（白噪聲的自相關(guān)是函數(shù)），因此通過(guò)遞推式可以判斷v(k+1)與X(k+1|k)不相關(guān)。(b). w(k+1)中的每一項(xiàng)與X(k+1|k+1)中的每一項(xiàng)都不相關(guān)，w(k+1)中的每一項(xiàng)與X(k+1|k)中的每一項(xiàng)都不相關(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

37、) = X(k+1|k) -Kg(k+1)*Z(k+1|k) = X(k+1|k) -Kg(k+1)* (HX(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)與上式的第二項(xiàng)也不相關(guān)，就說(shuō)明結(jié)論(b)成立。根據(jù)(a)中的結(jié)論，X(k+1|k)的遞歸展開(kāi)式中出現(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

38、，k-1) 也不相關(guān)，也就得出w(k+1)與X(k+1|k) 不相關(guān)。進(jìn)而可知，w(k+1)與X(k+1|k+1)不相關(guān)。正是因?yàn)?(a) (b)中的兩個(gè)不相關(guān)，卡爾曼公式中的預(yù)測(cè)協(xié)方差矩（卡爾曼公式(2)）和新息協(xié)方差矩陣（卡爾曼公式(3)中的“分母”部分）才可以是簡(jiǎn)單的加式。附注2：卡爾曼濾波的五個(gè)公式先驗(yàn)預(yù)測(cè)值與先驗(yàn)預(yù)測(cè)協(xié)方差矩陣的計(jì)算。求解協(xié)方差時(shí)，都認(rèn)為預(yù)測(cè)值的期望是實(shí)際值。因此，X(k+1|k)的協(xié)方差矩陣同樣也是 X(k+1|k) 的協(xié)方差矩陣，又因?yàn)槠頧(k+1|k)的期望是0，因此協(xié)方差矩陣反映了X(k+1|k)在向量空間中的模長(zhǎng)。注意，協(xié)方差矩陣都是對(duì)稱矩陣。X(k+1|

39、k)=A X(k|k)+B U(k) (1)P(k+1|k)=A P(k|k) A+Q(k) (2)卡爾曼增益矩陣的計(jì)算。量測(cè)預(yù)測(cè)值為Z(k+1|k) = H X(k+1|k)新息協(xié)方差見(jiàn)公式（3）中的“分母”部分。量測(cè)預(yù)測(cè)值的期望是實(shí)際量測(cè)值。因此，Z(k+1|k)的協(xié)方差矩陣同樣也是 Z(k+1|k) 的協(xié)方差矩陣，又因?yàn)槠頩(k+1|k)的期望是0，因此協(xié)方差矩陣反映了Z(k+1|k)在向量空間中的模長(zhǎng)。Kg(k+1)= P(k+1|k) H/ (H P(k+1|k) H + R(k+1) (3)后驗(yàn)預(yù)測(cè)值與后驗(yàn)預(yù)測(cè)協(xié)方差矩陣的計(jì)算X(k+1|k+1)= X(k+1|k)+Kg(k+1

40、) (Z(k+1)-H X(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(

41、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); 運(yùn)行程序，得到系統(tǒng)Kalman濾波器的增益矩陣L與估計(jì)誤差的協(xié)方差P為： L = 1.0641 1.1566 2.0393 P = 0.0678 0.0664 0.1064 0.0664 0.0695 0.11

42、57 0.1064 0.1157 0.2039卡爾曼濾波算法及MATLAB實(shí)現(xiàn)(2012-08-29 21:39:56)轉(zhuǎn)載這一段時(shí)間對(duì)現(xiàn)代濾波進(jìn)行了學(xué)習(xí)，對(duì)自適應(yīng)濾波器和卡爾曼濾波器有了一定認(rèn)識(shí)，并對(duì)它們用MATLAB對(duì)語(yǔ)音信號(hào)進(jìn)行了濾波，發(fā)現(xiàn)卡爾曼濾波器還是比較有用，能夠在較大的噪聲中還原原來(lái)的信號(hào)。新的學(xué)期馬上就開(kāi)始了，由于TI的開(kāi)發(fā)板一直在維修，所以學(xué)習(xí)TI開(kāi)發(fā)板的計(jì)劃擱置，但是對(duì)聲音信號(hào)的處理及濾波器的認(rèn)識(shí)有了進(jìn)一步提高。新的學(xué)期繼續(xù)努力！卡爾曼濾波的基本思想是：以最小均方誤差為最佳估計(jì)準(zhǔn)則，采用信號(hào)與噪聲的狀態(tài)空間模型，利用前一時(shí)刻的估計(jì)值和當(dāng)前時(shí)刻的觀測(cè)值來(lái)更新對(duì)狀態(tài)變量的估計(jì)，

43、求出當(dāng)前時(shí)刻的估計(jì)值，算法根據(jù)建立的系統(tǒng)方程和觀測(cè)方程對(duì)需要處理的信號(hào)做出滿足最小均方誤差的估計(jì)。語(yǔ)音信號(hào)在較長(zhǎng)時(shí)間內(nèi)是非平穩(wěn)的，但在較短的時(shí)間內(nèi)的一階統(tǒng)計(jì)量和二階統(tǒng)計(jì)量近似為常量，因此語(yǔ)音信號(hào)在相對(duì)較短的時(shí)間內(nèi)可以看成白噪聲激勵(lì)以線性時(shí)不變系統(tǒng)得到的穩(wěn)態(tài)輸出。假定語(yǔ)音信號(hào)可看成由一AR模型產(chǎn)生：時(shí)間更新方程：測(cè)量更新方程：K(t)為卡爾曼增益，其計(jì)算公式為：其中、分別為過(guò)程模型噪聲協(xié)方差和測(cè)量模型噪聲協(xié)方差，測(cè)量協(xié)方差可以通過(guò)觀測(cè)得到，則較難確定，在本實(shí)驗(yàn)中則通過(guò)與兩者比較得到。由于語(yǔ)音信號(hào)短時(shí)平穩(wěn)，因此在進(jìn)行卡爾曼濾波之前對(duì)信號(hào)進(jìn)行分幀加窗操作，在濾波之后對(duì)處理得到的信號(hào)進(jìn)行合幀，這里選取幀長(zhǎng)為256，而幀重疊個(gè)數(shù)為128；下圖為原聲音信號(hào)與加噪聲后的信號(hào)以及聲音信號(hào)與經(jīng)卡爾曼濾波處理后的信號(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ù)%load voice.maty=m1(2,:);x=y+0.08*randn(1,len