卡爾曼Kalman濾波課件

1、第三章卡爾曼(Kalman)濾波第三章卡爾曼(Kalman)濾波第一節(jié) 引言第一節(jié) 引言卡爾曼生平卡爾曼全名Rudolf Emil Kalman，匈牙利數(shù)學(xué)家，1930年出生于匈牙利首都布達(dá)佩斯。1953，1954年于麻省理工學(xué)院分別獲得電機(jī)工程學(xué)士及碩士學(xué)位。1957年于哥倫比亞大學(xué)獲得博士學(xué)位。我們?cè)诂F(xiàn)代控制理論中要學(xué)習(xí)的卡爾曼濾波器，正是源于他的博士論文和1960年發(fā)表的論文A New Approach to Linear Filtering and Prediction Problems（線性濾波與預(yù)測(cè)問題的新方法）。 卡爾曼生平卡爾曼全名Rudolf Emil Kalman，匈1.引

2、言卡爾曼(Kalman)濾波和維納(Wiener)濾波都是以最小均方誤差為準(zhǔn)則的最佳線性估計(jì)或?yàn)V波。1.引言卡爾曼(Kalman)濾波和維納(Wiener)濾波2.適用范圍維納濾波只適用于平穩(wěn)隨機(jī)過程(信號(hào))卡爾曼濾波沒有這個(gè)限制，信號(hào)可以是平穩(wěn)的，也可以是非平穩(wěn)的。2.適用范圍維納濾波只適用于平穩(wěn)隨機(jī)過程(信號(hào))3.處理方法維納濾波器根據(jù)全部過去的和當(dāng)前的觀測(cè)數(shù)據(jù)x(n),x(n-1), 來估計(jì)信號(hào)的當(dāng)前值以均方誤差最小條件下求解系統(tǒng)的傳遞函數(shù)H(z)或單位沖激響應(yīng)h(n)3.處理方法維納濾波器根據(jù)全部過去的和當(dāng)前的觀測(cè)數(shù)據(jù)x(n)卡爾曼濾波不需要全部過去的觀察數(shù)據(jù)來估計(jì)信號(hào)的當(dāng)前值它是用狀

3、態(tài)空間法描述系統(tǒng)，即由狀態(tài)方程和量測(cè)方程組成。解是以估計(jì)值(是狀態(tài)變量的估計(jì)值)的形式給出的只根據(jù)前一個(gè)估計(jì)值 和最近一個(gè)觀察數(shù)據(jù)卡爾曼濾波不需要全部過去的觀察數(shù)據(jù)來估計(jì)信號(hào)的當(dāng)前值它是用狀其算法是遞推且狀態(tài)空間法采用在時(shí)域內(nèi)設(shè)計(jì)濾波器的方法因而適用于多維隨機(jī)過程的估計(jì)；離散卡爾曼算法適用計(jì)算機(jī)處理。其算法是遞推且狀態(tài)空間法采用因而適用于多維隨機(jī)過程的估計(jì)；4.信號(hào)模型的建立從信號(hào)模型的建立來看：維納濾波的信號(hào)模型是從信號(hào)與噪聲的相關(guān)函數(shù)得到?？柭鼮V波的信號(hào)模型則是從狀態(tài)方程和量測(cè)方程得到。4.信號(hào)模型的建立從信號(hào)模型的建立來看：卡爾曼濾波器的特點(diǎn)是什么？卡爾曼濾波器的特點(diǎn)是什么？第二節(jié) 卡

4、爾曼濾波器的信號(hào)模型離散狀態(tài)方程與量測(cè)方程第二節(jié) 卡爾曼濾波器的信號(hào)模型離散狀態(tài)方程與量測(cè)方程引入在討論維納濾波時(shí)，提出一個(gè)基本概念： 任何具有有理功率譜密度的隨機(jī)信號(hào)都可看作是白色噪聲通過一個(gè)線性網(wǎng)絡(luò)所形成。 由此得到維納濾波器的信號(hào)模型 引入在討論維納濾波時(shí)，提出一個(gè)基本概念：為了得到卡爾曼過濾的信號(hào)模型，必須首先討論狀態(tài)方程和量測(cè)方程。 為了得到卡爾曼過濾的信號(hào)模型，必須首先討論狀態(tài)方程和量測(cè)方程一、離散狀態(tài)方程及其解 離散狀態(tài)方程的基本形式是： 其中x(k)代表一組狀態(tài)變量組成的多維狀態(tài)矢量，而A，B都是矩陣，它們是由系統(tǒng)的拓?fù)浣Y(jié)構(gòu)、元件性質(zhì)和數(shù)值所確定的。是激勵(lì)信號(hào)。 一、離散狀態(tài)方

