3-卡爾曼濾波-1詳解

1、1 Kalman Filtering2 背景介紹：背景介紹： Kalman，匈牙利數(shù)學(xué)家。 卡爾曼濾波器源于他的博士論 文和1960年發(fā)表的論文A New Approach to Linear Filtering and Prediction Problems（線 性濾波與預(yù)測問題的新方法）。3 估計原理和卡爾曼濾波估計原理和卡爾曼濾波1. 狀態(tài)估計原理狀態(tài)估計原理 2. 為什么要用狀態(tài)估計理論為什么要用狀態(tài)估計理論 3. 經(jīng)典控制理論與現(xiàn)代控制理論經(jīng)典控制理論與現(xiàn)代控制理論 4. 什么是卡爾曼濾波什么是卡爾曼濾波5.卡爾曼濾波器的軟硬件實現(xiàn)卡爾曼濾波器的軟硬件實現(xiàn)6.卡爾曼濾波器的應(yīng)用卡爾曼

2、濾波器的應(yīng)用 4 1.狀態(tài)估計原理狀態(tài)估計原理 狀態(tài)估計是卡爾曼濾波的重要組成部分。一狀態(tài)估計是卡爾曼濾波的重要組成部分。一般來說，根據(jù)觀測數(shù)據(jù)對隨機(jī)量進(jìn)行定量推斷就般來說，根據(jù)觀測數(shù)據(jù)對隨機(jī)量進(jìn)行定量推斷就是估計問題，特別是對動態(tài)行為的狀態(tài)估計，它是估計問題，特別是對動態(tài)行為的狀態(tài)估計，它能實現(xiàn)實時運行狀態(tài)的估計和預(yù)測功能。比如對能實現(xiàn)實時運行狀態(tài)的估計和預(yù)測功能。比如對飛行器狀態(tài)估計。飛行器狀態(tài)估計。 5 狀態(tài)估計對于了解和控制一個系統(tǒng)具有重狀態(tài)估計對于了解和控制一個系統(tǒng)具有重要意義，所應(yīng)用的方法屬于統(tǒng)計學(xué)中的估計理論。要意義，所應(yīng)用的方法屬于統(tǒng)計學(xué)中的估計理論。最常用的是最小二乘估計，線

3、性最小方差估計、最常用的是最小二乘估計，線性最小方差估計、最小方差估計、遞推最小二乘估計等。其他如風(fēng)最小方差估計、遞推最小二乘估計等。其他如風(fēng)險準(zhǔn)則的貝葉斯估計、最大似然估計、隨機(jī)逼近險準(zhǔn)則的貝葉斯估計、最大似然估計、隨機(jī)逼近等方法也都有應(yīng)用。等方法也都有應(yīng)用。 6 受噪聲干擾的狀態(tài)量是個隨機(jī)量，不可能測得精受噪聲干擾的狀態(tài)量是個隨機(jī)量，不可能測得精確值，但可對它進(jìn)行一系列觀測，并依據(jù)一組觀測值，確值，但可對它進(jìn)行一系列觀測，并依據(jù)一組觀測值，按某種統(tǒng)計觀點對它進(jìn)行估計。按某種統(tǒng)計觀點對它進(jìn)行估計。 使估計值盡可能準(zhǔn)確地接近真實值，這就是最優(yōu)使估計值盡可能準(zhǔn)確地接近真實值，這就是最優(yōu)估計。估計

4、。 真實值與估計值之差稱為估計誤差。真實值與估計值之差稱為估計誤差。 若估計值的數(shù)學(xué)期望與真實值相等，這種估計稱若估計值的數(shù)學(xué)期望與真實值相等，這種估計稱為無偏估計。為無偏估計。 7 卡爾卡爾曼曼提出的遞推最優(yōu)估計理論，采用狀態(tài)空提出的遞推最優(yōu)估計理論，采用狀態(tài)空間描述法，算法采用遞推形式，卡爾曼濾波能處理間描述法，算法采用遞推形式，卡爾曼濾波能處理多維和非平穩(wěn)的隨機(jī)過程。多維和非平穩(wěn)的隨機(jī)過程。 卡爾曼濾波理論的提出，克服了威納濾波理論卡爾曼濾波理論的提出，克服了威納濾波理論的局限性，使其在工程上得到了廣泛的應(yīng)用，尤其的局限性，使其在工程上得到了廣泛的應(yīng)用，尤其在控制、制導(dǎo)、導(dǎo)航、通訊等現(xiàn)代

