卡爾曼濾波算法(含詳細(xì)推導(dǎo))

1、卡爾曼濾波算法及推導(dǎo)1、kalman濾波問題考慮一離散時(shí)間的動(dòng)態(tài)系統(tǒng)，它由描述狀態(tài)向量的過程方程和描述觀測(cè)向量的觀測(cè)方程共同表示。（1）、過程方程 式中，M 1向量x(n)表示系統(tǒng)在離散時(shí)間n的狀態(tài)向量，它是不可觀測(cè)的；M M矩陣F（n+1,n）成為狀態(tài)轉(zhuǎn)移矩陣，描述動(dòng)態(tài)系統(tǒng)在時(shí)間n的狀態(tài)到n+1的狀態(tài)之間的轉(zhuǎn)移，應(yīng)為已知。而M 1向量 為過程噪聲向量，它描述狀態(tài)轉(zhuǎn)移中間的加性噪聲或誤差。 ) 1.(), 1(11)()()(nvnxnnFnx)(nv11、kalman濾波問題（1）、觀測(cè)方程 式中，N 1向量y(n)表示動(dòng)態(tài)系統(tǒng)在時(shí)間n的觀測(cè)向量； N M矩陣C（n）稱為觀測(cè)矩陣（描述狀態(tài)經(jīng)

2、過其作用，變成可預(yù)測(cè)的），要求也是已知的；v2(n)表示觀測(cè)噪聲向量，其維數(shù)與觀測(cè)向量的相同。過程方程也稱為狀態(tài)方程，為了分析的方便，通常假定過程噪聲v1(n)和觀測(cè)噪聲v2(n)均為零均值的白噪聲過程，它們的相關(guān)矩陣分別為：)2.()(2)()()(nvnxnCny1、kalman濾波問題)3.()()(),(, 0111knnQknHkvnvE)4.()()(),(, 0222knnQknHkvnvE1、kalman濾波問題還假設(shè)狀態(tài)的初始值x(0)與v1(n) 、 v2(n)，n 0均不相關(guān)，并且噪聲向量v1(n)與v2(n)也不相關(guān)，既有：)5.(, 0)()(21knkvnvEH2、

3、新息過程考慮一步預(yù)測(cè)問題，給定觀測(cè)值y(1), .，y(n-1)，求觀測(cè)向量y(n)的最小二乘估計(jì)，記作 （1）、新息過程的性質(zhì) y(n)的新息過程定義為：式中，N 1向量 表示觀測(cè)數(shù)據(jù)y(n)的新的信息，簡稱新息。)1(),.,1( )(1nyynynydef)6.().()()(1nynyn)(n2、新息過程新息 具有以下性質(zhì)：性質(zhì)1 n時(shí)刻的新息 與所有過去的觀測(cè)數(shù)據(jù)y(1), .，y(n-1)正交，即：性質(zhì)2 新息過程由彼此正交的隨機(jī)向量序列 組成，即有 )(n)(n)7.(11 , 0)()(nkkynEH)8(.11 , 0)()(nkknEH)(n2、新息過程性質(zhì)3 表示觀測(cè)數(shù)據(jù)

4、的隨機(jī)向量序列y(1) ,y(n)與表示新息過程的隨機(jī)向量序列a(1),a(n) 一一對(duì)應(yīng) ，即以上性質(zhì)表明：n時(shí)刻的新息a(n)是一個(gè)與n上課之前的觀測(cè)數(shù)據(jù)y(1), .，y(n-1)不相關(guān)，并具有白噪聲性質(zhì)的隨機(jī)過程，但它卻能夠提供有關(guān)y(n)的新息，這就上信息的內(nèi)在物理含義。)9.().(),.1 ()(),.1 (nnyy2、新息過程（2）、新息過程的計(jì)算 下面分析新息過程的相關(guān)矩陣 在kalman濾波中，并不直接估計(jì)觀測(cè)數(shù)據(jù)向量的進(jìn)一步預(yù)測(cè) ，而是先計(jì)算狀態(tài)向量的一步預(yù)測(cè)然后再用到下式得到 ：)11().1(),.1 ()(1nyynxndefx)10.().()()(nnEnRH)