5、程及其解 離散狀態(tài)方程的基本形式是： 其中x(狀態(tài)方程是多維一階的差分方程。當(dāng)已知初始狀態(tài)x(0)，可用遞推的方法得到它的解 狀態(tài)方程是多維一階的差分方程。 卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件。 。 卡爾曼Kalman濾波課件 總結(jié) 總結(jié)二、離散時(shí)間系統(tǒng)的量測(cè)方程二、離散時(shí)間系統(tǒng)的量測(cè)方程卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼濾波的信號(hào)模型多維情況卡爾曼濾波的信號(hào)模型多維情況卡爾曼濾波的信號(hào)模型一維情況卡爾曼濾波的信號(hào)模型一維情況例1例1解：解：第三節(jié)卡

6、爾曼濾波的方法第三節(jié)卡爾曼濾波的方法1、卡爾曼濾波的基本思想1、卡爾曼濾波的基本思想2、研究對(duì)象離散系統(tǒng)離散系統(tǒng)的n維狀態(tài)方程：離散系統(tǒng)的m維量測(cè)方程：2、研究對(duì)象離散系統(tǒng)離散系統(tǒng)的n維狀態(tài)方程：離散系統(tǒng)的m維3、卡爾曼濾波一步遞推法模型3、卡爾曼濾波一步遞推法模型卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件4、求卡爾曼濾波遞推公式4、求卡爾曼濾波遞推公式卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件新息過程的性質(zhì)新息過程的性質(zhì)卡爾曼Kalman濾波課

7、件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件5、離散卡爾曼濾波算法總結(jié)

8、狀態(tài)方程：量測(cè)方程：統(tǒng)計(jì)特性：5、離散卡爾曼濾波算法總結(jié)狀態(tài)方程：量測(cè)方程：統(tǒng)計(jì)特性：初始條件：遞推公式：增益方程：初始條件：遞推公式：增益方程：均方誤差陣：均方誤差陣：卡爾曼Kalman濾波課件6、卡爾曼濾波算法的計(jì)算流程圖6、卡爾曼濾波算法的計(jì)算流程圖例2例2解：解：卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件7、一步預(yù)測(cè)估計(jì)的卡爾曼預(yù)測(cè)器7、一步預(yù)測(cè)估計(jì)的卡爾曼預(yù)測(cè)器卡爾曼Kalman濾波課件卡爾曼K

9、alman濾波課件卡爾曼Kalman濾波課件8、預(yù)測(cè)與濾波之間比較8、預(yù)測(cè)與濾波之間比較9、同時(shí)有過濾和預(yù)測(cè)輸出的方框圖由圖可知：能夠從卡爾曼濾波器中獲得一步預(yù)測(cè)。9、同時(shí)有過濾和預(yù)測(cè)輸出的方框圖由圖可知：能夠從卡爾曼濾波器例3例3解：解：卡爾曼Kalman濾波課件Kalman預(yù)測(cè)的跟蹤性能 Kalman預(yù)測(cè)的跟蹤性能 增益的變化曲線 增益的變化曲線 10、卡爾曼濾波公式中各個(gè)參數(shù)之間關(guān)系10、卡爾曼濾波公式中各個(gè)參數(shù)之間關(guān)系卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件第四節(jié)卡爾曼濾波與維納濾波的關(guān)系第四節(jié)卡爾曼濾波與維納濾波的關(guān)系1、舉例1、舉例卡爾曼Kalman濾波課件卡爾曼Kal

10、man濾波課件2、結(jié)論2、結(jié)論卡爾曼Kalman濾波課件維納濾波卡爾曼濾波已知條件誤差準(zhǔn)則均方誤差最小均方誤差最小解的形式模型建立信號(hào)與噪聲相關(guān)函數(shù)狀態(tài)方程與量測(cè)方程維納濾波卡爾曼濾波已知條件誤差準(zhǔn)則均方誤差最小均方誤差最小解3、卡爾曼濾波與維納濾波不同(1)卡爾曼濾波與維納濾波中解決最佳濾波的方法不相同。維納濾波：是用頻域及傳遞函數(shù)的方法；卡爾曼濾波：是用時(shí)域及狀態(tài)變量的辦法；3、卡爾曼濾波與維納濾波不同(1)卡爾曼濾波與維納濾波中解決(2)卡爾曼在理論上是維納濾波的推廣和發(fā)展，特別在處理多變量系統(tǒng)、時(shí)變線性系統(tǒng)及非線性系統(tǒng)的最佳濾波等領(lǐng)域，為我們提供了一種比較有效的方法，克服了基于頻域處理

11、所遇到的困難。這些困難包括：維納濾波要求平穩(wěn)，而卡爾曼濾波則不要求；卡爾曼容許初始時(shí)間不是負(fù)無窮大，這在很多情況下是有實(shí)際意義的；(2)卡爾曼在理論上是維納濾波的推廣和發(fā)展，特別在處理多變量(3)卡爾曼濾波的另一個(gè)不同點(diǎn)是把狀態(tài)或信號(hào)過程的產(chǎn)生看成是白噪聲激勵(lì)有限維數(shù)系統(tǒng)的輸出；維納濾波要求過程的自相關(guān)函數(shù)和互相關(guān)函數(shù)的簡單知識(shí)，而卡爾曼濾波則要求時(shí)域中狀態(tài)變量及信號(hào)產(chǎn)生過程的詳細(xì)知識(shí)。(3)卡爾曼濾波的另一個(gè)不同點(diǎn)是把狀態(tài)或信號(hào)過程的產(chǎn)生看成是4、卡爾曼濾波的優(yōu)點(diǎn)在時(shí)域上采用線性遞推形式對(duì)觀測(cè)值進(jìn)行處理，能實(shí)時(shí)地給出系統(tǒng)狀態(tài)的最優(yōu)估計(jì)，并突破了單維輸入和輸出的限制?？柭鼮V波算法的這些優(yōu)點(diǎn)使

12、它在信號(hào)和信息系統(tǒng)中得到比較廣泛的應(yīng)用。4、卡爾曼濾波的優(yōu)點(diǎn)在時(shí)域上采用線性遞推形式對(duì)觀測(cè)值進(jìn)行處理5、卡爾曼濾波的缺點(diǎn)（1）模型誤差和數(shù)值發(fā)散。模型誤差:卡爾曼濾波算法的關(guān)鍵是建立系統(tǒng)的狀態(tài)模型。但實(shí)際系統(tǒng)有時(shí)很難得到精確描述，往往只能用近似模型來代替，因?yàn)榧词鼓軌颢@得精確的模型，也常會(huì)因?yàn)榫_模型太復(fù)雜，維數(shù)過高而與實(shí)時(shí)處理必須減少計(jì)算量及盡量簡化模型的要求相矛盾。近似或簡化的模型都與精確模型之間存在誤差，模型誤差必然會(huì)給濾波帶來影響，嚴(yán)重時(shí)還會(huì)造成濾波結(jié)果不收斂。抑制方法：采用逐漸衰減記憶法、限定記憶法、限定下界法和人為增加模型輸入噪聲方差。5、卡爾曼濾波的缺點(diǎn)（1）模型誤差和數(shù)值發(fā)散。