5、工程方面。在控制、制導(dǎo)、導(dǎo)航、通訊等現(xiàn)代工程方面。8關(guān)于維納濾波和卡爾曼濾關(guān)于維納濾波和卡爾曼濾維納濾波和卡爾曼濾波都是解決線性濾波和預(yù)測問題的方法維納濾波和卡爾曼濾波都是解決線性濾波和預(yù)測問題的方法，并且都是以均方誤差最小為準(zhǔn)則的，在平穩(wěn)條件下兩者的，并且都是以均方誤差最小為準(zhǔn)則的，在平穩(wěn)條件下兩者的穩(wěn)態(tài)結(jié)果是一致的。但是它們解決問題的方法有很大區(qū)別。穩(wěn)態(tài)結(jié)果是一致的。但是它們解決問題的方法有很大區(qū)別。1. 維納濾波是根據(jù)全部過去觀測值和當(dāng)前觀測值來估計信號維納濾波是根據(jù)全部過去觀測值和當(dāng)前觀測值來估計信號的當(dāng)前值，因此它的解形式是系統(tǒng)的傳遞函數(shù)的當(dāng)前值，因此它的解形式是系統(tǒng)的傳遞函數(shù)H(z

6、)或單位脈或單位脈沖響應(yīng)沖響應(yīng)h(n) ，因此更常稱這種系統(tǒng)為最佳線性過濾器或濾波，因此更常稱這種系統(tǒng)為最佳線性過濾器或濾波器；卡爾曼濾波是用當(dāng)前一個估計值和最近一個觀測值來估器；卡爾曼濾波是用當(dāng)前一個估計值和最近一個觀測值來估計信號的當(dāng)前值，它是用狀態(tài)方程和遞推的方法進(jìn)行估計的計信號的當(dāng)前值，它是用狀態(tài)方程和遞推的方法進(jìn)行估計的，它的解形式是以估計值，它的解形式是以估計值 (常常是狀態(tài)變量值）。因此更常常常是狀態(tài)變量值）。因此更常稱這種系統(tǒng)為線性最優(yōu)估計器或濾波器。稱這種系統(tǒng)為線性最優(yōu)估計器或濾波器。2. 維納濾波只適用于平穩(wěn)隨機(jī)過程，卡爾曼濾波就沒有這個維納濾波只適用于平穩(wěn)隨機(jī)過程，卡爾曼

7、濾波就沒有這個限制。限制。93. 維納過濾中信號和噪聲是用相關(guān)函數(shù)表示的，因此設(shè)計維納維納過濾中信號和噪聲是用相關(guān)函數(shù)表示的，因此設(shè)計維納濾波器要求已知信號和噪聲的相關(guān)函數(shù)。卡爾曼過濾中信號和濾波器要求已知信號和噪聲的相關(guān)函數(shù)?？柭^濾中信號和噪聲是狀態(tài)方程和量測方程表示的，因此設(shè)計卡爾曼濾波器要噪聲是狀態(tài)方程和量測方程表示的，因此設(shè)計卡爾曼濾波器要求已知狀態(tài)方程和量測方程求已知狀態(tài)方程和量測方程(當(dāng)然，相關(guān)函數(shù)與狀態(tài)方程和量當(dāng)然，相關(guān)函數(shù)與狀態(tài)方程和量測方程之間會存在一定的關(guān)系測方程之間會存在一定的關(guān)系)?？柭^濾方法看來似乎比維納過濾方法優(yōu)越，它用遞推法計卡爾曼過濾方法看來似乎比維納過

8、濾方法優(yōu)越，它用遞推法計算，不需要知道全部過去的數(shù)據(jù)，從而運用計算機(jī)計算方便，算，不需要知道全部過去的數(shù)據(jù)，從而運用計算機(jī)計算方便，而且它可用于平穩(wěn)和不平穩(wěn)的隨機(jī)過程而且它可用于平穩(wěn)和不平穩(wěn)的隨機(jī)過程(信號信號)，非時變和時變，非時變和時變的系統(tǒng)。的系統(tǒng)。10但從發(fā)展歷史上來看維納濾波的思想是但從發(fā)展歷史上來看維納濾波的思想是40年代初提出來的，年代初提出來的，1949年正式以書的形式出版?？柭鼮V波到年正式以書的形式出版?？柭鼮V波到60年代初才提出年代初才提出來，它是在維納濾波的基礎(chǔ)上發(fā)展起來的，雖然如上所述它來，它是在維納濾波的基礎(chǔ)上發(fā)展起來的，雖然如上所述它比維納濾波方法有不少優(yōu)越的地

9、方，但是最佳線性濾波問題比維納濾波方法有不少優(yōu)越的地方，但是最佳線性濾波問題是由維納濾波首先解決的，維納濾波的物理概念比較清楚，是由維納濾波首先解決的，維納濾波的物理概念比較清楚，也可以認(rèn)為卡爾曼濾波僅僅是對最佳線性過濾問題提出的一也可以認(rèn)為卡爾曼濾波僅僅是對最佳線性過濾問題提出的一種新的算法。種新的算法。當(dāng)被估計的狀態(tài)變量很多時，卡爾曼濾波運算量以及存儲量當(dāng)被估計的狀態(tài)變量很多時，卡爾曼濾波運算量以及存儲量將會變得很大，因為需要存儲和更新狀態(tài)預(yù)測誤差的自相關(guān)將會變得很大，因為需要存儲和更新狀態(tài)預(yù)測誤差的自相關(guān)矩陣。這也是他的缺點。矩陣。這也是他的缺點。wiener濾波用于標(biāo)量濾波，而濾波用于

10、標(biāo)量濾波，而kalman濾波可用于矢量濾波濾波可用于矢量濾波，所以應(yīng)用也更廣些。，所以應(yīng)用也更廣些。112 2為什么要用狀態(tài)估計理論為什么要用狀態(tài)估計理論 在許多實際問題中，由于隨機(jī)過程的存在，常常不在許多實際問題中，由于隨機(jī)過程的存在，常常不能直接獲得系統(tǒng)的狀態(tài)參數(shù)，需要從夾雜著隨機(jī)干擾的能直接獲得系統(tǒng)的狀態(tài)參數(shù)，需要從夾雜著隨機(jī)干擾的觀測信號中分離出系統(tǒng)的狀態(tài)參數(shù)。觀測信號中分離出系統(tǒng)的狀態(tài)參數(shù)。 例如，飛機(jī)在飛行過程中所處的位置、速度等狀態(tài)例如，飛機(jī)在飛行過程中所處的位置、速度等狀態(tài)參數(shù)需要通過雷達(dá)或其它測量裝置進(jìn)行觀測，而雷達(dá)等參數(shù)需要通過雷達(dá)或其它測量裝置進(jìn)行觀測，而雷達(dá)等測量裝置也

11、存在隨機(jī)干擾，因此在觀測到飛機(jī)的位置、測量裝置也存在隨機(jī)干擾，因此在觀測到飛機(jī)的位置、速度等信號中就夾雜著隨機(jī)干擾，要想正確地得到飛機(jī)速度等信號中就夾雜著隨機(jī)干擾，要想正確地得到飛機(jī)的狀態(tài)參數(shù)是不可能的，只能根據(jù)觀測到的信號來估計的狀態(tài)參數(shù)是不可能的，只能根據(jù)觀測到的信號來估計和預(yù)測飛機(jī)的狀態(tài)，這就是估計問題。和預(yù)測飛機(jī)的狀態(tài)，這就是估計問題。12 從觀測到的信號中估計出狀態(tài)的估值，并且希從觀測到的信號中估計出狀態(tài)的估值，并且希望估值與狀態(tài)的真值越小越好，即要求有：望估值與狀態(tài)的真值越小越好，即要求有： 成立；成立； 因此存在最優(yōu)估計問題，這就是卡爾曼濾波。因此存在最優(yōu)估計問題，這就是卡爾曼濾

12、波。 卡爾曼濾波的最優(yōu)估計需滿足以下三個條件：卡爾曼濾波的最優(yōu)估計需滿足以下三個條件： 無偏性，即估計值的均值等于狀態(tài)的真值；無偏性，即估計值的均值等于狀態(tài)的真值； 估計的方差最小；估計的方差最?。?實時性。實時性。min)()( txtx13 從以上分析可以看出卡爾曼濾波就是在有隨從以上分析可以看出卡爾曼濾波就是在有隨機(jī)干擾和噪聲的情況下，以線性最小方差估計方機(jī)干擾和噪聲的情況下，以線性最小方差估計方法給出狀態(tài)的最優(yōu)估計值，卡爾曼濾波是在統(tǒng)計法給出狀態(tài)的最優(yōu)估計值，卡爾曼濾波是在統(tǒng)計的意義上給出最接近狀態(tài)真值的估計值。因此，的意義上給出最接近狀態(tài)真值的估計值。因此，卡爾曼濾波在空間技術(shù)、測軌

13、、導(dǎo)航、攔截與通卡爾曼濾波在空間技術(shù)、測軌、導(dǎo)航、攔截與通訊等方面獲得了廣泛的應(yīng)用。訊等方面獲得了廣泛的應(yīng)用。143 3經(jīng)典控制理論與現(xiàn)代控制理論經(jīng)典控制理論與現(xiàn)代控制理論 經(jīng)典控制理論只適應(yīng)與單輸入經(jīng)典控制理論只適應(yīng)與單輸入單輸出的線性定常系單輸出的線性定常系統(tǒng)，研究方法是傳遞函數(shù)。傳遞函數(shù)在本質(zhì)上是一種頻率統(tǒng)，研究方法是傳遞函數(shù)。傳遞函數(shù)在本質(zhì)上是一種頻率法，要靠各個頻率分量描述信號。因此，頻率法限制了系法，要靠各個頻率分量描述信號。因此，頻率法限制了系統(tǒng)對整個過程在時間域內(nèi)進(jìn)行控制的能力，所以經(jīng)典控制統(tǒng)對整個過程在時間域內(nèi)進(jìn)行控制的能力，所以經(jīng)典控制理論很難實現(xiàn)實時控制。同時，經(jīng)典控制理

14、論也很難實現(xiàn)理論很難實現(xiàn)實時控制。同時，經(jīng)典控制理論也很難實現(xiàn)最優(yōu)控制。最優(yōu)控制。 反 饋 量 控 制 器Gc 輸 入 量 + - 受 控 對 象Go 反 饋 環(huán) 節(jié)H 輸 出 量 u r c 控 制 量 偏 差 n 擾 動 量 b 經(jīng)典控制系統(tǒng)的組成經(jīng)典控制系統(tǒng)的組成15 由于經(jīng)典控制理論的上述局限性，隨著科學(xué)由于經(jīng)典控制理論的上述局限性，隨著科學(xué)技術(shù)的發(fā)展，特別是空間技術(shù)和各類高速飛行器技術(shù)的發(fā)展，特別是空間技術(shù)和各類高速飛行器的快速發(fā)展，要求控制高速度、高精度的受控對的快速發(fā)展，要求控制高速度、高精度的受控對象，控制系統(tǒng)更加復(fù)雜，要求控制理論解決多輸象，控制系統(tǒng)更加復(fù)雜，要求控制理論解決

15、多輸入多輸出、非線性以及最優(yōu)控制等設(shè)計問題。這入多輸出、非線性以及最優(yōu)控制等設(shè)計問題。這些新的控制要求經(jīng)典控制理論是無法解決的。些新的控制要求經(jīng)典控制理論是無法解決的。16 現(xiàn)代控制理論是建立在狀態(tài)空間基礎(chǔ)上的，現(xiàn)代控制理論是建立在狀態(tài)空間基礎(chǔ)上的，它不用傳遞函數(shù)，而是用狀態(tài)向量方程作為基本工它不用傳遞函數(shù)，而是用狀態(tài)向量方程作為基本工具，因此可以用來分析多輸入具，因此可以用來分析多輸入多輸出、非線性以多輸出、非線性以及時變復(fù)雜系統(tǒng)的研究。及時變復(fù)雜系統(tǒng)的研究。 現(xiàn)代控制理論本質(zhì)上是時域法，信號的描述現(xiàn)代控制理論本質(zhì)上是時域法，信號的描述和傳遞都是在時間域進(jìn)行，所以現(xiàn)代控制理論具有和傳遞都是在

16、時間域進(jìn)行，所以現(xiàn)代控制理論具有實現(xiàn)實時控制的能力。由于采用了狀態(tài)空間法，現(xiàn)實現(xiàn)實時控制的能力。由于采用了狀態(tài)空間法，現(xiàn)代控制理論有利于設(shè)計人員根據(jù)給定的性能指標(biāo)設(shè)代控制理論有利于設(shè)計人員根據(jù)給定的性能指標(biāo)設(shè)計出最優(yōu)的控制系統(tǒng)。計出最優(yōu)的控制系統(tǒng)。 17 卡爾曼濾波控制系統(tǒng)結(jié)構(gòu)圖卡爾曼濾波控制系統(tǒng)結(jié)構(gòu)圖 由于系統(tǒng)的狀態(tài)由于系統(tǒng)的狀態(tài)x是不確定的，卡爾曼濾波器的是不確定的，卡爾曼濾波器的任務(wù)就是在有隨機(jī)干擾任務(wù)就是在有隨機(jī)干擾w和噪聲和噪聲v的情況下給出系統(tǒng)的情況下給出系統(tǒng)狀態(tài)狀態(tài)x的最優(yōu)估算值的最優(yōu)估算值 ，它在統(tǒng)計意義下最接近狀，它在統(tǒng)計意義下最接近狀態(tài)的真值態(tài)的真值x，從而實現(xiàn)最優(yōu)控制，從

17、而實現(xiàn)最優(yōu)控制u( )的目的。的目的。 x x 18 4.4.什么是卡爾曼濾波：什么是卡爾曼濾波： 卡爾曼濾波是在線性最小方差估計的基卡爾曼濾波是在線性最小方差估計的基礎(chǔ)上，提出的在數(shù)學(xué)結(jié)構(gòu)上比較簡單的而且礎(chǔ)上，提出的在數(shù)學(xué)結(jié)構(gòu)上比較簡單的而且是最優(yōu)線性遞推濾波方法，具有計算量小、是最優(yōu)線性遞推濾波方法，具有計算量小、存儲量低，實時性高的優(yōu)點。特別是對經(jīng)歷存儲量低，實時性高的優(yōu)點。特別是對經(jīng)歷了初始濾波后的過渡狀態(tài)，濾波效果非常好。了初始濾波后的過渡狀態(tài)，濾波效果非常好。 19 如果系統(tǒng)是高斯的，卡爾曼濾波是最優(yōu)的如果系統(tǒng)是高斯的，卡爾曼濾波是最優(yōu)的最小均方誤差估計；最小均方誤差估計； 如果系

18、統(tǒng)不是高斯的，卡爾曼濾波是線性如果系統(tǒng)不是高斯的，卡爾曼濾波是線性最小均方誤差估計，是最優(yōu)的線性估計器。最小均方誤差估計，是最優(yōu)的線性估計器。 卡爾曼濾波是以最小均方誤差為估計的最卡爾曼濾波是以最小均方誤差為估計的最佳準(zhǔn)則，來尋求一套遞推估計的算法。佳準(zhǔn)則，來尋求一套遞推估計的算法。20 基本思想是：采用信號與噪聲的狀態(tài)空基本思想是：采用信號與噪聲的狀態(tài)空間模型，利用前一時刻的估計值和現(xiàn)時刻的間模型，利用前一時刻的估計值和現(xiàn)時刻的觀測值來更新對狀態(tài)變量的估計，求現(xiàn)在時觀測值來更新對狀態(tài)變量的估計，求現(xiàn)在時刻的估計值。它適合于實時處理和計算機(jī)運刻的估計值。它適合于實時處理和計算機(jī)運算。算。 21

19、 卡爾曼濾波的實質(zhì)是由量測值重構(gòu)系統(tǒng)卡爾曼濾波的實質(zhì)是由量測值重構(gòu)系統(tǒng)的狀態(tài)向量。它以的狀態(tài)向量。它以“預(yù)測預(yù)測實測實測修正修正”的的順序遞推，根據(jù)系統(tǒng)的量測值來消除隨機(jī)干順序遞推，根據(jù)系統(tǒng)的量測值來消除隨機(jī)干擾，再現(xiàn)系統(tǒng)的狀態(tài)，或根據(jù)系統(tǒng)的量測值擾，再現(xiàn)系統(tǒng)的狀態(tài)，或根據(jù)系統(tǒng)的量測值從被污染的系統(tǒng)中恢復(fù)系統(tǒng)的本來面目。從被污染的系統(tǒng)中恢復(fù)系統(tǒng)的本來面目。 22卡爾曼濾波特點：卡爾曼濾波特點： 卡爾曼濾波是解決狀態(tài)空間模型估計與預(yù)測的卡爾曼濾波是解決狀態(tài)空間模型估計與預(yù)測的有力工具之一，它不需存儲歷史數(shù)據(jù)，就能夠從一有力工具之一，它不需存儲歷史數(shù)據(jù)，就能夠從一系列的不完全以及包含系列的不完全以

20、及包含噪聲噪聲的的測量測量中，估計中，估計動態(tài)系動態(tài)系統(tǒng)統(tǒng)的狀態(tài)。的狀態(tài)。 卡爾曼濾波是一種卡爾曼濾波是一種遞歸遞歸的估計，即只要獲知上的估計，即只要獲知上一時刻狀態(tài)的估計值以及當(dāng)前狀態(tài)的觀測值就可以一時刻狀態(tài)的估計值以及當(dāng)前狀態(tài)的觀測值就可以計算出當(dāng)前狀態(tài)的估計值，因此不需要記錄觀測或計算出當(dāng)前狀態(tài)的估計值，因此不需要記錄觀測或者估計的歷史信息。者估計的歷史信息。 23 卡爾曼濾波器與大多數(shù)我們常用的濾波卡爾曼濾波器與大多數(shù)我們常用的濾波器不同之處，在于它是一種純粹的器不同之處，在于它是一種純粹的時域時域濾波濾波器，不需要像器，不需要像低通濾波器低通濾波器等等頻域頻域濾波器那樣，濾波器那樣，

21、需要在頻域設(shè)計再轉(zhuǎn)換到時域?qū)崿F(xiàn)。需要在頻域設(shè)計再轉(zhuǎn)換到時域?qū)崿F(xiàn)。245.卡爾曼濾波器的軟硬件實現(xiàn)卡爾曼濾波器的軟硬件實現(xiàn) 目前目前,卡爾曼濾波器已經(jīng)有很多不同的實現(xiàn)形式?？柭鼮V波器已經(jīng)有很多不同的實現(xiàn)形式。卡爾曼最初提出的形式現(xiàn)在一般稱為簡單卡爾曼濾卡爾曼最初提出的形式現(xiàn)在一般稱為簡單卡爾曼濾波器。除此以外波器。除此以外,還有施密特擴(kuò)展卡爾曼濾波器還有施密特擴(kuò)展卡爾曼濾波器,信信息濾波器以及平方根濾波器。最常見的卡爾曼濾波息濾波器以及平方根濾波器。最常見的卡爾曼濾波器是鎖相環(huán)器是鎖相環(huán) ，采用，采用FPGA硬件可以實現(xiàn)卡爾曼濾波硬件可以實現(xiàn)卡爾曼濾波器。器。 25硬件實現(xiàn)硬件實現(xiàn)卡爾曼濾波器

