卡爾曼濾波器-學(xué)術(shù)講座

1、卡爾曼濾波簡(jiǎn)介背景介紹：Kalman，匈牙利數(shù)學(xué)家。 卡爾曼濾波器源于他的博士論 文和1960年發(fā)表的論文A New Approach to Linear Filtering and Prediction Problems（線 性濾波與預(yù)測(cè)問題的新方法）。 估計(jì)原理和卡爾曼濾波1. 狀態(tài)估計(jì)原理 2. 為什么要用狀態(tài)估計(jì)理論 3. 經(jīng)典控制理論與現(xiàn)代控制理論 4. 什么是卡爾曼濾波5.卡爾曼濾波器的軟硬件實(shí)現(xiàn)6.卡爾曼濾波器的應(yīng)用 1.狀態(tài)估計(jì)原理 狀態(tài)估計(jì)是卡爾曼濾波的重要組成部分。一般來說，根據(jù)觀測(cè)數(shù)據(jù)對(duì)隨機(jī)量進(jìn)行定量推斷就是估計(jì)問題，特別是對(duì)動(dòng)態(tài)行為的狀態(tài)估計(jì)，它能實(shí)現(xiàn)實(shí)時(shí)運(yùn)行狀態(tài)的估計(jì)

2、和預(yù)測(cè)功能。比如對(duì)飛行器狀態(tài)估計(jì)。 狀態(tài)估計(jì)對(duì)于了解和控制一個(gè)系統(tǒng)具有重要意義，所應(yīng)用的方法屬于統(tǒng)計(jì)學(xué)中的估計(jì)理論。最常用的是最小二乘估計(jì)，線性最小方差估計(jì)、最小方差估計(jì)、遞推最小二乘估計(jì)等。其他如風(fēng)險(xiǎn)準(zhǔn)則的貝葉斯估計(jì)、最大似然估計(jì)、隨機(jī)逼近等方法也都有應(yīng)用。 受噪聲干擾的狀態(tài)量是個(gè)隨機(jī)量，不可能測(cè)得精確值，但可對(duì)它進(jìn)行一系列觀測(cè)，并依據(jù)一組觀測(cè)值，按某種統(tǒng)計(jì)觀點(diǎn)對(duì)它進(jìn)行估計(jì)。使估計(jì)值盡可能準(zhǔn)確地接近真實(shí)值，這就是最優(yōu)估計(jì)。真實(shí)值與估計(jì)值之差稱為估計(jì)誤差。若估計(jì)值的數(shù)學(xué)期望與真實(shí)值相等，這種估計(jì)稱為無偏估計(jì)。 卡爾曼提出的遞推最優(yōu)估計(jì)理論，采用狀態(tài)空間描述法，在算法采用遞推形式，卡爾曼濾波能處

3、理多維和非平穩(wěn)的隨機(jī)過程。 卡爾曼濾波理論的提出，克服了威納濾波理論的局限性使其在工程上得到了廣泛的應(yīng)用，尤其在控制、制導(dǎo)、導(dǎo)航、通訊等現(xiàn)代工程方面。2為什么要用狀態(tài)估計(jì)理論 在許多實(shí)際問題中，由于隨機(jī)過程的存在，常常不能直接獲得系統(tǒng)的狀態(tài)參數(shù)，需要從夾雜著隨機(jī)干擾的觀測(cè)信號(hào)中分離出系統(tǒng)的狀態(tài)參數(shù)。例如，飛機(jī)在飛行過程中所處的位置、速度等狀態(tài)參數(shù)需要通過雷達(dá)或其它測(cè)量裝置進(jìn)行觀測(cè)，而雷達(dá)等測(cè)量裝置也存在隨機(jī)干擾，因此在觀測(cè)到飛機(jī)的位置、速度等信號(hào)中就夾雜著隨機(jī)干擾，要想正確地得到飛機(jī)的狀態(tài)參數(shù)是不可能的，只能根據(jù)觀測(cè)到的信號(hào)來估計(jì)和預(yù)測(cè)飛機(jī)的狀態(tài)，這就是估計(jì)問題。 從觀測(cè)到的信號(hào)中估計(jì)出狀態(tài)的

4、估值，并且希望估值與狀態(tài)的真值越小越好，即要求有： 成立； 因此存在最優(yōu)估計(jì)問題，這就是卡爾曼濾波。 卡爾曼濾波的最優(yōu)估計(jì)需滿足以下三個(gè)條件： 無偏性，即估計(jì)值的均值等于狀態(tài)的真值； 估計(jì)的方差最??； 實(shí)時(shí)性。 從以上分析可以看出卡爾曼濾波就是在有隨機(jī)干擾和噪聲的情況下，以線性最小方差估計(jì)方法給出狀態(tài)的最優(yōu)估計(jì)值，卡爾曼濾波是在統(tǒng)計(jì)的意義上給出最接近狀態(tài)真值的估計(jì)值。因此卡爾曼濾波在空間技術(shù)、測(cè)軌、導(dǎo)航、攔截與通訊等方面獲得了廣泛的應(yīng)用。3經(jīng)典控制理論與現(xiàn)代控制理論 經(jīng)典控制理論只適應(yīng)與單輸入單輸出的線性定常系統(tǒng)，研究方法是傳遞函數(shù)。傳遞函數(shù)在本質(zhì)上是一種頻率法，要靠各個(gè)頻率分量描述信號(hào)。因此