13、數(shù)值發(fā)散:舍入誤差的影響以及遞推算法使得舍入誤差積累的影響。計(jì)算機(jī)存貯單元的長度有限，不可避免地存在舍入誤差，它相當(dāng)于在狀態(tài)方程和量測(cè)方程中加入噪聲，帶來的后果是有可能改變某些矩陣的性質(zhì)，引起誤差矩陣失去正定性和對(duì)稱性，如均方誤差陣列受到擾動(dòng)而離開穩(wěn)定解，如沒失去正定性，仍可返回穩(wěn)定解，可用雙精度運(yùn)算得以改善，但會(huì)增加運(yùn)算量，目前采用平方根法，即求均方誤差陣P改用其平方根P1/2實(shí)現(xiàn)。數(shù)值發(fā)散:舍入誤差的影響以及遞推算法使得舍入誤差積累的影響。(2)實(shí)時(shí)要求。影響卡爾曼濾波算法的實(shí)時(shí)性主要是狀態(tài)維數(shù)n和增益矩陣的計(jì)算，它們往往有很大的計(jì)算量。一般在計(jì)算中采取某些措施，例如應(yīng)用定常系統(tǒng)新算法或在

14、精度損失允許情況下盡量減小維數(shù)等措施，從而減小計(jì)算量以滿足實(shí)時(shí)濾波的要求。(2)實(shí)時(shí)要求。影響卡爾曼濾波算法的實(shí)時(shí)性主要是狀態(tài)維數(shù)n和6、卡爾曼濾波的應(yīng)用在空間技術(shù)、工業(yè)過程控制與電子工程等領(lǐng)域得到了比較廣泛的應(yīng)用，特別在信號(hào)處理的二次加工數(shù)據(jù)處理方面應(yīng)用更廣，諸如雷達(dá)的位置、速度的估計(jì)，以及空中交通管制系統(tǒng)對(duì)飛行器航跡的估計(jì)與導(dǎo)航等領(lǐng)域都得到了廣泛而成功的應(yīng)用。6、卡爾曼濾波的應(yīng)用在空間技術(shù)、工業(yè)過程控制與電子工程等領(lǐng)域(1)應(yīng)用舉例-雷達(dá)跟蹤目標(biāo)物說明卡爾曼濾波的應(yīng)用。雷達(dá)跟蹤目標(biāo)的基本原理是通過發(fā)射脈沖，根據(jù)接收到的脈沖與發(fā)射脈沖的時(shí)間間隔，來確定目標(biāo)物的距離和速度。由于干擾的影響，接收

