動態(tài)線性系統(tǒng)的卡爾曼濾波解析課件

1、第九講： 動態(tài)線性系統(tǒng)的卡爾曼濾波工程實踐中的估計問題有兩類：1、系統(tǒng)的參數(shù)部分或全部未知-有待確定；2、實施最優(yōu)控制時，需要了解系統(tǒng)的狀態(tài)，而系統(tǒng)中部分或全部狀態(tài)變量不能直接測得。這樣就：包含了兩類估計問題：參數(shù)估計 狀態(tài)估計。不同估計準則會導(dǎo)致不同的估計方法；同樣不同的觀測信息，也會導(dǎo)致不同的估計方法。估計問題都是由三個部分構(gòu)成：1、函數(shù)模型（估計約束條件）2、隨機模型（估計驗前信息）3、估計準則。前面討論的各種最小二乘分解法，都是針對靜態(tài)函數(shù)模型的。即通過觀測向量L所估計的參數(shù)向量是不依賴于時間t的。但是，在許多實際的問題中，如：應(yīng)用計算技術(shù)進行適時控制的需要；大壩的變形監(jiān)測、GPS導(dǎo)航

2、等等，以上被估計參數(shù)都是隨著時間的變化而不斷變化的，必需在觀測過程中不斷的對系統(tǒng)的狀態(tài)進行估計，并且隨著新觀測值的獲得不斷修正這種估計。 卡爾曼濾波理論便是適應(yīng)適時控制的需要，對系統(tǒng)的狀態(tài)參數(shù)進行線性估計的一種遞推算法。Kalman濾波是卡爾曼（kalman）于1960年提出的一種濾波方法。特點：是對狀態(tài)空間進行估計。狀態(tài)空間估計一般是動態(tài)估計。估計過程利用：系統(tǒng)狀態(tài)方程、觀測方程、系統(tǒng)過程噪聲以及觀測噪聲的統(tǒng)計特性構(gòu)成濾波算法。Kalman采用遞推算法。何謂kalman濾波？如：要研究的對象是一個房間的溫度，根據(jù)經(jīng)驗，房間溫度恒定的。1）根據(jù)上一時刻的溫度值來推算下一時刻的溫度 （稱：預(yù)測值

3、或估計值，有偏差）；2）溫度計測量（稱：觀測值，有偏差）。根據(jù)經(jīng)驗預(yù)測估計值和當(dāng)前狀態(tài)的觀測值，并結(jié)合其各自噪聲來估算出房間的實際溫度值?；靖拍睿?、狀態(tài)向量隨時間不斷變化的隨機變量稱為動態(tài)系統(tǒng)在時刻的“狀態(tài)”向量。如：GPS導(dǎo)航、變形監(jiān)測中是以點的位置、運動速度為狀態(tài)向量的。狀態(tài)向量：點的位置、運動速度、加速度。2、狀態(tài)方程（描述隨機過程的狀態(tài)變化）看一簡單例子：勻加速直線運動K點K+1點K點：位置 xk K+1點：位置xk+1 速度 vk 速度vk+1 加速度k 加速度k+1 t狀態(tài)向量則K點與K+1點3個量之間關(guān)系為： 勻加速，故 xk+1 = xk+vkt+1/2t2 vk+1 =

4、vk+t k = k+1 = 狀態(tài)用矩陣表示（狀態(tài)方程） 其中： 為系統(tǒng)矩陣，表示位置變化的轉(zhuǎn)移 ； 為狀態(tài)（過程）噪聲?？紤]系統(tǒng)噪聲，狀態(tài)方程為：3、觀測方程（描述隨機過程與觀測量的關(guān)系） 在K+1處對點的位置（狀態(tài)的一項）進行了觀測，得觀測向量k+1，則觀測方程：其中：為觀測噪聲。線性系統(tǒng)：狀態(tài)方程為線性的，稱線性系統(tǒng)。觀測方程：觀測向量與狀態(tài)向量之間存在的某種函數(shù)關(guān)系，也稱為輸出方程。白噪聲：協(xié)方差為零的噪聲。有色噪聲：不同時刻的動態(tài)噪聲或觀測噪聲是相關(guān)的。下面將針對以下問題進行學(xué)習(xí)：1、隨機線性系統(tǒng)的數(shù)學(xué)模型（離散線性系統(tǒng)）；2、隨機線性離散系統(tǒng)的kalman濾波方程；3、kalman