5、，頻率法限制了系統(tǒng)對(duì)整個(gè)過程在時(shí)間域內(nèi)進(jìn)行控制的能力，所以經(jīng)典控制理論很難實(shí)現(xiàn)實(shí)時(shí)控制。同時(shí)，經(jīng)典控制理論也很難實(shí)現(xiàn)最優(yōu)控制。 經(jīng)典控制系統(tǒng)的組成 由于經(jīng)典控制理論的上述局限性，隨著科學(xué)技術(shù)的發(fā)展，特別是空間技術(shù)和各類高速飛行器的快速發(fā)展，要求控制高速度、高精度的受控對(duì)象，控制系統(tǒng)更加復(fù)雜，要求控制理論解決多輸入多輸出、非線性以及最優(yōu)控制等設(shè)計(jì)問題。這些新的控制要求經(jīng)典控制理論是無法解決的。 現(xiàn)代控制理論是建立在狀態(tài)空間基礎(chǔ)上的，它不用傳遞函數(shù)，而是用狀態(tài)向量方程作為基本工具，因此可以用來分析多輸入多輸出、非線性以及時(shí)變復(fù)雜系統(tǒng)的研究?，F(xiàn)代控制理論本質(zhì)上是時(shí)域法，信號(hào)的描述和傳遞都是在時(shí)間域進(jìn)

6、行，所以現(xiàn)代控制理論具有實(shí)現(xiàn)實(shí)時(shí)控制的能力。由于采用了狀態(tài)空間法，現(xiàn)代控制理論有利于設(shè)計(jì)人員根據(jù)給定的性能指標(biāo)設(shè)計(jì)出最優(yōu)的控制系統(tǒng)。 卡爾曼濾波控制系統(tǒng)結(jié)構(gòu)圖 由于系統(tǒng)的狀態(tài)x是不確定的，卡爾曼濾波器的任務(wù)就是在有隨機(jī)干擾w和噪聲v的情況下給出系統(tǒng)狀態(tài)x的最優(yōu)估算值 ，它在統(tǒng)計(jì)意義下最接近狀態(tài)的真值x，從而實(shí)現(xiàn)最優(yōu)控制u( )的目的。 4.什么是卡爾曼濾波： 卡爾曼濾波是美國(guó)工程師Kalman 在線性最小方差估計(jì)的基礎(chǔ)上，提出的在數(shù)學(xué)結(jié)構(gòu)上比較簡(jiǎn)單的而且是最優(yōu)線性遞推濾波方法，具有計(jì)算量小、存儲(chǔ)量低，實(shí)時(shí)性高的優(yōu)點(diǎn)。特別是對(duì)經(jīng)歷了初始濾波后的過渡狀態(tài)，濾波效果非常好。 卡爾曼濾波是以最小均方誤

7、差為估計(jì)的最佳準(zhǔn)則，來尋求一套遞推估計(jì)的算法，其基本思想是：采用信號(hào)與噪聲的狀態(tài)空間模型，利用前一時(shí)刻的估計(jì)值和現(xiàn)時(shí)刻的觀測(cè)值來更新對(duì)狀態(tài)變量的估計(jì)，求出現(xiàn)在時(shí)刻的估計(jì)值。它適合于實(shí)時(shí)處理和計(jì)算機(jī)運(yùn)算。 卡爾曼濾波的實(shí)質(zhì)是由量測(cè)值重構(gòu)系統(tǒng)的狀態(tài)向量。它以“預(yù)測(cè)實(shí)測(cè)修正”的順序遞推，根據(jù)系統(tǒng)的量測(cè)值來消除隨機(jī)干擾，再現(xiàn)系統(tǒng)的狀態(tài)，或根據(jù)系統(tǒng)的量測(cè)值從被污染的系統(tǒng)中恢復(fù)系統(tǒng)的本來面目。 卡爾曼濾波特點(diǎn)： 卡爾曼濾波是解決狀態(tài)空間模型估計(jì)與預(yù)測(cè)的有力工具之一，它不需存儲(chǔ)歷史數(shù)據(jù)，就能夠從一系列的不完全以及包含噪聲的測(cè)量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)?？柭鼮V波是一種遞歸的估計(jì)，即只要獲知上一時(shí)刻狀態(tài)的估計(jì)值