5、(1ny)12.().()()(11nxnCny2、新息過程將上式代入新息過程的定義式（6），可得到：這就是新息過程的實(shí)際計(jì)算公式，條件是：一步預(yù)測(cè)的狀態(tài)向量估計(jì) 業(yè)已求出。定義向量的一步預(yù)測(cè)誤差：)14.().()(), 1(1nxnxnnedef)13.().()()()()()()()(211nvnxnxnCnxnCnyn)(1nx2、新息過程將此式代入式（13），則有在新息過程的相關(guān)矩陣定義式（10）中代入式（14），并注意到觀測(cè)矩陣C（n）是一已知的確定矩陣，故有式中Q2(n)是觀測(cè)噪聲v2(n)的相關(guān)矩陣，而表示（一步）預(yù)測(cè)狀態(tài)誤差的相關(guān)矩陣)15().() 1,()()(2nvn

6、nenCn)16.(.).()() 1,()()()()()1,() 1,()()(222nQnCnnKnCnvnvEnCnnenneEnCnRHHHH)17.(.).1,() 1,() 1,(nnenneEnnKH3、kalman濾波算法由上一節(jié)的的新息過程的相關(guān)知識(shí)和信息后，即可轉(zhuǎn)入kalman濾波算法的核心問題的討論：如何利用新息過程估計(jì)狀態(tài)向量的預(yù)測(cè)？最自然的方法是用新息過程序列a(1),a(n)的線性組合直接構(gòu)造狀態(tài)向量的一布預(yù)測(cè)：式中W1(k)表示與一步預(yù)測(cè)項(xiàng)對(duì)應(yīng)的權(quán)矩陣，且k為離散時(shí)間。現(xiàn)在的問題是如何確定這個(gè)權(quán)矩陣？（1）、狀態(tài)向量的一布預(yù)測(cè) 根據(jù)正交性原理，最優(yōu)預(yù)測(cè)的估計(jì)誤差

7、nkdefkkWnyynnxx111)()()(),.,1 (1()() 1() 1(n)1,e(n1nxnx3、kalman濾波算法應(yīng)該與已知值正交，故有將式（18）代入（19），并利用新息過程的正交性，得到由此可以求出權(quán)矩陣的表達(dá)式：)20.().()() 1()(11KRknxEkWH)()()()()()() 1(11kRkWkkEkWknxEHH)19.(,.,1, 0)() 1() 1()(), 1(1nkknxnxEknneEHH3、kalman濾波算法 將式（20）代入式（18），狀態(tài)向量的一步預(yù)測(cè)的最小均方估計(jì)可表示為注意到 并利用狀態(tài)方程(1)，易知下式對(duì)k=0，1，n成立

8、：)21.().()()() 1()()( )() 1()()( )() 1() 1(1111111nnRnnxEkkRknxEkkRknxEnHnkHnkHx,.,1 , 0, 0)()(1nkknvE3、kalman濾波算法將式（22）代入式（21）右邊第一項(xiàng)（求和項(xiàng)），可將其化簡為：)22.().()(), 1()()()(), 1()() 1(1knxEnnFknvnxnnFEknxEHH)23.(.).(), 1()()()()(), 1()()()()1(111111nxnnFkkRknxEnnFkkRknxEnkHnkH3、kalman濾波算法若定義 并將式（23）和式（24）代

9、入式（21），則得到狀態(tài)向量一步預(yù)測(cè)的更新公式：式（25）在kalman濾波算法中起著關(guān)鍵的作用，因?yàn)樗砻?，n+1時(shí)刻的狀態(tài)向量的一步預(yù)測(cè)分為非自適應(yīng)（即確定）部分 和自適應(yīng)（即校正）部分G(n)a(n)。從這個(gè)意義上講，G(n)稱為kalman增益（矩陣）是合適的。)()() 1()(1kRknxEnGHdef)25.().()()(), 1() 1(nnGnxnnFnx)(), 1(nxnnF3、kalman濾波算法（2）、 kalman增益的計(jì)算 為了完成kalman自適應(yīng)濾波算法，需要進(jìn)一步推導(dǎo)kalman增益的實(shí)際計(jì)算公式。由定義式（24）知，只需要推導(dǎo)期望項(xiàng) 的具體計(jì)算公式即可。

10、 將新息過程的計(jì)算公式（13）代入式（22），不難得出： 這里使用了狀態(tài)向量與觀測(cè)噪聲不相關(guān)的事實(shí)。進(jìn)一步地，由正交原理引理知，在最小均方誤差準(zhǔn)則下求得的一步預(yù)測(cè)估 與預(yù)測(cè)誤差e(n,n-1)彼此正交，即)() 1(knxEH)(1nx)26).()1,()(), 1()()1,()()(), 1()()(), 1()()1(2nCnnenxEnnFnvnnenCnxEnnFnnxEnnFnnxEHHHHH0)1,()(1NNenxEH3、kalman濾波算法因此，由式(26)及式(27)易得： 將式(27)代入式(24)，便得到kalman增益的計(jì)算公式如下：式中R(n)是信息過程的相關(guān)矩陣

11、，由式(10)定義。)28.().()() 1,(), 1()(1nRnCnnKnnFnGH)27).() 1,(), 1()()1,() 1,(), 1()()1,()1,()(), 1()() 1(nCnnKnnFnCnnenneEnnFnCnnennenxEnnFnnxEHHHHHH3、kalman濾波算法（3）、Riccati方程 由式(28)表示的kalman增益與預(yù)測(cè)狀態(tài)誤差的相關(guān)矩陣K(n,n-1)有關(guān)，為了最后完成kalman自適應(yīng)濾波算法，還需要再推導(dǎo)K(n,n-1)的遞推公式。 考察狀態(tài)向量的預(yù)測(cè)誤差：將狀態(tài)方程(1)和狀態(tài)向量的一步預(yù)測(cè)更新公式(25)代入式(29)中，有

12、：將觀測(cè)方程(2)代入上式，并代入 ，則有：)()(1)-ne(n,1nxnx)29.().1() 1(n)1,e(n1nxnx)()()()()()()(), 1(n)1,e(n111nvnxnCnynGnxnxnnF)30.().()()(1)ne(n,)()(), 1(n)1,e(n21nvnGnvnCnGnnF3、kalman濾波算法求式(3)所示狀態(tài)向量的一步預(yù)測(cè)誤差向量的相關(guān)矩陣，容易證明：式中使用了e(n+1,n)，v1(n)，v2(n)彼此不相關(guān)的事實(shí)，以及和 等關(guān)系式。 對(duì)式(31)的右邊進(jìn)行展開，然后代入式(28)和(29)，可以證明：狀態(tài)向量預(yù)測(cè)誤差的相關(guān)矩陣的遞推公式為

13、：式中 式(32)稱為Riccati差分方程。)32.().(), 1()(), 1(), 1(1nQnnFkPnnFnnKH)33).(1,()()(), 1() 1,()(1nnKnCnGnnFnnKnP)31.().()()()()()(), 1()1,()()(), 1(), 1(), 1(), 1(21nGnQnGnQnCnGnnFnnKnCnGnnFnnenneEnnKHHH)()()(111nQnvnvEH)()()(222nQnvnvEH3、kalman濾波算法若定義 是利用已知的y(1),y(n)求得的狀態(tài)向量的濾波估計(jì)，則定義濾波狀態(tài)向量的誤差向量，可以證明：因此，Ricc

14、ati差分方程中的矩陣P(n)事實(shí)上是濾波誤差狀態(tài)向量的相關(guān)矩陣。（4）、kalman濾波算法 將上面推導(dǎo)得到的式(28)、(16)、(13)、(25)、(33)和(32)依次加以歸納，得到基于一步預(yù)測(cè)的kalman自適應(yīng)濾波算法如下。初始條件：)(nx)1 () 1 (,)1 () 1 ()1 () 1 ()0 , 1 ()1 () 1 (1xExxxxxEKxExH其中)35.(.(n)e(n)eP(n)HE)34.(.).()(e(n)1nxnx3、kalman濾波算法輸入觀測(cè)向量過程： 觀測(cè)向量=y(1),y(n)已知參數(shù)： 狀態(tài)轉(zhuǎn)移矩陣F(n+1,n) 觀測(cè)矩陣C(n) 過程噪聲向量的相關(guān)矩陣Q1(n) 觀測(cè)噪聲向量的相關(guān)矩陣Q2(n)計(jì)算：n=1,2,3,)34.(.).()(e(n)1nxnx)34.(.).()(e(n)1nxnx)36.(.)()() 1,()()() 1,(), 1()(12anQnCnnKnCnCnnKnnFnGHH)36.(.).()()(n)1bnxnCny)36.(.).1,()()(), 1() 1,()(1dnnKnCnGnnFnnKnP)36.(.).()()(), 1() 1(11cnnGnxnnFnx)36.(.).(),