卡爾曼濾波器分類及基本公式課件

1、卡爾曼濾波器（Kalman Filter）濾波的基本概念濾波是什么？所謂濾波，就是從混合在一起的諸多信號中提取出所需要的信號。信號的分類（數(shù)學關系）？（1）確定性信號：可以表示為確定的時間函數(shù)，可確定其在任何時刻的量值。（具有確定的頻譜）（2）隨機信號：不能用確定的數(shù)學關系式來描述的，不能預測其未來任何瞬時值，其值的變化服從統(tǒng)計規(guī)律。（頻譜不確定，功率譜確定）濾波的基本概念確定性信號的濾波可采用低通、高通、帶通、帶阻等模擬濾波器或者計算機通過算法實現(xiàn)常規(guī)濾波隨機信號的濾波根據(jù)有用信號和干擾信號的功率譜設計濾波器維納濾波（Wiener Filtering）或卡爾曼濾波（Kalman Filter

2、）隨機信號的濾波也可以看做是估計問題?？柭鼮V波的由來卡爾曼濾波的由來卡爾曼，全名Rudolf Emil Kalman，匈牙利數(shù)學家，1930年出生于匈牙利首都布達佩斯。1953，1954年于麻省理工學院分別獲得電機工程學士及碩士學位。1957年于哥倫比亞大學獲得博士學位。我們在現(xiàn)代控制理論中要學習的卡爾曼濾波器，正是源于他的博士論文和1960年發(fā)表的論文A New Approach to Linear Filtering and Prediction Problems（線性濾波與預測問題的新方法）。 卡爾曼濾波的由來卡爾曼濾波的由來 卡爾曼濾波理論作為最優(yōu)估計的一種，它的創(chuàng)立是科學技術和社會

3、需要發(fā)展到一定程度的必然結果。在1795年，高斯為測定行星運動軌道而提出最小二乘估計法。為了解決火力控制系統(tǒng)精度跟蹤問題，維納于1942年提出了維納濾波理論，利用有用信號和干擾信號的功率譜確定線性濾波器的頻率特性，首次將數(shù)理統(tǒng)計理論與線性理論有機的聯(lián)系在一起，形成了對隨機信號做平滑、估計或者預測的最優(yōu)估計新理論。但是采用頻域設計法是造成維納濾波器設計困難的根本原因。于是，人們逐漸轉(zhuǎn)向?qū)で笤跁r域內(nèi)直接設計最優(yōu)濾波器的方法，而卡爾曼研究的卡爾曼濾波理論很好的解決了這個問題卡爾曼濾波的特點卡爾曼濾波的特點（1）卡爾曼濾波處理的對象是隨機信號；（2）被處理的信號無有用和干擾之分，濾波的目的是要估計出所

4、有被處理的信號(區(qū)別于維納濾波)；（3）系統(tǒng)的白噪聲激勵和測量噪聲并不是需要濾除的對象，它們的統(tǒng)計特性是估計過程中需要利用的信息；（區(qū)別最小二乘）（4）算法是遞推的，且使用狀態(tài)空間法在時域內(nèi)設計濾波器，適用于對多維隨機過程的估計；（5）被估計量既可以是平穩(wěn)的，也可以是非平穩(wěn)的；（6）估計過程中，只需要考慮過程噪聲和測量噪聲及當前時刻系統(tǒng)狀態(tài)的統(tǒng)計特性。（計算機計算時，所占空間?。┧悸匪悸稰art 1 線性系統(tǒng)的卡爾曼濾波方程線性離散系統(tǒng)線性連續(xù)系統(tǒng)Part 2 非線性系統(tǒng)的卡爾曼濾波方程擴展卡爾曼濾波器 EKF無跡卡爾曼濾波器 UKF例子假設我們要研究一個房間的溫度，以一分鐘為時間單位。根據(jù)我

5、們的經(jīng)驗判斷，這個房間的溫度是恒定的，但是對我們的經(jīng)驗不是完全相信，可能存在上下幾度的偏差，我們把該偏差看做是高斯白噪聲。另外，我們在房間里放一個溫度計，溫度計也不準確，測量值會與實際值存在偏差，我們也把這偏差看做是高斯白噪聲?，F(xiàn)在，我們要根據(jù)我們的經(jīng)驗溫度和溫度計的測量值及它們各自的噪聲來估算出房間的實際溫度?？柭鼮V波的基本方程例子假如我們要估算 k 時刻的實際溫度值。首先你要根據(jù) k-1 時刻的溫度值，來預測 k 時刻的溫度（K時刻的經(jīng)驗溫度）。因為你相信溫度是恒定的，所以你會得到 k 時刻的溫度預測值是跟 k-1 時刻一樣的，假設是 23 度（*公式一），同時該值（預測值）的高斯噪聲的