5、濾波的應(yīng)用。 一、 離散線性系統(tǒng)的數(shù)學(xué)模型離散時間系統(tǒng)：如僅在確定的瞬間來研究系統(tǒng)的性能，則把這樣的系統(tǒng)叫做離散時間系統(tǒng)。包括兩種： 1）系統(tǒng)本身就是一個離散系統(tǒng)； 2）本身是連續(xù)系統(tǒng)，為研究方便，僅在離散時間內(nèi)研究其性能。1、函數(shù)模型：狀態(tài)方程和觀測方程稱為動態(tài)系統(tǒng)的函數(shù)模型。離散系統(tǒng)的狀態(tài)方程和觀測方程為：是狀態(tài)外加的控制（輸入）向量，是常量（非隨機）即：非隨機控制項。一般忽略不計。稱系數(shù)矩陣。對于連續(xù)線性系統(tǒng)的狀態(tài)方程和觀測方程：設(shè)位置、速度為狀態(tài)向量，加速度為系統(tǒng)噪聲；按照運動狀態(tài)知它們之間關(guān)系（是一個微分方程）：觀測方程： 2、離散線性系統(tǒng)的隨機模型：Dirac函數(shù)二、 離散線性系統(tǒng)

6、的卡爾曼濾波1）狀態(tài)方程、觀測方程離散線性系統(tǒng)的估計 即利用觀測向量L1，L2，LK，根據(jù)其數(shù)學(xué)模型求定第j時刻狀態(tài)向量X的最佳估值 。估計量 分為三種情況:=，稱為濾波；(用依據(jù)過去直至現(xiàn)在的觀測值來估計現(xiàn)在的狀態(tài)，多用于隨機系統(tǒng)的實時控制。），稱為預(yù)測或外推；（依據(jù)過去直至現(xiàn)在的觀測值來預(yù)測未來的狀態(tài)，用于對系統(tǒng)未來狀態(tài)的預(yù)測和實時控制），平滑或內(nèi)插。（依據(jù)過去直至現(xiàn)在的觀測值來估計過去歷史狀態(tài)，用于通過分析實驗或?qū)嶒灁?shù)據(jù)對系統(tǒng)進行評估）預(yù)測是濾波的基礎(chǔ)，濾波又是平滑的基礎(chǔ)。2）隨機模型：上述模型的卡爾曼慮波稱為“完全不相關(guān)的白噪聲作用下的卡爾曼慮波”。注意：隨機模型參數(shù)僅僅給出初始狀態(tài)的

7、統(tǒng)計特性。即其他參數(shù)均為非隨機的。狀態(tài)估計的方法：1）最小方差估計或極大驗后估計原理；2）廣義最小二乘原理。以下將按照廣義最小二乘原理并采用逐次平差方法導(dǎo)出卡爾曼濾波方程。設(shè)有K個觀測向量，則狀態(tài)方程和觀測方程為：為方便起見（類似觀測方程），將狀態(tài)方程改寫為：并有：相應(yīng)觀測方程： 的方差陣：以上實質(zhì)是：將動態(tài)噪聲項看成是相應(yīng)于某種觀測向量的觀測噪聲。設(shè)有K個觀測向量，則狀態(tài)方程和觀測方程為：因只有初始狀態(tài)是隨機參數(shù)，根據(jù)廣義最小二乘原理，應(yīng)將隨機參數(shù)X0的先驗期望看成是虛擬觀測值。則可以寫出誤差方程：這樣，就將上述動態(tài)系統(tǒng)的估計問題變換為一個最小二乘問題。這時，式中 均看作是非隨機參數(shù)；而 看

8、作是方差陣為 的相應(yīng)“觀測值”，它們之間的協(xié)方差陣均為零。進行“逐次平差”：(1)取第一、第二式進行最小二乘間接平差法方程形如：其中:整理法方程，可求解利用協(xié)方差傳播律可得解的誤差方差陣、協(xié)方差陣(2)以上解作為虛擬觀測值，與L1 的誤差方程一起作第二次平差:誤差方程組、解法方程并整理，得：(3)再以上解作為虛擬觀測值，與LS（1）一起作第三次平差：誤差方程解為：（4）依此類推，就可得到遞推計算公式（5個），即“卡爾曼濾波方程”。（可分3步進行）一步預(yù)測值：濾波值：增益矩陣： 時間更新（預(yù)測值）（1）計算先驗狀態(tài)估計值（2）計算先驗誤差估計值測量更新（修正估計值）（1）計算修正矩陣（2）更新觀

