卡爾曼濾波原理

1、過程方程：X(k+1) =  A X(k) + B U(k) + W(k)               >>>>式1量測(cè)方程：Z(k+1) =  H X（k+1）+ V(k+1)             &

2、#160;    >>>>式2A和B是系統(tǒng)參數(shù)，對(duì)于多模型系統(tǒng)，他們?yōu)榫仃?；H是測(cè)量系統(tǒng)的參數(shù)，對(duì)于多測(cè)量系統(tǒng)，H為矩陣。W(k)和V(k)分別表示過程和測(cè)量的噪聲。他們被假設(shè)成高斯白噪聲，他們的協(xié)方差 分別是Q，R。為了不失一般性，下面的討論中將X,Z都視為矩陣，其中X是m行的單列矩陣，Z是n行的單列矩陣。 說明：下面的表達(dá)式中，不帶前綴的量都代表實(shí)際量，其小括號(hào)里面的“k”或“k+1”代表該量是第k或第k+1時(shí)刻的實(shí)際量，如“Z(k+1)”就代表第k+1時(shí)刻的實(shí)際測(cè)量值；帶前綴“”的量都代表預(yù)測(cè)量，如果小括號(hào)里面是“k+1|k

3、”，就代表k+1時(shí)刻的先驗(yàn)預(yù)測(cè)值，如果小括號(hào)里面是“k+1|k+1”，就代表k+1時(shí)刻的后驗(yàn)預(yù)測(cè)值；（測(cè)量值可以通過測(cè)量得到，所以只有先驗(yàn)預(yù)測(cè)，沒有后驗(yàn)預(yù)測(cè)。而實(shí)際狀態(tài)值無法得知，既有先驗(yàn)預(yù)測(cè)，又有后驗(yàn)預(yù)測(cè)）帶前綴“”的量都代表與預(yù)測(cè)值對(duì)應(yīng)的偏差值。 實(shí)際狀態(tài)值與先驗(yàn)預(yù)測(cè)狀態(tài)值的偏差 = 實(shí)際狀態(tài)值 先驗(yàn)預(yù)測(cè)狀態(tài)值X(k+1|k)      =     X(k+1)    -      X(k+1|k) &

4、#160;           >>>> 式3 實(shí)際測(cè)量值與先驗(yàn)預(yù)測(cè)測(cè)量值的偏差 = 當(dāng)前測(cè)量值 - 先驗(yàn)預(yù)測(cè)測(cè)量值Z(k+1|k)  = Z(k+1)  -   Z(k+1|k)                    

5、60;              >>>>式4  并且先驗(yàn)預(yù)測(cè)測(cè)量值  =  轉(zhuǎn)換矩陣H * 先驗(yàn)預(yù)測(cè)狀態(tài)值Z(k+1|k) =  H X(k+1|k)                  

6、;          >>>> 式5 得到測(cè)量值后，再對(duì)當(dāng)前狀態(tài)值X(k+1) 進(jìn)行后驗(yàn)預(yù)測(cè)（設(shè)后驗(yàn)預(yù)測(cè)值為 Z(k+1|k+1) ） ，則后驗(yàn)預(yù)測(cè)值（同時(shí)也是最終預(yù)測(cè)值）的偏差為X(k+1|k+1)  =     X(k+1)    -      X(k+1|k+1)       &

7、#160;         >>>>式6  為了得到當(dāng)前狀態(tài)值X(k+1)， 根據(jù)式3，需要：X(k+1) =  X(k+1|k)  + X(k+1|k)                        >>

8、;>> 式7上式中，我們可以通過卡爾曼公式1（見附注2）計(jì)算出X(k+1|k)，但我們無法得知實(shí)際狀態(tài)值X(k+1)，因而X(k+1|k) 也無法得知。我們最終的目的是得出一個(gè)比較接近實(shí)際狀態(tài)值X(k+1)的濾波值X(k+1|k+1)，根據(jù)式7，只要能準(zhǔn)確的估計(jì)出X(k+1|k)即可。X(k+1|k)本身雖無法得知，但Z(k+1|k) 卻可以通過測(cè)量得到，而且它們二者存在一定的相關(guān)性。不妨再設(shè)存在一個(gè)矩陣K（m行n列矩陣），能使得 X(k+1|k) = K * Z(k+1|k)         

