離散卡爾曼濾波

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

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

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

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

5、X X在在Z Z上的無偏估計，即上的無偏估計，即 0)(XEZXXE 性質(zhì)性質(zhì)2 2 線性線性1 1 線性最小方差估計是具有線性性質(zhì)，即若線性最小方差估計是具有線性性質(zhì)，即若X X的線性的線性最小方差估計為最小方差估計為 ，則，則 的線性最小的線性最小方差估計為方差估計為/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遞推線性最小方差估計遞推線性最小方差估計卡爾曼濾波卡爾曼濾波 卡爾曼濾波

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

7、)(tXX X（k k）也可以說是綜合利用）也可以說是綜合利用k k時刻以前的所有量測值得到時刻以前的所有量測值得到 的的一次僅處理一個量測量一次僅處理一個量測量計算量大大減小計算量大大減小2 2 卡爾曼濾波方程卡爾曼濾波方程2.12.1離散系統(tǒng)的數(shù)學(xué)描述離散系統(tǒng)的數(shù)學(xué)描述 設(shè)離散化后的系統(tǒng)狀態(tài)方程和量測方程分別為：設(shè)離散化后的系統(tǒng)狀態(tài)方程和量測方程分別為：kkkkkkkkkkVXHZWXX1111,X Xk 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)一

8、步狀態(tài)轉(zhuǎn)移矩陣（轉(zhuǎn)移矩陣（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維量測噪聲維量測噪聲 Q Qk k和和R Rk k分別稱為系統(tǒng)噪聲和量測噪聲的方差矩陣，分別稱為系統(tǒng)噪聲和量測噪聲的方差矩陣，在卡爾曼濾波中要求它們分別是已知值的非負(fù)定陣和在卡爾曼濾波中要求它們分別是已知值的非負(fù)定陣和正定陣；正定陣； k jk j是是Kronecker Kronecker 函數(shù)，即：

9、函數(shù)，即：)(1)(0jkjkkj 卡爾曼濾波要求卡爾曼濾波要求WWk k 和和VVk k 是互不相關(guān)的零均值的是互不相關(guān)的零均值的白噪聲序列，有：白噪聲序列，有：kjkTjkkjkTjkRVVEQWWE Var Var 為對為對求方差的符號求方差的符號 卡爾曼濾波要求卡爾曼濾波要求m mx0 x0和和C Cx0 x0為已知量，為已知量， 初始狀態(tài)的初始狀態(tài)的 一、二階統(tǒng)計特性為：一、二階統(tǒng)計特性為：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ù)測方程狀態(tài)一步預(yù)測方程)(1/1/kkkkkkkkXHZKXX 狀態(tài)估值計算方程狀態(tài)估值計算方程11/1/)(kTkkkkTkkkkRHPHHPK 濾波增益方程濾波增益方程TkkkTkkkkkkkQPP1111,11,1/ 一步預(yù)測均方差方程一步預(yù)測均方差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/ 估計均方差方程估計均方差方程2.2 2.2 離散卡爾曼濾波方程離散卡爾曼濾波方程 11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/T

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

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

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

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

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

16、必須有初始值 和和 才能進(jìn)行才能進(jìn)行0X0P 為了保證估值的無偏性，應(yīng)選擇：為了保證估值的無偏性，應(yīng)選擇：000 xmXEXTxxTmXmXEXXXXEP)()(00000000000 xCXVar 這樣才能保證估計均方差陣這樣才能保證估計均方差陣P Pk k始終最小。始終最小。1. 1.濾波初值的選取濾波初值的選取 有上述的卡爾曼濾波基本方程中的均方誤差的公式有上述的卡爾曼濾波基本方程中的均方誤差的公式2. 2.估計均方誤差陣的等價形式及選用估計均方誤差陣的等價形式及選用/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)方程和量測方程都是離散型的情況。但實際的物理統(tǒng)方程和量測方程都是離散型的情況。但實際的物理系統(tǒng)一般都是連續(xù)的，動力學(xué)特性用連續(xù)的微分方程系統(tǒng)一般都是連續(xù)的，動力學(xué)特性用連續(xù)的微分方程來描述。所以在使用卡爾曼基本方程之前，必須對系來描述。所以在使用卡爾曼基本方程之前，必須對系統(tǒng)方程和量測方程進(jìn)行離散化處理。統(tǒng)方程和量測方程進(jìn)行離散化處理。 設(shè)描述物理系統(tǒng)動力特性的系統(tǒng)方程為設(shè)描述物理系統(tǒng)動力特性的系統(tǒng)方程為3. 3.一步轉(zhuǎn)移陣和等效離散系統(tǒng)噪聲方差陣的計算一步轉(zhuǎn)

18、移陣和等效離散系統(tǒng)噪聲方差陣的計算( )( )( )( )( )X tF t X tG t W t其中系統(tǒng)驅(qū)動源其中系統(tǒng)驅(qū)動源WW（t t）為白噪聲過程，即）為白噪聲過程，即 0TE w tE w t wqtq q為為w w（t t）的方差強度矩陣。）的方差強度矩陣。根據(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 對該方程求解并進(jìn)行約等變化，得對該方程求解并進(jìn)行約等變化

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