6、偏差是 5 度（5 是這樣得到的：如果 k-1 時刻估算出的最優(yōu)溫度值的偏差是 3，你對自己預測的不確定度是 4 度，他們平方相加再開方，就是 5（*公式二） 。然后，你從溫度計那里得到了 k時刻的溫度值，假設是25 度，同時該值的偏差是 4 度?？柭鼮V波的基本方程例子卡爾曼濾波的基本方程現(xiàn)在，我們用于估算K時刻房間的實際溫度有兩個溫度值：估計值23度和測量值25度。究竟實際溫度是多少呢？是相信自己還是相信溫度計？究竟相信誰多一點？我們需要用他們的均方誤差來判斷。因為， （*公式三），所以我們可以估算出K時刻的最優(yōu)溫度值為： 度（*公式四）。得到了K時刻的最優(yōu)溫度，下一步就是對K+1時刻的溫

7、度值進行最優(yōu)估算，需要得到K時刻的最優(yōu)溫度（24.56）的偏差，算法如下： （*公式五）就這樣，卡爾曼濾波器就不斷的把均方誤差遞歸，從而估算出最優(yōu)的溫度值，運行速度快，且只保留上一時刻的協(xié)方差。無控制離散型卡爾曼濾波的基本方程（1）狀態(tài)的一步預測方程：（2）均方誤差的一步預測：（3）濾波增益方程（權重）：（4）濾波估計方程（K時刻的最優(yōu)值）：（5）均方誤差更新矩陣（K時刻的最優(yōu)均方誤差）：1 基于離散系統(tǒng)模型的卡爾曼濾波的基本公式1.1 無控制的離散型卡爾曼濾波基本方程 帶有控制的離散型卡爾曼濾波基本方程系統(tǒng)的狀態(tài)方程：系統(tǒng)的測量方程：如果滿足為過程噪聲的協(xié)方差，其為非負定陣；為測量噪聲的協(xié)方

8、差，其為正定陣。1 基于離散系統(tǒng)模型的卡爾曼濾波的基本公式1.2 帶有控制的離散型卡爾曼濾波基本方程 線性離散型卡爾曼濾波方程的一般形式系統(tǒng)方程和測量方程的一般形式：如果滿足為過程噪聲的協(xié)方差，其為非負定陣；為測量噪聲的協(xié)方差，其為正定陣。1 基于離散系統(tǒng)模型的卡爾曼濾波的基本公式1.3 離散型卡爾曼濾波方程的一般形式 引入矩陣 ，對狀態(tài)方程進行等效變換：其中：為過程噪聲的協(xié)方差，其為非負定陣；為測量噪聲的協(xié)方差，其為正定陣。1 基于離散系統(tǒng)模型的卡爾曼濾波的基本公式1.3 離散型卡爾曼濾波方程的一般形式 一般形式的卡爾曼濾波方程（1）狀態(tài)的一步預測方程：（2）均方誤差的一步預測：（3）濾波增

9、益方程（權重）：（4）濾波估計方程（K時刻的最優(yōu)值）：（5）濾波均方誤差更新矩陣（K時刻的最優(yōu)均方誤差）：1 基于離散系統(tǒng)模型的卡爾曼濾波的基本公式1.3 離散型卡爾曼濾波方程的一般形式 離散型卡爾曼濾波基本方程使用要點（1）濾波初值的選取卡爾曼濾波是一種遞推算法，啟動時必須先給初值情況一：一般情況下，取 ，卡爾曼濾波器是無偏的，即濾波穩(wěn)定，但是實際上這樣的初值很難得到；情況二：如果系統(tǒng)是一致完全隨機可控和一致完全隨機可觀測的，則卡爾曼濾波器一定是一致漸近穩(wěn)定的，此時盲目的選取濾波初值不影響最終估計值（大多數(shù)情況下）。1 基于離散系統(tǒng)模型的卡爾曼濾波的基本公式1.4 離散型卡爾曼濾波基本方程使

10、用要點 離散型卡爾曼濾波基本方程使用要點（2）估計均方誤差的等價形式及選用 公式（1）形式簡單，計算量小，但是積累誤差容易使協(xié)方差矩陣失去非負定性甚至對稱性，所以實際中常使用公式（2）；如果在濾波初值對被估計量的統(tǒng)計特性缺乏了解，選取濾波初值盲目，則宜采用公式（3）。1 基于離散系統(tǒng)模型的卡爾曼濾波的基本公式1.4 離散型卡爾曼濾波基本方程使用要點 離散型卡爾曼濾波基本方程使用要點（3）連續(xù)系統(tǒng)離散化 卡爾曼濾波的基本方程只適用于系統(tǒng)方程和測量方程均為離散的情況，但實際的物理系統(tǒng)一般都是連續(xù)的，動力學特性用連續(xù)微分方程來描述，所以在使用基本方程之前，需要對系統(tǒng)方程和測量方程進行離散化處理。 連