22、有良好的濾波效果，但由于其計算量卡爾曼濾波器有良好的濾波效果，但由于其計算量大，當(dāng)采樣率高時，一個采樣周期內(nèi)難以完成計算，大，當(dāng)采樣率高時，一個采樣周期內(nèi)難以完成計算，且計算機(jī)的字長有限，使計算中舍入誤差和截斷誤且計算機(jī)的字長有限，使計算中舍入誤差和截斷誤差積累、傳遞，造成數(shù)值不穩(wěn)定，因此用差積累、傳遞，造成數(shù)值不穩(wěn)定，因此用MCU和和DSP難以實現(xiàn)。難以實現(xiàn)。FPGA可以實現(xiàn)并行計算，它有多可以實現(xiàn)并行計算，它有多個乘法器和累加器并行處理數(shù)據(jù)，采用個乘法器和累加器并行處理數(shù)據(jù)，采用FPGA實現(xiàn)實現(xiàn)的卡爾曼濾波器，由于輸入和輸出數(shù)據(jù)計算同時進(jìn)的卡爾曼濾波器，由于輸入和輸出數(shù)據(jù)計算同時進(jìn)行，因此

23、可以大大提高濾波速度。行，因此可以大大提高濾波速度。 26 一般，數(shù)字濾波器的一般，數(shù)字濾波器的FPGA實現(xiàn)是用實現(xiàn)是用VHDL或或Verilog HDL等硬件描述語言通過編寫底層代碼實等硬件描述語言通過編寫底層代碼實現(xiàn)。這種編程方式效率低，難度大。現(xiàn)。這種編程方式效率低，難度大。 利用利用Altera公司公司FPGA的的DSP開發(fā)工具開發(fā)工具DSP Builder設(shè)計卡爾曼濾波器，比基于硬件描述語言的設(shè)計卡爾曼濾波器，比基于硬件描述語言的設(shè)計設(shè)計,周期更短周期更短,設(shè)計更容易。設(shè)計更容易。 27 基于現(xiàn)場可編程邏輯門陣列基于現(xiàn)場可編程邏輯門陣列FPGA器件和模數(shù)器件和模數(shù)轉(zhuǎn)換器設(shè)計的數(shù)據(jù)采集