8、以及當(dāng)前狀態(tài)的觀測(cè)值就可以計(jì)算出當(dāng)前狀態(tài)的估計(jì)值，因此不需要記錄觀測(cè)或者估計(jì)的歷史信息。 卡爾曼濾波器與大多數(shù)我們常用的濾波器不同之處，在于它是一種純粹的時(shí)域?yàn)V波器，不需要像低通濾波器等頻域?yàn)V波器那樣，需要在頻域設(shè)計(jì)再轉(zhuǎn)換到時(shí)域?qū)崿F(xiàn)。5.卡爾曼濾波器的軟硬件實(shí)現(xiàn) 目前,卡爾曼濾波器已經(jīng)有很多不同的實(shí)現(xiàn)形式。卡爾曼最初提出的形式現(xiàn)在一般稱為簡(jiǎn)單卡爾曼濾波器。除此以外,還有施密特?cái)U(kuò)展卡爾曼濾波器,信息濾波器以及平方根濾波器。最常見的卡爾曼濾波器是鎖相環(huán) ，采用FPGA硬件可以實(shí)現(xiàn)卡爾曼濾波器。 硬件實(shí)現(xiàn)：卡爾曼濾波器有良好的濾波效果，但由于其計(jì)算量大，當(dāng)采樣率高時(shí)，一個(gè)采樣周期內(nèi)難以完成計(jì)算，且計(jì)

9、算機(jī)的字長(zhǎng)有限，使計(jì)算中舍入誤差和截?cái)嗾`差積累、傳遞，造成數(shù)值不穩(wěn)定，因此用MCU和DSP難以實(shí)現(xiàn)。FPGA可以實(shí)現(xiàn)并行計(jì)算，它有多個(gè)乘法器和累加器并行處理數(shù)據(jù)，采用FPGA實(shí)現(xiàn)的卡爾曼濾波器，由于輸入和輸出數(shù)據(jù)計(jì)算同時(shí)進(jìn)行，因此可以大大提高濾波速度。 一般，數(shù)字濾波器的FPGA實(shí)現(xiàn)是用VHDL或Verilog HDL等硬件描述語言通過編寫底層代碼實(shí)現(xiàn)。這種編程方式效率低，難度大。利用Altera公司FPGA的DSP開發(fā)工具DSP Builder設(shè)計(jì)卡爾曼濾波器，比基于硬件描述語言的設(shè)計(jì),周期更短,設(shè)計(jì)更容易。 基于現(xiàn)場(chǎng)可編程邏輯門陣列FPGA器件和模數(shù)轉(zhuǎn)換器設(shè)計(jì)的數(shù)據(jù)采集系統(tǒng)為硬件平臺(tái)，進(jìn)行

10、算法設(shè)計(jì)?；谀K化設(shè)計(jì)思想，設(shè)計(jì)時(shí)鐘分頻模塊、AD轉(zhuǎn)換芯片的FPGA控制模塊和卡爾曼濾波模塊?？柭鼮V波模塊采用DSP Builder設(shè)計(jì)，轉(zhuǎn)換成硬件描述語言VHDL后，應(yīng)用軟件Modelsim、QuartusII進(jìn)行仿真并完成硬件驗(yàn)證。軟件實(shí)現(xiàn)： 許多物理進(jìn)程，如路上行駛的車輛、圍繞地球軌道運(yùn)轉(zhuǎn)的衛(wèi)星、由繞組電流驅(qū)動(dòng)的電機(jī)軸或正弦射頻載波信號(hào)，均可用線性系統(tǒng)來近似。線性系統(tǒng)是指能用如下兩個(gè)方程描述的簡(jiǎn)單進(jìn)程： 狀態(tài)方程： 輸出方程： 在上述方程中，A、B和C均為矩陣，k是時(shí)間系數(shù)，x稱為系統(tǒng)狀態(tài)，u是系統(tǒng)的已知輸入，y是所測(cè)量的輸出。w和z表示噪音，其中變量w稱為進(jìn)程噪音，z稱為測(cè)量噪音，

11、它們都是向量。 則卡爾曼濾波的算法流程為： 1.預(yù)估計(jì)X(k)= F(k,k-1)X(k-1)2.計(jì)算預(yù)估計(jì)協(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.計(jì)算卡爾曼增益矩陣6.K(k) = C(k)H(k)H(k)C(k)H(k)+R(k)(-1)7.R(k) = N(k)N(k) 8.更新估計(jì) 9.X(k)=X(k)+K(k)Y(k)-H(k)X(k)10.計(jì)算更新后估計(jì)協(xié)防差矩陣11.C(k) = I-K(k)H(k)C(k)I- K(k)H(k)+K(k)R(k)K(k)12.X(k+1) = X(k)13.C(k+1) = C(k)14.重復(fù)以上步驟該算法可用C語言編程，在計(jì)算機(jī)上實(shí)現(xiàn)。 6.卡爾曼濾波器的應(yīng)用 卡爾曼濾波器最初是專為飛行器導(dǎo)航而研發(fā)的，目前已成功應(yīng)用在許多領(lǐng)域中?？柭鼮V波器主要用來預(yù)估那些只能被系統(tǒng)本身間接或不精確觀測(cè)的系統(tǒng)狀態(tài)。許多工程系