9、;                               >>>>式8那么最終的預(yù)測(cè)任務(wù)其實(shí)就是找到K。由于X(k+1|k)和Z(k+1|k)都是單列矩陣，因此不難看出，滿足式8的矩陣K應(yīng)有無窮多個(gè)。矩陣K中第i行第j列反映了量測(cè)變量偏差矩陣Z(k+1|k)的第j個(gè)元素

10、對(duì)狀態(tài)變量偏差矩陣X(k+1|k)的第i個(gè)元素的貢獻(xiàn)。因此矩陣K的物理意義很明顯，K的第i行第j列的元素表示：對(duì)于第i個(gè)待測(cè)的狀態(tài)量來說，第j個(gè)測(cè)量?jī)x器測(cè)到的偏差的可信度。某個(gè)測(cè)量值對(duì)應(yīng)的可信度越高，濾波器越“相信”該測(cè)量值。 既然滿足條件的K有無窮多個(gè)，那應(yīng)該使用哪個(gè)K呢？實(shí)際上，我們并不知道X(k+1|k)的值，所以也就無法直接計(jì)算出K，而只能通過某種方法找到一個(gè)Kg，使得將Kg帶入式8后，等號(hào)兩邊的差（的平方）的期望盡可能小。我們最終的預(yù)測(cè)值或?yàn)V波值是后驗(yàn)預(yù)測(cè)值X(k+1|k+1)，因此最后的預(yù)測(cè)也應(yīng)使 X(k+1|k+1) 的期望為0且方差最小（這與讓8式兩端的差最小是一致的

11、，下面的式9體現(xiàn)了這一點(diǎn)），這樣預(yù)測(cè)值才最可靠。下面詳細(xì)說明。 X(k+1|k+1) =  X(k+1|k) + Kg * Z(k+1|k)        （后驗(yàn)預(yù)測(cè)的狀態(tài)值）X(k+1|k+1)  =     X(k+1)    -      X(k+1|k+1)    （后驗(yàn)預(yù)測(cè)的偏差） X(k+1|k+1)  =

12、                   X(k+1)                         -      

13、60;      X(k+1|k+1)                       =     ( X(k+1|k)  +  X(k+1|k) ) -      (  X(k+1|k) +&

14、#160;Kg * Z(k+1|k)  )                     =                   X(k+1|k)      

15、;              -             Kg * Z(k+1|k)                      &

16、#160;                                                 &

17、#160;                                >>>>式9 Z(k+1|k)       =      &#

18、160;            Z(k+1)                         -             Z(k+

19、1|k)                     =     (  H X（k+1）+ V(k+1)  )              -    

20、60; (  H X(k+1|k)  )                     =     H (  X（k+1）-X(k+1|k)  )  + V(k+1)        &#

21、160;            =     H  X(k+1|k)  + V(k+1)                           

22、          >>>>式10 接下來的分析中，為了更直觀的說明卡爾曼濾波的原理，我們用幾何方法來解釋。這時(shí)，X和Z矩陣中的每個(gè)元素應(yīng)看做向量空間中的一個(gè)向量而不再是一個(gè)單純的數(shù)。這個(gè)向量空間（統(tǒng)計(jì)測(cè)試空間）可以看成無窮多維的，每一個(gè)維對(duì)應(yīng)一個(gè)可能的狀態(tài)。X和Z矩陣中的每個(gè)元素向量都是由所有可能的狀態(tài)按照各自出現(xiàn)的概率組合而成（在測(cè)量之前，X和Z 的實(shí)際值都是不可知的）。X和Z中的每個(gè)元素向量都應(yīng)是0均值的，他們與自己的內(nèi)積就是他們的協(xié)方差矩陣。我們無法給出X和Z中每

23、個(gè)元素向量的具體表達(dá)，但我們通過協(xié)方差矩陣就可以知道所有元素向量的模長(zhǎng)，以及相互之間的夾角（從內(nèi)積計(jì)算）。為了方便用幾何方法解釋，我們假設(shè)狀態(tài)變量X是一個(gè)1行1列的矩陣（即只有一個(gè)待測(cè)狀態(tài)量），而量測(cè)變量Z是一個(gè)2行1列的矩陣（即有兩個(gè)測(cè)量?jī)x器，共同測(cè)量同一個(gè)狀態(tài)量X），也就是說，m=1，n=2。矩陣X中只有X1一項(xiàng)，矩陣Z中有Z1和Z2兩項(xiàng)。Kg此時(shí)應(yīng)是一個(gè)1行2列的矩陣，兩個(gè)元素分別記作Kg1 和 Kg2 。H和V此時(shí)應(yīng)是一個(gè)2行1列的矩陣。將矩陣表達(dá)式9和10按元素展開：X(k+1|k+1)1    =     X(k+1|k)

