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

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

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

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

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

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

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

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

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

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

11、音，z稱為測量噪音，它們都是向量。 則卡爾曼濾波的算法流程為：,1.預(yù)估計X(k)= F(k,k-1)X(k-1)2.計算預(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(k)N(k),8.更新估計 9.X(k)=X(k)+K(k)Y(k)-H(k)X(k)10.計算更新后估計協(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.重復以上步驟該算法可用C語言編程，在計算機上實現(xiàn)。,6.卡爾曼濾波器的應(yīng)用 卡爾曼濾波器最初是專為飛行器導航而研發(fā)的，目前已成功應(yīng)用在許多領(lǐng)域中?？柭鼮V波器主要用來預(yù)估那些只能被系統(tǒng)本身間接或不精確觀測的系統(tǒng)狀態(tài)。許多工程系統(tǒng)和嵌入式系統(tǒng)都需要卡爾曼濾波。,比如,在雷達中，人們感興趣的是跟蹤目標，但目標的位置，速度，加速度的測量值往往在任何時候都有噪聲?？柭鼮V波利用目標