卡爾曼Kalman濾波課件

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

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

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

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

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

6、爾曼濾波的方法第三節(jié)卡爾曼濾波的方法1、卡爾曼濾波的基本思想1、卡爾曼濾波的基本思想2、研究對象離散系統(tǒng)離散系統(tǒng)的n維狀態(tài)方程：離散系統(tǒng)的m維量測方程：2、研究對象離散系統(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)方程：量測方程：統(tǒng)計(jì)特性：5、離散卡爾曼濾波算法總結(jié)狀態(tài)方程：量測方程：統(tǒng)計(jì)特性：初始條件：遞推公式：增益方程：初始條件：遞推公式：增益方程：均方誤差陣：均方誤差陣：卡爾曼Kalman濾波課件6、卡爾曼濾波算法的計(jì)算流程圖6、卡爾曼濾波算法的計(jì)算流程圖例2例2解：解：卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件卡爾曼Kalman濾波課件7、一步預(yù)測估計(jì)的卡爾曼預(yù)測器7、一步預(yù)測估計(jì)的卡爾曼預(yù)測器卡爾曼Kalman濾波課件卡爾曼K

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

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

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

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

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

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

15、到的脈沖波形變化很大，那么一次的測量結(jié)果可能存在很大的誤差。為了減小誤差，往往采取發(fā)射一串脈沖的方法進(jìn)行測量。(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è)解：蒙特卡洛方法是指對多次產(chǎn)生隨機(jī)數(shù)來模擬過程，并通過求平均來得到我們所需要的過程。這里就要求我們仿真100次。解：蒙特卡洛方法是指對多次產(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