24、系統(tǒng)為硬件平臺，進(jìn)行算法轉(zhuǎn)換器設(shè)計的數(shù)據(jù)采集系統(tǒng)為硬件平臺，進(jìn)行算法設(shè)計。設(shè)計?；谀K化設(shè)計思想，設(shè)計時鐘分頻模塊、基于模塊化設(shè)計思想，設(shè)計時鐘分頻模塊、AD轉(zhuǎn)換轉(zhuǎn)換芯片的芯片的FPGA控制模塊和卡爾曼濾波模塊?？柭刂颇K和卡爾曼濾波模塊?？柭鼮V波模塊采用濾波模塊采用DSP Builder設(shè)計，轉(zhuǎn)換成硬件描述設(shè)計，轉(zhuǎn)換成硬件描述語言語言VHDL后，應(yīng)用軟件后，應(yīng)用軟件Modelsim、QuartusII進(jìn)進(jìn)行仿真并完成硬件驗證。行仿真并完成硬件驗證。28軟件實現(xiàn)軟件實現(xiàn) 許多物理進(jìn)程，如路上行駛的車輛、圍繞地許多物理進(jìn)程，如路上行駛的車輛、圍繞地球軌道運轉(zhuǎn)的衛(wèi)星、由繞組電流驅(qū)動的電機(jī)軸

