卡爾曼濾波原理

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

2、量，如果小括號(hào)里面是“k+1|k”，就代表k+1時(shí)刻的先驗(yàn)預(yù)測(cè)值，如果小括號(hào)里面是“k+1|k+1”，就代表k+1時(shí)刻的后驗(yàn)預(yù)測(cè)值；（測(cè)量值可以通過(guò)測(cè)量得到，所以只有先驗(yàn)預(yù)測(cè)，沒(méi)有后驗(yàn)預(yù)測(cè)。而實(shí)際狀態(tài)值無(wú)法得知，既有先驗(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) 式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) 式4并且先驗(yàn)預(yù)測(cè)測(cè)量值 = 轉(zhuǎn)換矩陣H* 先驗(yàn)預(yù)測(cè)狀態(tài)值Z(k+1|

3、k)=HX(k+1|k) 式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) 式6為了得到當(dāng)前狀態(tài)值X(k+1)， 根據(jù)式3，需要：X(k+1)= X(k+1|k) +X(k+1|k) 式7上式中，我們可以通過(guò)卡爾曼公式1（見(jiàn)附注2）計(jì)算出X(k+1|k)，但我們無(wú)法得知實(shí)際狀態(tài)值X(k+1)，因而X(k+1|k) 也無(wú)法得知。我們最終的目的是得出一個(gè)比較接近實(shí)際狀態(tài)值X(k+1)的濾波值X(k+1|k+1)，根據(jù)式7，只要能準(zhǔn)確的估計(jì)

4、出X(k+1|k)即可。X(k+1|k)本身雖無(wú)法得知，但Z(k+1|k) 卻可以通過(guò)測(cè)量得到，而且它們二者存在一定的相關(guān)性。不妨再設(shè)存在一個(gè)矩陣K（m行n列矩陣），能使得X(k+1|k) = K * Z(k+1|k) 式8那么最終的預(yù)測(cè)任務(wù)其實(shí)就是找到K。由于X(k+1|k)和Z(k+1|k)都是單列矩陣，因此不難看出，滿(mǎn)足式8的矩陣K應(yīng)有無(wú)窮多個(gè)。矩陣K中第i行第j列反映了量測(cè)變量偏差矩陣Z(k+1|k)的第j個(gè)元素對(duì)狀態(tài)變量偏差矩陣X(k+1|k)的第i個(gè)元素的貢獻(xiàn)。因此矩陣K的物理意義很明顯，K的第i行第j列的元素表示：對(duì)于第i個(gè)待測(cè)的狀態(tài)量來(lái)說(shuō)，第j個(gè)測(cè)量?jī)x器測(cè)到的偏差的可信度。某個(gè)

5、測(cè)量值對(duì)應(yīng)的可信度越高，濾波器越“相信”該測(cè)量值。既然滿(mǎn)足條件的K有無(wú)窮多個(gè)，那應(yīng)該使用哪個(gè)K呢？實(shí)際上，我們并不知道X(k+1|k)的值，所以也就無(wú)法直接計(jì)算出K，而只能通過(guò)某種方法找到一個(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式兩端的差最小是一致的，下面的式9體現(xiàn)了這一點(diǎn)），這樣預(yù)測(cè)值才最可靠。下面詳細(xì)說(shuō)明。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

6、+1) - X(k+1|k+1) （后驗(yàn)預(yù)測(cè)的偏差）X(k+1|k+1) = X(k+1) - X(k+1|k+1) = (X(k+1|k) + X(k+1|k) - ( X(k+1|k) +Kg * Z(k+1|k) ) = X(k+1|k) - Kg * Z(k+1|k) 式9Z(k+1|k) = Z(k+1) - Z(k+1|k) = (HX（k+1）+ V(k+1) ) - (HX(k+1|k) ) =H( X（k+1）-X(k+1|k) ) +V(k+1) =H X(k+1|k) +V(k+1) 式10接下來(lái)的分析中，為了更直觀的說(shuō)明卡爾曼濾波的原理，我們用幾何方法來(lái)解釋。這時(shí)，X和

7、Z矩陣中的每個(gè)元素應(yīng)看做向量空間中的一個(gè)向量而不再是一個(gè)單純的數(shù)。這個(gè)向量空間（統(tǒng)計(jì)測(cè)試空間）可以看成無(wú)窮多維的，每一個(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é)方差矩陣。我們無(wú)法給出X和Z中每個(gè)元素向量的具體表達(dá)，但我們通過(guò)協(xié)方差矩陣就可以知道所有元素向量的模長(zhǎng)，以及相互之間的夾角（從內(nèi)積計(jì)算）。為了方便用幾何方法解釋?zhuān)覀兗僭O(shè)狀態(tài)變量X是一個(gè)1行1列的矩陣（即只有一個(gè)待測(cè)狀態(tài)量），而量測(cè)變量Z是一個(gè)2行1列的矩陣（即有兩個(gè)測(cè)量?jī)x