24、1              -      (Kg1 * Z(k+1|k)1 + Kg2 * Z(k+1|k)2 )                          &

25、#160;                           >>>> 式9iZ(k+1|k)i   =     Hi  X(k+1|k)     +     V(

26、k+1)i                                   >>>>式10i X(k+1|k) 中各個(gè)元素（向量）的線性組合可以產(chǎn)生一個(gè)m維或更低維的向量子空間Vx，這里，按照我們的假設(shè)，m=1，所以Vx應(yīng)

27、是一維的； 同時(shí)V(k+1)中的各個(gè)元素（向量）的線性組合也可以產(chǎn)生一個(gè)n維或更低維的向量子空間Vv，這里，按照我們的假設(shè)，n=2，所以Vv應(yīng)是二維的。由于V(k+1)中的每一項(xiàng)與X(k+1|k)中的每一項(xiàng)都不相關(guān)（見附注1），故這兩個(gè)子空間相互垂直。如下圖所示。式10i所體現(xiàn)的Z(k+1|k)i、Hi X(k+1|k)、V(k+1)i  三者之間的幾何關(guān)系，也在下圖中描繪了出來。     從上圖中可以看出，Z(k+1|k)中各個(gè)元素（向量）的線性組合也可以產(chǎn)生一個(gè)n維或更低維的向量子空間Vz，這里已假設(shè)n=2，所以Vz是一個(gè)二維的平

28、面，就是上圖中兩條紅色的線所構(gòu)成的平面。   圖2中（注意此圖中的橢圓代表的是Vz空間，而圖1中則代表Vv空間，二者不一樣），粉色的向量就是Kg1 * Z(k+1|k)1 + Kg2 * Z(k+1|k)2 ， 記此粉色向量為 Y ，Y為Z(k+1|k)1和Z(k+1|k)2線性組合而成，它始終在子空間Vz中。根據(jù)式9i，X(k+1|k+1)1 等于X(k+1|k)1和Y的差向量，為使X(k+1|k+1)1長(zhǎng)度最短（協(xié)方差最?。琄g的選取應(yīng)使得X(k+1|k+1)1垂直于Vz空間。通過先驗(yàn)預(yù)測(cè)的協(xié)方差矩陣（見卡爾曼公式2），可以得到X(k+1

29、|k)中各個(gè)元素的模長(zhǎng)以及彼此間的夾角。這是因?yàn)閰f(xié)方差矩陣中的第i行第j列其實(shí)就代表了X(k+1|k)中第i個(gè)元素向量與第j個(gè)元素向量的內(nèi)積。通過測(cè)量可以得到新息協(xié)方差（見卡爾曼公式3的分母部分），進(jìn)而可以知道Z(k+1|k)中各個(gè)元素的模長(zhǎng)以及彼此間的夾角。通過已知的量測(cè)噪聲協(xié)方差矩陣R，可以得出V(k+1) 中各個(gè)元素的模長(zhǎng)以及彼此間的夾角。最后根據(jù)X(k+1|k+1)1與Y垂直以及圖1中所示的幾何關(guān)系，用高中學(xué)的立體幾何和向量知識(shí)就可以求得兩個(gè)Kg的值了。如果將向量的內(nèi)積都用協(xié)方差矩陣表示，就會(huì)發(fā)現(xiàn)，我們最后求得的Kg，其實(shí)就是卡爾曼公式3。 （上面討論的是較低次的卡爾曼濾波，