25、或球軌道運轉(zhuǎn)的衛(wèi)星、由繞組電流驅(qū)動的電機(jī)軸或正弦射頻載波信號，均可用線性系統(tǒng)來近似。線正弦射頻載波信號，均可用線性系統(tǒng)來近似。線性系統(tǒng)是指能用如下兩個方程描述的簡單進(jìn)程：性系統(tǒng)是指能用如下兩個方程描述的簡單進(jìn)程： 29狀態(tài)方程：狀態(tài)方程： 輸出方程：輸出方程：在上述方程中，在上述方程中，A、B和和C均為矩陣，均為矩陣，k是時間系數(shù)，是時間系數(shù)，x稱稱為系統(tǒng)狀態(tài)，為系統(tǒng)狀態(tài)，u是系統(tǒng)的已知輸入，是系統(tǒng)的已知輸入，y是所測量的輸出。是所測量的輸出。w和和z表示噪音，其中變量表示噪音，其中變量w稱為進(jìn)程噪音，稱為進(jìn)程噪音，z稱為測量噪音，稱為測量噪音，它們都是向量。它們都是向量。30 卡爾曼濾波的算法流程為：卡爾曼濾波的算法流程為：1.預(yù)估計預(yù)估計X(k)= F(k,k-1)X(k-1) 2.計算預(yù)估計協(xié)方差矩陣計算預(yù)估計協(xié)方差矩陣3.C(k)=F(k,k-1)C(k)F(k,k-1)+T(k,k- 1)Q(k)T(k,k-1)4.Q(k) = U(k)U(k) 5.計算卡爾曼增益矩陣計算卡爾曼增益矩陣6.K(k) = C(k)H(k)H(k)C(k)H(k)+R(k)(-1)7.R(k) = N