8、器，共同測(cè)量同一個(gè)狀態(tài)量X），也就是說(shuō)，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按元素展開(kāi)：X(k+1|k+1)1 = X(k+1|k)1 - (Kg1 * Z(k+1|k)1 + Kg2 * Z(k+1|k)2 ) 式9iZ(k+1|k)i =Hi X(k+1|k) + V(k+1)i 式10iX(k+1|k) 中各個(gè)元素（向量）的線性組合可以產(chǎn)生一個(gè)m維或更低維的向量子空間Vx，這里，按照我們的假設(shè)，m=1，所以Vx應(yīng)是一維的； 同時(shí)V(k+1

9、)中的各個(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)（見(jiàn)附注1），故這兩個(gè)子空間相互垂直。如下圖所示。式10i所體現(xiàn)的Z(k+1|k)i、Hi X(k+1|k)、V(k+1)i 三者之間的幾何關(guān)系，也在下圖中描繪了出來(lái)。從上圖中可以看出，Z(k+1|k)中各個(gè)元素（向量）的線性組合也可以產(chǎn)生一個(gè)n維或更低維的向量子空間Vz，這里已假設(shè)n=2，所以Vz是一個(gè)二維的平面，就是上圖中兩條紅色的線所構(gòu)成的平面。圖2中（注意此圖中的橢圓代表的是Vz空間，而圖1中則代表V

10、v空間，二者不一樣），粉色的向量就是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空間。通過(guò)先驗(yàn)預(yù)測(cè)的協(xié)方差矩陣（見(jiàn)卡爾曼公式2），可以得到X(k+1|k)中各個(gè)元素的模長(zhǎng)以及彼此間的夾角。這是因?yàn)閰f(xié)方差矩陣中的第i行第j列其實(shí)就代表了X(k+1|k)中第i個(gè)元素向量與第j個(gè)元素向量的內(nèi)積。通過(guò)測(cè)量可以得到

11、新息協(xié)方差（見(jiàn)卡爾曼公式3的分母部分），進(jìn)而可以知道Z(k+1|k)中各個(gè)元素的模長(zhǎng)以及彼此間的夾角。通過(guò)已知的量測(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。（上面討論的是較低次的卡爾曼濾波，只有一個(gè)待測(cè)量，兩個(gè)測(cè)量?jī)x器。這種情況還是比較常見(jiàn)的，比如傾角測(cè)量系統(tǒng)中，我們用加速度計(jì)和陀螺儀共同測(cè)量?jī)A角。對(duì)于更高次的卡爾曼濾波，X和Z都是多行矩陣時(shí)，用幾何方法已經(jīng)

