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

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

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

3、或估計(jì)值，有偏差）；2）溫度計(jì)測(cè)量（稱：觀測(cè)值，有偏差）。根據(jù)經(jīng)驗(yàn)預(yù)測(cè)估計(jì)值和當(dāng)前狀態(tài)的觀測(cè)值，并結(jié)合其各自噪聲來(lái)估算出房間的實(shí)際溫度值。基本概念：1、狀態(tài)向量隨時(shí)間不斷變化的隨機(jī)變量稱為動(dòng)態(tài)系統(tǒng)在時(shí)刻的“狀態(tài)”向量。如：GPS導(dǎo)航、變形監(jiān)測(cè)中是以點(diǎn)的位置、運(yùn)動(dòng)速度為狀態(tài)向量的。狀態(tài)向量：點(diǎn)的位置、運(yùn)動(dòng)速度、加速度。2、狀態(tài)方程（描述隨機(jī)過程的狀態(tài)變化）看一簡(jiǎn)單例子：勻加速直線運(yùn)動(dòng)K點(diǎn)K+1點(diǎn)K點(diǎn)：位置 xk K+1點(diǎn)：位置xk+1 速度 vk 速度vk+1 加速度k 加速度k+1 t狀態(tài)向量則K點(diǎn)與K+1點(diǎn)3個(gè)量之間關(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、觀測(cè)方程（描述隨機(jī)過程與觀測(cè)量的關(guān)系） 在K+1處對(duì)點(diǎn)的位置（狀態(tài)的一項(xiàng)）進(jìn)行了觀測(cè)，得觀測(cè)向量k+1，則觀測(cè)方程：其中：為觀測(cè)噪聲。線性系統(tǒng)：狀態(tài)方程為線性的，稱線性系統(tǒng)。觀測(cè)方程：觀測(cè)向量與狀態(tài)向量之間存在的某種函數(shù)關(guān)系，也稱為輸出方程。白噪聲：協(xié)方差為零的噪聲。有色噪聲：不同時(shí)刻的動(dòng)態(tài)噪聲或觀測(cè)噪聲是相關(guān)的。下面將針對(duì)以下問題進(jìn)行學(xué)習(xí)：1、隨機(jī)線性系統(tǒng)的數(shù)學(xué)模型（離散線性系統(tǒng)）；2、隨機(jī)線性離散系統(tǒng)的kalman濾波方程；3、kalman

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

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

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

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

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

10、算（4）以（1）-（3）的公式計(jì)算（5）計(jì)算例2 將一物體以初速度51米/秒從地面垂直上拋，在時(shí)刻tK觀測(cè)物體離地面的距離SK，得觀測(cè)值LK如下：設(shè)各觀測(cè)值的觀測(cè)是白噪聲，其方差Di=1；初始狀態(tài)（初始距離和速度）為其先驗(yàn)方差為設(shè)不考慮動(dòng)態(tài)噪聲，求在各時(shí)刻tK物體離地面的距離和瞬時(shí)速度的估值 和它們的方差 。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ù)運(yùn)動(dòng)學(xué)，建立k-1時(shí)刻與k時(shí)刻物體離地面距離、瞬時(shí)速度的關(guān)系，即動(dòng)態(tài)方程?；?qū)憺椋海ǚ请S

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

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