9、測值（3）更新誤差協(xié)方差關(guān)于濾波公式:（1） 項稱為“預(yù)報殘差”（新息）；（2）JK濾波增益矩陣；公式的物理意義是:濾波值等于預(yù)報值加一修正項,該修正項由預(yù)報殘差乘增益矩陣構(gòu)成?？柭鼞]波的實質(zhì)利用卡爾曼濾波器進行濾波時,需要知道系統(tǒng)的狀態(tài)方程和量測方程。計算包括： 1）從一狀態(tài)推（K）求另一狀態(tài)（K+1）的預(yù)測值； 2）從此狀態(tài)本身有的觀測LK+1推求該狀態(tài)估值；即：歷史信息與觀測信息的綜合。三、卡爾曼濾波的應(yīng)用以下通過幾個實例來進一步理解卡爾曼濾波過程。例1 設(shè)狀態(tài)方程和觀測方程為：隨機模型是：又已知兩次觀測數(shù)據(jù)L1=4，L2=2，求解：（1） 計算一步預(yù)測值（2）求增益距陣J1，（3）計

10、算（4）以（1）-（3）的公式計算（5）計算例2 將一物體以初速度51米/秒從地面垂直上拋，在時刻tK觀測物體離地面的距離SK，得觀測值LK如下：設(shè)各觀測值的觀測是白噪聲，其方差Di=1；初始狀態(tài)（初始距離和速度）為其先驗方差為設(shè)不考慮動態(tài)噪聲，求在各時刻tK物體離地面的距離和瞬時速度的估值 和它們的方差 。tK（秒）0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0 8.0 9.0LK（米）0.0 45.3 80.1 105.8 121.7 123.9 109.5 85.5 52.3（1）根據(jù)運動學(xué)，建立k-1時刻與k時刻物體離地面距離、瞬時速度的關(guān)系，即動態(tài)方程?；?qū)憺椋海ǚ请S

11、機項）（時間間隔1秒）（2）觀測方程為或?qū)憺?）計算預(yù)報值2）計算預(yù)報值的方差3）計算增益距陣J4）計算估值5）計算估值方差6）按以上1）-5）重復(fù)計算求得各時刻估值及方差模型（CV)模型(CA)將加速度的變化率視為動態(tài)噪聲加速度視為動態(tài)噪聲例3在動態(tài)水準監(jiān)測中常用的動態(tài)方程：思考： 在GPS變形監(jiān)測中，將變形體視為一個動態(tài)系統(tǒng)，將一組觀測值作為系統(tǒng)輸出，則卡爾曼濾波就可以用來描述這個變形體的運動情況。 試寫出GPS網(wǎng)的狀態(tài)方程、觀測方程。1）設(shè)GPS監(jiān)測網(wǎng)有n個監(jiān)測點組成，以GPS點三維位置和三維速率為狀態(tài)向量；2）設(shè)i點在時刻t的： 位置向量為- 瞬時速率為- 將瞬時加速度看成是隨機干擾3

12、）i點的狀態(tài)向量為：4）全網(wǎng)的狀態(tài)向量為：5)離散卡爾曼濾波的狀態(tài)方程純量形式為：卡爾曼濾波的優(yōu)點：1)濾波方程是一組遞推計算公式,計算過程是一個不斷預(yù)測、修正的過程；2)不需保留用過的觀測值序列,并且當(dāng)?shù)玫叫碌挠^測數(shù)據(jù)時,可隨時計算新的濾波值,便于實時處理觀測成果,把參數(shù)估計和預(yù)報有機地結(jié)合起來.3)卡爾曼濾波特別適合變形監(jiān)測、組合導(dǎo)航等動態(tài)數(shù)據(jù)處理。4）卡爾曼濾波值是最小方差無偏估值?？柭鼮V波應(yīng)用于動態(tài)觀測系統(tǒng)中，可實時獲得系統(tǒng)當(dāng)前狀態(tài)；且濾波精度較高；卡爾曼濾波除了可掌握系統(tǒng)的當(dāng)前狀態(tài)外，還可預(yù)測系統(tǒng)未來。（可以模擬動態(tài)目標(biāo)系統(tǒng)的變化規(guī)律等）（預(yù)測和平滑）我們已介紹的近代平差方法：1）