12、無(wú)法直觀解釋?zhuān)荒苡镁仃嚪治龅姆椒ㄗC明。求解Kg的詳細(xì)過(guò)程參考 卡爾曼濾波器及其應(yīng)用基礎(chǔ)國(guó)防工業(yè)出版社敬喜 編 ）卡爾曼濾波的核心過(guò)程，就是求解能使得E X(k+1|k+1)* X(k+1|k+1)取最小值的Kg增益矩陣的過(guò)程，X(k+1|k+1)代表的是X(k+1|k+1)的轉(zhuǎn)置（這里X(k+1|k+1)中的元素代表數(shù)值，不是向量）。前面已經(jīng)提到過(guò)，卡爾曼增益矩陣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) - X(k+1|k) =X(k+1) -(AX(k|k)

13、 +BU(k) ) =(AX(k) +BU(k) + W(k) ) - (AX(k) +BU(k) -AX(k|k)= W(k) +AX(k|k)= W(k) +A( X(k|k-1) - Kg(k)* Z(k|k-1) ) -這一步利用了式9 = W(k) +A( X(k|k-1) - Kg(k)* (HX(k|k-1) + v(k) ) ) -這一步利用了式10 = W(k) -AKg(k) *v(k) +A(I- Kg(k) *H) X(k|k-1)上式最后一行出現(xiàn)了X(k|k-1)，可見(jiàn)X(k+1|k)可以遞歸表示。而且遞歸式中的過(guò)程噪聲W(k)與v(k+1)不相關(guān)，同時(shí)由于 v本身是

14、白噪聲，所以 v(k+1)與v(k)亦不相關(guān)（白噪聲的自相關(guān)是函數(shù)），因此通過(guò)遞推式可以判斷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) = X(k+1) - X(k+1|k+1) =( X(k+1|k) + X(k+1|k) ) - (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)* (HX(k+1|k) + v(k+1) ) = - Kg(

15、k+1)* v(k+1) + (I- Kg(k+1) *H) X(k+1|k)我們已經(jīng)知道w(k+1)與v(k+1)不相關(guān)，因此只要X(k+1|k+1)與上式的第二項(xiàng)也不相關(guān)，就說(shuō)明結(jié)論(b)成立。根據(jù)(a)中的結(jié)論，X(k+1|k)的遞歸展開(kāi)式中出現(xiàn)的 v(k) ，w(k) ， v(k-1)，w(k-1)等等，顯然 w(k+1)與 v (m=k，k-1) 都不相關(guān)，另外，由于w(k+1)的自相關(guān)為函數(shù)，因此w(k+1)與 w(m=k，k-1) 也不相關(guān)，也就得出w(k+1)與X(k+1|k) 不相關(guān)。進(jìn)而可知，w(k+1)與X(k+1|k+1)不相關(guān)。正是因?yàn)?(a) (b)中的兩個(gè)不相關(guān)，

16、卡爾曼公式中的預(yù)測(cè)協(xié)方差矩（卡爾曼公式(2)）和新息協(xié)方差矩陣（卡爾曼公式(3)中的“分母”部分）才可以是簡(jiǎn)單的加式。附注2：卡爾曼濾波的五個(gè)公式先驗(yàn)預(yù)測(cè)值與先驗(yàn)預(yù)測(cè)協(xié)方差矩陣的計(jì)算。求解協(xié)方差時(shí)，都認(rèn)為預(yù)測(cè)值的期望是實(shí)際值。因此，X(k+1|k)的協(xié)方差矩陣同樣也是 X(k+1|k) 的協(xié)方差矩陣，又因?yàn)槠頧(k+1|k)的期望是0，因此協(xié)方差矩陣反映了X(k+1|k)在向量空間中的模長(zhǎng)。注意，協(xié)方差矩陣都是對(duì)稱(chēng)矩陣。X(k+1|k)=A X(k|k)+B U(k) (1)P(k+1|k)=A P(k|k) A+Q(k) (2)卡爾曼增益矩陣的計(jì)算。量測(cè)預(yù)測(cè)值為Z(k+1|k) = H X(k+1|k)新息協(xié)方差見(jiàn)公式（3）中的“分母”部分