離散卡爾曼濾波

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

2、最小方差估計(jì)的估計(jì)準(zhǔn)則是估計(jì)的均方誤差最小，即：最小方差估計(jì)的估計(jì)準(zhǔn)則是估計(jì)的均方誤差最小，即： ( )( ) |minLTXXZE XX ZXX Z 最小方差估計(jì)的誤差小于等于其他估計(jì)的均方誤差最小方差估計(jì)的誤差小于等于其他估計(jì)的均方誤差! ! 估計(jì)的均方誤差就是估計(jì)誤差的方差，即：估計(jì)的均方誤差就是估計(jì)誤差的方差，即： 0)(XEZXXE TTXEXXEXEXXE 最小方差估計(jì)具有無(wú)偏性質(zhì)，即它的估計(jì)誤差（亦可最小方差估計(jì)具有無(wú)偏性質(zhì)，即它的估計(jì)誤差（亦可用用 表示）的均值為零。即：表示）的均值為零。即：X 因此，最小方差估計(jì)不但使估值因此，最小方差估計(jì)不但使估值 的均方誤差最的均方誤差最

3、小，而且這種最小的均方誤差就是估計(jì)的誤差方差小，而且這種最小的均方誤差就是估計(jì)的誤差方差 )(ZX1.21.2線性最小方差估計(jì)線性最小方差估計(jì) 如果將估值如果將估值 規(guī)定為量測(cè)矢量規(guī)定為量測(cè)矢量Z Z的線性函數(shù)，即的線性函數(shù)，即X 使得下述指標(biāo)滿足使得下述指標(biāo)滿足 式中式中A A和和b b分別是（分別是（n nm m）階和）階和n n維的矩陣和矢量。這維的矩陣和矢量。這 樣的估計(jì)方法稱為樣的估計(jì)方法稱為線性最小方差估計(jì)線性最小方差估計(jì)，有時(shí)用符號(hào)，有時(shí)用符號(hào)E E* *X/ZX/Z表示。表示。 LXZAZb ( )( ) |minLTX XZE X X ZX X Z 有關(guān)量測(cè)量有關(guān)量測(cè)量Z Z

4、的線性函數(shù)有無(wú)窮多個(gè)，但能使的線性函數(shù)有無(wú)窮多個(gè)，但能使X X具有具有最小方差估計(jì)的線性函數(shù)只有一個(gè)，記為最小方差估計(jì)的線性函數(shù)只有一個(gè)，記為 00LXZA Zb 利用上述的指標(biāo)我們可以得出利用上述的指標(biāo)我們可以得出A0A0和和b0b0， 0101XZZXXZZZACCbmCCm 因此因此X X在在Z Z上的線性最小方差估計(jì)為上的線性最小方差估計(jì)為 1LXXZZZX ZmC CZ m 線性最小方差估計(jì)的均方誤差為線性最小方差估計(jì)的均方誤差為1XXZZZXPCCCC1.2.11.2.1線性最小方差估計(jì)的性質(zhì)線性最小方差估計(jì)的性質(zhì) 性質(zhì)性質(zhì)1 1 無(wú)偏性無(wú)偏性 線性最小方差估計(jì)是線性最小方差估計(jì)是

5、X X在在Z Z上的無(wú)偏估計(jì)，即上的無(wú)偏估計(jì)，即 0)(XEZXXE 性質(zhì)性質(zhì)2 2 線性線性1 1 線性最小方差估計(jì)是具有線性性質(zhì)，即若線性最小方差估計(jì)是具有線性性質(zhì)，即若X X的線性的線性最小方差估計(jì)為最小方差估計(jì)為 ，則，則 的線性最小的線性最小方差估計(jì)為方差估計(jì)為/EXZF Xe其中其中F F為確定性矩陣，為確定性矩陣，e e為確定性向量。為確定性向量。/E FX e ZFE X Ze 性質(zhì)性質(zhì)3 3 線性線性2 2 若若Y Y與與Z Z不相關(guān)，則不相關(guān)，則/ ,/E X Y ZE X YE X ZEX1.31.3遞推線性最小方差估計(jì)遞推線性最小方差估計(jì)卡爾曼濾波卡爾曼濾波 卡爾曼濾波

6、的準(zhǔn)則與線性最小方差估計(jì)相同卡爾曼濾波的準(zhǔn)則與線性最小方差估計(jì)相同 估值同樣是量測(cè)值的線性函數(shù)估值同樣是量測(cè)值的線性函數(shù) 只要包括初始值在內(nèi)的濾波器初值選擇正確，它的估只要包括初始值在內(nèi)的濾波器初值選擇正確，它的估值也是無(wú)偏的值也是無(wú)偏的 )()()()(TTZXZXEZXXZXXEbAZX 0)(XEZXXE 計(jì)算方法計(jì)算方法遞推形式遞推形式 在在k k時(shí)刻以前估值的基礎(chǔ)上，根據(jù)時(shí)刻以前估值的基礎(chǔ)上，根據(jù)k k時(shí)刻的量測(cè)值時(shí)刻的量測(cè)值Z Zk k, ,遞推得到遞推得到k k時(shí)刻的狀態(tài)估計(jì)值時(shí)刻的狀態(tài)估計(jì)值 ：根據(jù)根據(jù)k-1k-1時(shí)刻以前時(shí)刻以前所有的量測(cè)值得到所有的量測(cè)值得到 1kXkZkX

7、)(tXX X（k k）也可以說(shuō)是綜合利用）也可以說(shuō)是綜合利用k k時(shí)刻以前的所有量測(cè)值得到時(shí)刻以前的所有量測(cè)值得到 的的一次僅處理一個(gè)量測(cè)量一次僅處理一個(gè)量測(cè)量計(jì)算量大大減小計(jì)算量大大減小2 2 卡爾曼濾波方程卡爾曼濾波方程2.12.1離散系統(tǒng)的數(shù)學(xué)描述離散系統(tǒng)的數(shù)學(xué)描述 設(shè)離散化后的系統(tǒng)狀態(tài)方程和量測(cè)方程分別為：設(shè)離散化后的系統(tǒng)狀態(tài)方程和量測(cè)方程分別為：kkkkkkkkkkVXHZWXX1111,X Xk k為為k k時(shí)刻的時(shí)刻的n n維狀態(tài)向量維狀態(tài)向量（被估計(jì)量）（被估計(jì)量）Z Zk k為為k k時(shí)刻的時(shí)刻的m m維量測(cè)向量維量測(cè)向量k-1k-1到到k k時(shí)刻的系統(tǒng)一步狀態(tài)時(shí)刻的系統(tǒng)一

8、步狀態(tài)轉(zhuǎn)移矩陣（轉(zhuǎn)移矩陣（n nn n階）階）WWk-1k-1為為k-1k-1時(shí)刻的系統(tǒng)噪聲時(shí)刻的系統(tǒng)噪聲（r r維）維） k-1k-1為系統(tǒng)噪聲矩陣為系統(tǒng)噪聲矩陣（n nr r階）階）H Hk k為為k k時(shí)刻系統(tǒng)量測(cè)矩陣時(shí)刻系統(tǒng)量測(cè)矩陣（m mn n階）階）V Vk k為為k k時(shí)刻時(shí)刻m m維量測(cè)噪聲維量測(cè)噪聲 Q Qk k和和R Rk k分別稱為系統(tǒng)噪聲和量測(cè)噪聲的方差矩陣，分別稱為系統(tǒng)噪聲和量測(cè)噪聲的方差矩陣，在卡爾曼濾波中要求它們分別是已知值的非負(fù)定陣和在卡爾曼濾波中要求它們分別是已知值的非負(fù)定陣和正定陣；正定陣； k jk j是是Kronecker Kronecker 函數(shù)，即：

9、函數(shù)，即：)(1)(0jkjkkj 卡爾曼濾波要求卡爾曼濾波要求WWk k 和和VVk k 是互不相關(guān)的零均值的是互不相關(guān)的零均值的白噪聲序列，有：白噪聲序列，有：kjkTjkkjkTjkRVVEQWWE Var Var 為對(duì)為對(duì)求方差的符號(hào)求方差的符號(hào) 卡爾曼濾波要求卡爾曼濾波要求m mx0 x0和和C Cx0 x0為已知量，為已知量， 初始狀態(tài)的初始狀態(tài)的 一、二階統(tǒng)計(jì)特性為：一、二階統(tǒng)計(jì)特性為：00 xmXE00 xCXVar 且要求且要求X X0 0與與WWk k 和和VVk k 都不相關(guān)都不相關(guān)2.2 2.2 離散卡爾曼濾波方程離散卡爾曼濾波方程 1/)(kkkkkPHKIP或 11

10、,1/kkkkkXX 狀態(tài)一步預(yù)測(cè)方程狀態(tài)一步預(yù)測(cè)方程)(1/1/kkkkkkkkXHZKXX 狀態(tài)估值計(jì)算方程狀態(tài)估值計(jì)算方程11/1/)(kTkkkkTkkkkRHPHHPK 濾波增益方程濾波增益方程TkkkTkkkkkkkQPP1111,11,1/ 一步預(yù)測(cè)均方差方程一步預(yù)測(cè)均方差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/ 估計(jì)均方差方程估計(jì)均方差方程2.2 2.2 離散卡爾曼濾波方程離散卡爾曼濾波方程 11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/T

11、kkkTkkkkkkkKRKHKIPHKIP)()(1/時(shí)間修正時(shí)間修正方程方程量測(cè)修正量測(cè)修正方程方程（1 1）狀態(tài)一步預(yù)測(cè)方程）狀態(tài)一步預(yù)測(cè)方程 各濾波方程的物理意義：各濾波方程的物理意義：1kXX Xk-1k-1的卡爾曼濾波估值的卡爾曼濾波估值1/kkX利用利用X Xk-1k-1計(jì)算得到的一步預(yù)測(cè)計(jì)算得到的一步預(yù)測(cè) 上式就是通過(guò)上式就是通過(guò) 計(jì)算新息，把計(jì)算新息，把 估計(jì)出來(lái)，并估計(jì)出來(lái)，并左乘一個(gè)系數(shù)矩陣左乘一個(gè)系數(shù)矩陣 加到加到 中，從而得到中，從而得到 估值估值 和，和， 稱為濾波增益矩陣稱為濾波增益矩陣1/ kkX1/kkXkXkKkK（2 2）狀態(tài)估值計(jì)算方程）狀態(tài)估值計(jì)算方程

12、)(1/1/kkkkkkkkXHZKXXkkkkkkkkkkkkkkVXHXHVXHXHZ1/1/1/一步預(yù)測(cè)誤差一步預(yù)測(cè)誤差1/1/kkkkkXXX若把若把 看作是量測(cè)看作是量測(cè) 的一步預(yù)測(cè)，的一步預(yù)測(cè)，則則 就是量測(cè)的一步預(yù)測(cè)誤差就是量測(cè)的一步預(yù)測(cè)誤差1/ kkkXH)(1/ kkkkXHZkZ由兩部分組成：由兩部分組成： 和和 ， 正是在正是在 基礎(chǔ)上估計(jì)基礎(chǔ)上估計(jì) 所需信息，因此所需信息，因此又稱又稱 為新息為新息1/ kkXkV1/kkX1/kkXkX)(1/ kkkkXHZ 由于由于 也具有無(wú)偏性，即也具有無(wú)偏性，即 的均值為零，所的均值為零，所以以 也稱為一步預(yù)測(cè)誤差方差陣。上式

13、中的也稱為一步預(yù)測(cè)誤差方差陣。上式中的 和和 分別就是新息中的兩部分內(nèi)容分別就是新息中的兩部分內(nèi)容1/kkX1/kkX1/ kkPTkkkkHPH1/ kR（3 3）濾波增益方程）濾波增益方程一步預(yù)測(cè)均方差陣，即：一步預(yù)測(cè)均方差陣，即：/1/1/1,Tkkk kk kk kZH PE XX11/1/)(kTkkkkTkkkkRHPHHPK K Kk k選取的標(biāo)準(zhǔn)就是卡爾曼濾波的估計(jì)準(zhǔn)則，也就是選取的標(biāo)準(zhǔn)就是卡爾曼濾波的估計(jì)準(zhǔn)則，也就是使得使得 均方誤差陣最?。壕秸`差陣最小：kX如果如果R Rk k大，大，K Kk k就小就小R Rk k小，小，K Kk k就大就大（4 4）一步預(yù)測(cè)均方誤差方

14、程）一步預(yù)測(cè)均方誤差方程11/1/)(kTkkkkTkkkkRHPHHPK 從下式可以看出，求從下式可以看出，求K Kk k必須先求出必須先求出P Pk/k-1k/k-1 式中式中 ，為，為 的估計(jì)誤差，可以的估計(jì)誤差，可以看出一步預(yù)測(cè)均方誤差陣看出一步預(yù)測(cè)均方誤差陣P Pk/k-1k/k-1是從估計(jì)均方誤差陣是從估計(jì)均方誤差陣P Pk-1k-1轉(zhuǎn)移過(guò)來(lái)的，并且再加上系統(tǒng)噪聲方差的影響。轉(zhuǎn)移過(guò)來(lái)的，并且再加上系統(tǒng)噪聲方差的影響。111kkkXXX1kXTkkkTkkkkkkkQPP1111,11,1/ 的均方誤差陣，即：的均方誤差陣，即：TkkkXXEP111,1kX（5 5）估計(jì)均方誤差方程

15、）估計(jì)均方誤差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/1/)(kkkkkPHKIP或 （6 6）卡爾曼濾波的計(jì)算流程）卡爾曼濾波的計(jì)算流程1/ kk11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/1kX1kkkX1kPkP1kk1, kkTkkkQ111kRkHkRkH1/ kkPkKkZ2.32.3離散卡爾曼濾波基本方程使用要點(diǎn)離散卡爾曼濾波基本方程使用要點(diǎn) 在濾波開(kāi)始時(shí)，必須有初始值在濾波開(kāi)始時(shí)，

16、必須有初始值 和和 才能進(jìn)行才能進(jìn)行0X0P 為了保證估值的無(wú)偏性，應(yīng)選擇：為了保證估值的無(wú)偏性，應(yīng)選擇：000 xmXEXTxxTmXmXEXXXXEP)()(00000000000 xCXVar 這樣才能保證估計(jì)均方差陣這樣才能保證估計(jì)均方差陣P Pk k始終最小。始終最小。1. 1.濾波初值的選取濾波初值的選取 有上述的卡爾曼濾波基本方程中的均方誤差的公式有上述的卡爾曼濾波基本方程中的均方誤差的公式2. 2.估計(jì)均方誤差陣的等價(jià)形式及選用估計(jì)均方誤差陣的等價(jià)形式及選用/1()()TTkkkk kkkkkkPIK H PIK HK R K/1()kkkk kPIK HP 111/1Tkk

17、kkkkPPH R H（a）（b）（c） 由卡爾曼濾波方程的推導(dǎo)得知，基本方程只適用于系由卡爾曼濾波方程的推導(dǎo)得知，基本方程只適用于系統(tǒng)方程和量測(cè)方程都是離散型的情況。但實(shí)際的物理統(tǒng)方程和量測(cè)方程都是離散型的情況。但實(shí)際的物理系統(tǒng)一般都是連續(xù)的，動(dòng)力學(xué)特性用連續(xù)的微分方程系統(tǒng)一般都是連續(xù)的，動(dòng)力學(xué)特性用連續(xù)的微分方程來(lái)描述。所以在使用卡爾曼基本方程之前，必須對(duì)系來(lái)描述。所以在使用卡爾曼基本方程之前，必須對(duì)系統(tǒng)方程和量測(cè)方程進(jìn)行離散化處理。統(tǒng)方程和量測(cè)方程進(jìn)行離散化處理。 設(shè)描述物理系統(tǒng)動(dòng)力特性的系統(tǒng)方程為設(shè)描述物理系統(tǒng)動(dòng)力特性的系統(tǒng)方程為3. 3.一步轉(zhuǎn)移陣和等效離散系統(tǒng)噪聲方差陣的計(jì)算一步轉(zhuǎn)

18、移陣和等效離散系統(tǒng)噪聲方差陣的計(jì)算( )( )( )( )( )X tF t X tG t W t其中系統(tǒng)驅(qū)動(dòng)源其中系統(tǒng)驅(qū)動(dòng)源WW（t t）為白噪聲過(guò)程，即）為白噪聲過(guò)程，即 0TE w tE w t wqtq q為為w w（t t）的方差強(qiáng)度矩陣。）的方差強(qiáng)度矩陣。根據(jù)線性系統(tǒng)理論，系統(tǒng)方程的離散化形式為根據(jù)線性系統(tǒng)理論，系統(tǒng)方程的離散化形式為 1111( ),kktkkkkktXttt X ttGw d 其中，其中， 滿足方程滿足方程1,kktt ,kkk kt tF tt tt tI23231,.2!3!kkkkkTTI TFFF 對(duì)該方程求解并進(jìn)行約等變化，得對(duì)該方程求解并進(jìn)行約等變化

19、，得式中式中 T T為濾波周期。為濾波周期。 這個(gè)式子即為一步轉(zhuǎn)移矩陣的實(shí)時(shí)計(jì)算公式。這個(gè)式子即為一步轉(zhuǎn)移矩陣的實(shí)時(shí)計(jì)算公式。 ,kkFF t同樣，通過(guò)系統(tǒng)的離散化處理，得出等效離散系統(tǒng)噪聲同樣，通過(guò)系統(tǒng)的離散化處理，得出等效離散系統(tǒng)噪聲方差陣方差陣1!ikiiTQMi 一步預(yù)測(cè)方程改為：一步預(yù)測(cè)方程改為：,111111kk kkkkkkkkkkkXXWB UZH XVY 狀態(tài)估計(jì)方程改為：狀態(tài)估計(jì)方程改為：1111,1kkkkkUBXXkk)(11kkkkXHYZKXXkkkkk 其他濾波方程不變其他濾波方程不變4. 4.系統(tǒng)有確定性控制時(shí)的濾波基本方程系統(tǒng)有確定性控制時(shí)的濾波基本方程 設(shè)系統(tǒng)除了白噪聲外，還有確定性驅(qū)動(dòng)項(xiàng)設(shè)系統(tǒng)除了白噪聲外，還有確定性驅(qū)動(dòng)項(xiàng)5. 5.一步預(yù)測(cè)基本方程一步預(yù)測(cè)基本方程/1kk kZX和+1/kkX令令 一步預(yù)測(cè)基本方程式指利用一步預(yù)測(cè)基本方程式指利用 遞推計(jì)算遞推計(jì)算 的的 全套方程。全套方程。 根據(jù)基本方程的一步預(yù)測(cè)方程得根據(jù)基本方程的一步預(yù)測(cè)方程得 +1/k+1,kk+1,k/ 1/ 1=kkkk