離散卡爾曼濾波

1、第1頁，共37頁。1 1 卡爾曼濾波與最優(yōu)估計卡爾曼濾波與最優(yōu)估計卡爾曼濾波是一種最優(yōu)估計技術卡爾曼濾波是一種最優(yōu)估計技術 ！ 它能將僅與部分狀態(tài)有關的測量值進行處理，得出它能將僅與部分狀態(tài)有關的測量值進行處理，得出從某種統(tǒng)計意義上講估計誤差最小的更多的狀態(tài)的從某種統(tǒng)計意義上講估計誤差最小的更多的狀態(tài)的估計值。估計值。 估計誤差最小的標準稱為估計準則。估計誤差最小的標準稱為估計準則。 根據(jù)不同的估計準則和估計計算方法，有各種不同的最根據(jù)不同的估計準則和估計計算方法，有各種不同的最優(yōu)估計。優(yōu)估計。 卡爾曼濾波是一種遞推線性最小方差估計?？柭鼮V波是一種遞推線性最小方差估計。第2頁，共37頁。1

2、1.1 .1最小方差估計最小方差估計最小方差估計的估計準則是估計的均方誤差最小，即：最小方差估計的估計準則是估計的均方誤差最小，即： ( )( ) |minLTXXZE XX ZXX Z最小方差估計的誤差小于等于其他估計的均方誤差最小方差估計的誤差小于等于其他估計的均方誤差! !第3頁，共37頁。估計的均方誤差就是估計誤差的方差，即：估計的均方誤差就是估計誤差的方差，即： 0)(XEZXXE TTXEXXEXEXXE 最小方差估計具有無偏性質，即它的估計誤差（亦可最小方差估計具有無偏性質，即它的估計誤差（亦可用用 表示）的均值為零。即：表示）的均值為零。即：X 因此，最小方差估計不但使估值因此

3、，最小方差估計不但使估值 的均方誤差最小，的均方誤差最小，而且這種最小的均方誤差就是估計的誤差方差而且這種最小的均方誤差就是估計的誤差方差 )(ZX第4頁，共37頁。1.21.2線性最小方差估計線性最小方差估計 如果將估值如果將估值 規(guī)定為量測矢量規(guī)定為量測矢量Z Z的線性函數(shù)，即的線性函數(shù)，即X 使得下述指標滿足使得下述指標滿足式中式中A A和和b b分別是（分別是（n nm m）階和）階和n n維的矩陣和矢量。這維的矩陣和矢量。這 樣的估計樣的估計方法稱為方法稱為線性最小方差估計線性最小方差估計，有時用符號，有時用符號E E* *X/ZX/Z表示。表示。 LXZAZb ( )( ) |mi

4、nLTX XZE X X ZX X Z第5頁，共37頁。有關量測量有關量測量Z Z的線性函數(shù)有無窮多個，但能使的線性函數(shù)有無窮多個，但能使X X具有最小方差具有最小方差估計的線性函數(shù)只有一個，記為估計的線性函數(shù)只有一個，記為 00LXZA Zb 利用上述的指標我們可以得出利用上述的指標我們可以得出A0A0和和b0b0， 0101XZZXXZZZACCbmCCm第6頁，共37頁。 因此因此X X在在Z Z上的線性最小方差估計為上的線性最小方差估計為 1LXXZZZX ZmC CZ m線性最小方差估計的均方誤差為線性最小方差估計的均方誤差為1XXZZZXPCCCC第7頁，共37頁。1.2.11.2

5、.1線性最小方差估計線性最小方差估計的性質的性質性質性質1 1 無偏性無偏性 線性最小方差估計是線性最小方差估計是X X在在Z Z上的無偏估計，即上的無偏估計，即 0)(XEZXXE 性質性質2 2 線性線性1 1 線性最小方差估計是具有線性性質，即若線性最小方差估計是具有線性性質，即若X X的線性的線性最小方差估計為最小方差估計為 ，則，則 的線性最小的線性最小方差估計為方差估計為/EXZF Xe第8頁，共37頁。其中其中F F為確定性矩陣，為確定性矩陣，e e為確定性向量。為確定性向量。/E FX e ZFE X Ze性質性質3 3 線性線性2 2 若若Y Y與與Z Z不相關，則不相關，則

6、/ ,/E X Y ZE X YE X ZEX第9頁，共37頁。1.31.3遞推線性最小方差估計遞推線性最小方差估計卡爾曼濾波卡爾曼濾波 卡爾曼濾波的準則與線性最小方差估計相同卡爾曼濾波的準則與線性最小方差估計相同 估值同樣是量測值的線性函數(shù)估值同樣是量測值的線性函數(shù) 只要包括初始值在內的濾波器初值選擇正確，它的估值只要包括初始值在內的濾波器初值選擇正確，它的估值也是無偏的也是無偏的 )()()()(TTZXZXEZXXZXXEbAZX 0)(XEZXXE計算方法計算方法遞推形式遞推形式第10頁，共37頁。 在在k k時刻以前估值的基礎上，根據(jù)時刻以前估值的基礎上，根據(jù)k k時刻的量測值時刻的

7、量測值Z Zk k, ,遞推得遞推得到到k k時刻的狀態(tài)估計值時刻的狀態(tài)估計值 ：根據(jù)根據(jù)k-1k-1時刻以前時刻以前所有的量測值得到所有的量測值得到 1kXkZkX)(tXX X（k k）也可以說是綜合利用）也可以說是綜合利用k k時刻以前的所有量測值得到時刻以前的所有量測值得到 的的一次僅處理一個量測量一次僅處理一個量測量計算量大大減小計算量大大減小第11頁，共37頁。2 2 卡爾曼濾波方程卡爾曼濾波方程2.12.1離散系統(tǒng)的數(shù)學描述離散系統(tǒng)的數(shù)學描述 設離散化后的系統(tǒng)狀態(tài)方程和量測方程分別為：設離散化后的系統(tǒng)狀態(tài)方程和量測方程分別為：kkkkkkkkkkVXHZWXX1111,X Xk

8、k為為k k時刻的時刻的n n維狀態(tài)向量維狀態(tài)向量（被估計量）（被估計量）Z Zk k為為k k時刻的時刻的m m維量測向量維量測向量k-1k-1到到k k時刻的系統(tǒng)一步狀態(tài)轉時刻的系統(tǒng)一步狀態(tài)轉移矩陣（移矩陣（n nn n階）階）WWk-1k-1為為k-1k-1時刻的系統(tǒng)噪聲（時刻的系統(tǒng)噪聲（r r維）維） k-1k-1為系統(tǒng)噪聲矩陣為系統(tǒng)噪聲矩陣（n nr r階）階）H Hk k為為k k時刻系統(tǒng)量測矩陣時刻系統(tǒng)量測矩陣（m mn n階）階）V Vk k為為k k時刻時刻m m維量測噪聲維量測噪聲第12頁，共37頁。Q Qk k和和R Rk k分別稱為系統(tǒng)噪聲和量測噪聲的方差矩陣，在卡爾分

9、別稱為系統(tǒng)噪聲和量測噪聲的方差矩陣，在卡爾曼濾波中要求它們分別是已知值的非負定陣和正定陣；曼濾波中要求它們分別是已知值的非負定陣和正定陣； k jk j是是Kronecker Kronecker 函數(shù)，即：函數(shù)，即：)(1)(0jkjkkj 卡爾曼濾波要求卡爾曼濾波要求WWk k 和和VVk k 是互不相關的零均值的白噪聲是互不相關的零均值的白噪聲序列，有：序列，有：kjkTjkkjkTjkRVVEQWWE第13頁，共37頁。 Var Var 為對為對求方差的符號求方差的符號卡爾曼濾波要求卡爾曼濾波要求m mx0 x0和和C Cx0 x0為已知量，為已知量， 初始狀態(tài)的初始狀態(tài)的 一、二階統(tǒng)計

10、特性為：一、二階統(tǒng)計特性為：00 xmXE00 xCXVar且要求且要求X X0 0與與WWk k 和和VVk k 都不相關都不相關第14頁，共37頁。2 2.2 .2 離散卡爾曼濾波方程離散卡爾曼濾波方程 1/)(kkkkkPHKIP或 11,1/kkkkkXX 狀態(tài)一步預測方程狀態(tài)一步預測方程)(1/1/kkkkkkkkXHZKXX 狀態(tài)估值計算方程狀態(tài)估值計算方程11/1/)(kTkkkkTkkkkRHPHHPK 濾波增益方程濾波增益方程TkkkTkkkkkkkQPP1111,11,1/ 一步預測均方差方程一步預測均方差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/ 估計

11、均方差方程估計均方差方程第15頁，共37頁。2 2.2 .2 離散卡爾曼濾波方程離散卡爾曼濾波方程 11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/時間修正時間修正方程方程量測修正量測修正方程方程第16頁，共37頁。（1 1）狀態(tài)一步預測方程）狀態(tài)一步預測方程 各濾波方程的物理意義：各濾波方程的物理意義：1kXX Xk-1k-1的卡爾曼濾波估值的卡爾曼濾波估值1/kkX利用利用X Xk-1k-1計算得到的一步預測計算得到的

12、一步預測 第17頁，共37頁。 上式就是通過上式就是通過 計算新息，把計算新息，把 估計出來，并左估計出來，并左乘一個系數(shù)矩陣乘一個系數(shù)矩陣 加到加到 中，從而得到中，從而得到 估值估值 和，和， 稱為濾波增益矩陣稱為濾波增益矩陣1/ kkX1/kkXkXkKkK（2 2）狀態(tài)估值計算方程）狀態(tài)估值計算方程)(1/1/kkkkkkkkXHZKXXkkkkkkkkkkkkkkVXHXHVXHXHZ1/1/1/一步預測誤差一步預測誤差1/1/kkkkkXXX若把若把 看作是量測看作是量測 的一步預測，的一步預測，則則 就是量測的一步預測誤差就是量測的一步預測誤差1/ kkkXH)(1/ kkkkX

13、HZkZ由兩部分組成：由兩部分組成： 和和 ， 正是在正是在 基礎上估計基礎上估計 所需信息，因此所需信息，因此又稱又稱 為新息為新息1/ kkXkV1/kkX1/kkXkX)(1/ kkkkXHZ第18頁，共37頁。 由于由于 也具有無偏性，即也具有無偏性，即 的均值為零，所以的均值為零，所以 也稱為一步預測誤差方差陣。上式中的也稱為一步預測誤差方差陣。上式中的 和和 分別就是新息中的兩部分內容分別就是新息中的兩部分內容1/kkX1/kkX1/ kkPTkkkkHPH1/ kR（3 3）濾波增益方程）濾波增益方程一步預測均方差陣，即：一步預測均方差陣，即：/1/1/1,Tkkk kk kk

14、kZH PE XX11/1/)(kTkkkkTkkkkRHPHHPK K Kk k選取的標準就是卡爾曼濾波的估計準則，也就是使選取的標準就是卡爾曼濾波的估計準則，也就是使得得 均方誤差陣最?。壕秸`差陣最小：kX如果如果R Rk k大，大，K Kk k就小就小R Rk k小，小，K Kk k就大就大第19頁，共37頁。（4 4）一步預測均方誤差方程）一步預測均方誤差方程11/1/)(kTkkkkTkkkkRHPHHPK從下式可以看出，求從下式可以看出，求K Kk k必須先求出必須先求出P Pk/k-1k/k-1 式中式中 ，為，為 的估計誤差，可以看的估計誤差，可以看出一步預測均方誤差陣出一步

15、預測均方誤差陣P Pk/k-1k/k-1是從估計均方誤差陣是從估計均方誤差陣P Pk-1k-1轉移過來的，并且再加上系統(tǒng)噪聲方差的影響。轉移過來的，并且再加上系統(tǒng)噪聲方差的影響。111kkkXXX1kXTkkkTkkkkkkkQPP1111,11,1/ 的均方誤差陣，即：的均方誤差陣，即：TkkkXXEP111,1kX第20頁，共37頁。（5 5）估計均方誤差方程）估計均方誤差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/1/)(kkkkkPHKIP或 第21頁，共37頁。（6 6）卡爾曼濾波的計算流程）卡爾曼濾波的計算流程1/ kk11,1/kkkkkXX)(1/1/kkkk

16、kkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/1kX1kkkX1kPkP1kk1, kkTkkkQ111kRkHkRkH1/ kkPkKkZ第22頁，共37頁。2.32.3離散卡爾曼濾波基本方程使用要點離散卡爾曼濾波基本方程使用要點 在濾波開始時，必須有初始值在濾波開始時，必須有初始值 和和 才能進行才能進行0X0P為了保證估值的無偏性，應選擇：為了保證估值的無偏性，應選擇：000 xmXEXTxxTmXmXEXXXXEP)()(00000000000 xCXVa

17、r這樣才能保證估計均方差陣這樣才能保證估計均方差陣P Pk k始終最小。始終最小。1. 1.濾波初值的選取濾波初值的選取第23頁，共37頁。有上述的卡爾曼濾波基本方程中的均方誤差的公式有上述的卡爾曼濾波基本方程中的均方誤差的公式2. 2.估計均方誤差陣的等價形式及選用估計均方誤差陣的等價形式及選用/1()()TTkkkk kkkkkkPIK H PIK HK R K/1()kkkk kPIK HP 111/1Tkk kkkkPPH R H（a）（b）（c）第24頁，共37頁。由卡爾曼濾波方程的推導得知，基本方程只適用于系統(tǒng)方程由卡爾曼濾波方程的推導得知，基本方程只適用于系統(tǒng)方程和量測方程都是離

18、散型的情況。但實際的物理系統(tǒng)一般都是和量測方程都是離散型的情況。但實際的物理系統(tǒng)一般都是連續(xù)的，動力學特性用連續(xù)的微分方程來描述。所以在使用連續(xù)的，動力學特性用連續(xù)的微分方程來描述。所以在使用卡爾曼基本方程之前，必須對系統(tǒng)方程和量測方程進行離散卡爾曼基本方程之前，必須對系統(tǒng)方程和量測方程進行離散化處理?；幚怼?設描述物理系統(tǒng)動力特性的系統(tǒng)方程為設描述物理系統(tǒng)動力特性的系統(tǒng)方程為3. 3.一步轉移陣和等效離散系統(tǒng)噪聲方差陣的計算一步轉移陣和等效離散系統(tǒng)噪聲方差陣的計算( )( )( )( )( )X tF t X tG t W t其中系統(tǒng)驅動源其中系統(tǒng)驅動源WW（t t）為白噪聲過程，即）為白

19、噪聲過程，即第25頁，共37頁。 0TE w tE w t wqtq q為為w w（t t）的方差強度矩陣。）的方差強度矩陣。根據(jù)線性系統(tǒng)理論，系統(tǒng)方程的離散化形式為根據(jù)線性系統(tǒng)理論，系統(tǒng)方程的離散化形式為 1111( ),kktkkkkktXtt t X ttG w d 其中，其中， 滿足方程滿足方程1,kktt ,kkk kt tF tt tt tI第26頁，共37頁。23231,.2!3!kkkkkTTI TFFF 對該方程求解并進行約等變化，得對該方程求解并進行約等變化，得式中式中 T T為濾波周期。為濾波周期。 這個式子即為一步轉移矩陣的實時計算公式。這個式子即為一步轉移矩陣的實時計

20、算公式。 ,kkFF t同樣，通過系統(tǒng)的離散化處理，得出等效離散系統(tǒng)噪聲方差陣同樣，通過系統(tǒng)的離散化處理，得出等效離散系統(tǒng)噪聲方差陣1!ikiiTQMi第27頁，共37頁。一步預測方程改為：一步預測方程改為：,111111kk kkkkkkkkkkkXXWB UZH XVY 狀態(tài)估計方程改為：狀態(tài)估計方程改為：1111,1kkkkkUBXXkk)(11kkkkXHYZKXXkkkkk其他濾波方程不變其他濾波方程不變4. 4.系統(tǒng)有確定性控制時的濾波基本方程系統(tǒng)有確定性控制時的濾波基本方程設系統(tǒng)除了白噪聲外，還有確定性驅動項設系統(tǒng)除了白噪聲外，還有確定性驅動項第28頁，共37頁。5. 5.一步預測基本方程一步預測基本方程/1kk kZX和+1/kkX令令 一步預測基本方程式指利用一步預測基本方程式指利用 遞推計算遞推計算 的的 全套方程。全套方程。 根據(jù)基本方程的一步預測方程得根據(jù)基本方程的一步預測方程得 +1/k+1,kk+1,k/ 1/ 1=kkkk kkkkk kXXXK ZHXk+1,kkKK第29頁，共37頁。 則則+1/k+1,k/1/1=kkk kkk