30、只有一個(gè)待測(cè)量，兩個(gè)測(cè)量?jī)x器。這種情況還是比較常見的，比如傾角測(cè)量系統(tǒng)中，我們用加速度計(jì)和陀螺儀共同測(cè)量?jī)A角。對(duì)于更高次的卡爾曼濾波，X和Z都是多行矩陣時(shí)，用幾何方法已經(jīng)無法直觀解釋，只能用矩陣分析的方法證明。求解Kg的詳細(xì)過程參考 卡爾曼濾波器及其應(yīng)用基礎(chǔ)國防工業(yè)出版社敬喜 編 ） 卡爾曼濾波的核心過程，就是求解能使得E  X(k+1|k+1)  *  X(k+1|k+1) 取最小值的Kg增益矩陣的過程，X(k+1|k+1)代表的是X(k+1|k+1)的轉(zhuǎn)置（這里X(k+1|k+1)中的元素代表數(shù)值，不是向量）。前面已經(jīng)提到過，卡爾

31、曼增益矩陣Kg中的元素，都代表測(cè)量?jī)x器測(cè)到的偏差的可信度，或者叫估計(jì)權(quán)重。       附注1： (a).   v(k+1)中的每一項(xiàng)與X(k+1|k)中的每一項(xiàng)都不相關(guān) X(k+1|k)  =            X(k+1)        -     &

32、#160;   X(k+1|k)          =           X(k+1)         -  ( A X(k|k)  +  B U(k)  )   &#

33、160;      = ( A X(k) + B U(k) + W(k) )   -   (  A X(k)  +  B U(k)  -  A X(k|k) )         =      

34、;   W(k)   +   A X(k|k)         =         W(k)   +   A (  X(k|k-1)  -  Kg(k)* Z(k|k-1)  )  -這一步利用了式9   &#

35、160;            =                W(k)   +   A (  X(k|k-1)  -  Kg(k)* ( H X(k|k-1) + v(k) )  )   

36、0;         -這一步利用了式10                =                W(k)   -  A Kg(k) *v(k)  + 

37、; A ( I - Kg(k) * H )  X(k|k-1)上式最后一行出現(xiàn)了X(k|k-1)，可見X(k+1|k)可以遞歸表示。而且遞歸式中的過程噪聲W(k)與v(k+1)不相關(guān)，同時(shí)由于 v本身是白噪聲，所以 v(k+1)與v(k)亦不相關(guān)（白噪聲的自相關(guān)是函數(shù)），因此通過遞推式可以判斷v(k+1)與X(k+1|k)不相關(guān)。  (b).  w(k+1)中的每一項(xiàng)與X(k+1|k+1)中的每一項(xiàng)都不相關(guān)，w(k+1)中的每一項(xiàng)與X(k+1|k)中的每一項(xiàng)都不相關(guān)。X(k+1|k+1)

38、0; =            X(k+1)        -         X(k+1|k+1)          = (  X(k+1|k) + X(k+1|k)  )   - 

39、60; ( X(k+1|k) + Kg(k+1)*Z(k+1|k) )           =             X(k+1|k)       -       Kg(k+1)*Z(k+1|k)          =             X(k+1|k)       -     Kg(k+1)* (  H X(k+1|k) + v(k+1)   )       &