11、續(xù)系統(tǒng)的離散化處理包括對過程白噪聲的等效離散化處理。1 基于離散系統(tǒng)模型的卡爾曼濾波的基本公式1.4 離散型卡爾曼濾波基本方程使用要點 連續(xù)系統(tǒng)的狀態(tài)空間表達式為：為非負定矩陣；為正定陣其中：2 基于連續(xù)系統(tǒng)模型的卡爾曼濾波的基本公式 連續(xù)系統(tǒng)模型的卡爾曼濾波基本方程2 基于連續(xù)系統(tǒng)模型的卡爾曼濾波的基本公式 與連續(xù)系統(tǒng)模型等效的離散系統(tǒng)的數(shù)學模型：連續(xù)系統(tǒng)模型的卡爾曼濾波基本方程其中：是零均值分段常值白噪聲過程，其協(xié)方差為：連續(xù)系統(tǒng)模型的卡爾曼濾波基本方程2 基于連續(xù)系統(tǒng)模型的卡爾曼濾波的基本公式 （4）濾波估計方程（K時刻的最優(yōu)值）：（5）濾波均方誤差更新矩陣（K時刻的最優(yōu)均方誤差）：將其

12、變形求極限3 非線性系統(tǒng)的卡爾曼濾波方程普通卡爾曼濾波是在線性高斯情況下利用最小均方誤差準則獲得目標的動態(tài)估計，適應于過程和測量都屬于線性系統(tǒng)， 且誤差符合高斯分布的系統(tǒng)。 但是實際上很多系統(tǒng)都存在一定的非線性， 表現(xiàn)在過程方程 （狀態(tài)方程）是非線性的，或者觀測與狀態(tài)之間的關系（測量方程）是非線性的。這種情況下就不能使用一般的卡爾曼濾波了。解決的方法是將非線性關系進行線性近似，將其轉(zhuǎn)化成線性問題。對于非線性問題線性化常用的兩大途徑：（1） 將非線性環(huán)節(jié)線性化，對高階項采用忽略或逼近措施；（EKF）（2）用采樣方法近似非線性分布. ( UKF)擴展卡爾曼濾波器（EKF）3 非線性系統(tǒng)的卡爾曼濾波

13、方程3.1 擴展卡爾曼濾波器非線性系統(tǒng)模型：其中：假設在 時刻已獲得系統(tǒng)狀態(tài) 的濾波估計 ，將 和 在 附近線性化，即非線性系統(tǒng)將隨時在新估計的結果附近進行線性化。3 非線性系統(tǒng)的卡爾曼濾波方程3.1 擴展卡爾曼濾波器擴展卡爾曼濾波器（EKF）將 和 在 附近展開成泰勒級數(shù)，忽略二階以上的高階項，則得線性化方程為：將其變形，取非線性系統(tǒng)線性化后的系統(tǒng)狀態(tài)空間表達式為：3 非線性系統(tǒng)的卡爾曼濾波方程3.1 擴展卡爾曼濾波器EKF基本方程3 非線性系統(tǒng)的卡爾曼濾波方程3.1 擴展卡爾曼濾波器 系統(tǒng)模型測量模型初始條件其他規(guī)定狀態(tài)估計方程誤差協(xié)方差增益矩陣EKF的不足當非線性函數(shù)的Taylor展開式

14、的高階項無法忽略時， 線性化會使系統(tǒng)產(chǎn)生較大的誤差，甚至于濾波器難以穩(wěn)定；在許多實際問題中很難得到非線性函數(shù)的雅克比矩陣求導；EKF需要求導，所以必須清楚了解非線性函數(shù)的具體形式，無法作到黑盒封裝，從而難以模塊化應用。3 非線性系統(tǒng)的卡爾曼濾波方程3.1 擴展卡爾曼濾波器 （EKF)3 非線性系統(tǒng)的卡爾曼濾波方程3.2 無跡卡爾曼濾波器（UKF） 由于近似非線性函數(shù)的概率密度分布比近似非線性函數(shù)更容易，使用采樣方法近似非線性分布來解決非線性問題的途徑在最近得到了人們的廣泛關注。UKF 是一大類用采樣策略逼近非線性分布的方法! UKF以Unscented Transform(UT,無跡變換)為基

15、礎, 采用卡爾曼線性濾波框架, 具體的采樣形式為確定性采樣。UT變換采用確定性采樣策略，用多個粒子點逼近函數(shù)的概率密度分布，從而獲得更高階次的均值與方差。無跡變換（UT）3 非線性系統(tǒng)的卡爾曼濾波方程3.2 無跡卡爾曼濾波器 （UKF）變換原理：基于當前狀態(tài) 的均值 和方差 ，構造一組固定數(shù)目的采樣點，利用這組采樣點的樣本均值和樣本方差逼近非線性變化 的均值 和方差3 非線性系統(tǒng)的卡爾曼濾波方程3.2 無跡卡爾曼濾波器 （UKF） UT變換的具體過程（三步）：1）取點：根據(jù)輸入變量 的統(tǒng)計量 和 ，選擇一種 點采樣策略。得到輸入變量的 點 以及對應的權值 和 ；2）點非線性變換：對所采樣的輸入變量 點集 中的每個 點進行 線性變換. 得到變換后的 點集 ：3）新變量的統(tǒng)計特性：對變換后的變 點集 進行加權處理，從而得到輸出變量 的統(tǒng)計量 和 。具體的權值仍然依據(jù)對輸入變量 進行采樣的各個 點的對應權值。UKF整個估計過程：（1） 點采樣；（2）利用狀態(tài)方程傳遞采樣點；（3）利用預測采樣點及權值 計算預測均值和協(xié)方差；（4）利用( 2)