15、到的脈沖波形變化很大，那么一次的測(cè)量結(jié)果可能存在很大的誤差。為了減小誤差，往往采取發(fā)射一串脈沖的方法進(jìn)行測(cè)量。(1)應(yīng)用舉例-雷達(dá)跟蹤目標(biāo)物說明卡爾曼濾波的應(yīng)用?？柭麷alman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波

16、課件例4作業(yè)例4作業(yè)解：蒙特卡洛方法是指對(duì)多次產(chǎn)生隨機(jī)數(shù)來模擬過程，并通過求平均來得到我們所需要的過程。這里就要求我們仿真100次。解：蒙特卡洛方法是指對(duì)多次產(chǎn)生隨機(jī)數(shù)來模擬過程，并通過求平均MATLAB程序：clc;clear;T=5;%雷達(dá)掃描周期num=100;%*產(chǎn)生真實(shí)軌跡*N=800/T;x=zeros(N,1);y=zeros(N,1);vx=zeros(N,1);vy=zeros(N,1);x(1)=-2000;y(1)=500;vx=10;vy=0;ax=0;ay=0;%跟蹤加速度var=100;%觀察目標(biāo)位置的變化量MATLAB程序：%產(chǎn)生真實(shí)軌跡for i=1:N-1 x

17、(i+1)=x(i)+vx*T+0.5*ax*T2; y(i+1)=y(i)+vy*T+0.5*ay*T2;endnx=zeros(N,1);ny=zeros(N,1);nx=var*randn(N,1);ny=var*randn(N,1);zx=x+nx;zy=y+ny; %真實(shí)目標(biāo)軌跡%產(chǎn)生真實(shí)軌跡 %濾波100次for m=1:numz=2:1;xks(1)=zx(1);yks(1)=zy(1);xks(2)=zx(2);yks(2)=zy(2);o=4:4;g=4:2;h=2:4;q=2:2;xk=4:1;perr=4:4;o=1,T,0,0;0,1,0,0;0,0,1,T;0,0,0

18、,1;h=1 0 0 0;0 0 1 0;g=T/2,0;T/2,0;0,T/2;0,T/2;q=10000 0;0 10000;perr=var2,var2/T,0,0;var*var/T, 2*var2/(T2),0,0;0,0,var2,var2/T;0,0,var2/T,2*var2/(T2);vx=(zx(2)-zx(1)/2;vy=(zy(2)-zy(1)/2;xk=zx(1);vx;zy(1);vy; %濾波100次 %Kalman濾波開始for r=3:N; z=zx(r);zy(r); xk1=o*xk; perr1=o*perr*o; k=perr1*h*inv(h*perr1*h+q); xk=xk1+k*(z-h*xk1); perr=(eye(4)-k*h)*perr1; xks(r)=xk(1,1); yks(r)=xk(3,1); vkxs(r)=xk(2,1); ykxs(r)=xk(4,1); xkls(r)=xk1(1,1); ykls(r)=x