卡爾曼濾波算法(含詳細推導)

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

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

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

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

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

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

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

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

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

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

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

12、：將觀測方程(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)向量的一步預測誤差向量的相關矩陣，容易證明：式中使用了e(n+1,n)，v1(n)，v2(n)彼此不相關的事實，以及和 等關系式。 對式(31)的右邊進行展開，然后代入式(28)和(29)，可以證明：狀態(tài)向量預測誤差的相關矩陣的遞推公式為

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)向量的濾波估計，則定義濾波狀態(tài)向量的誤差向量，可以證明：因此，Ricc

14、ati差分方程中的矩陣P(n)事實上是濾波誤差狀態(tài)向量的相關矩陣。（4）、kalman濾波算法 將上面推導得到的式(28)、(16)、(13)、(25)、(33)和(32)依次加以歸納，得到基于一步預測的kalman自適應濾波算法如下。初始條件：)(nx)1 () 1 (,)1 () 1 ()1 () 1 ()0 , 1 ()1 () 1 (1xExxxxxEKxExH其中)35.(.(n)e(n)eP(n)HE)34.(.).()(e(n)1nxnx3、kalman濾波算法輸入觀測向量過程： 觀測向量=y(1),y(n)已知參數(shù)： 狀態(tài)轉移矩陣F(n+1,n) 觀測矩陣C(n) 過程噪聲向量的相關矩陣Q1(n) 觀測噪聲向量的相關矩陣Q2(n)計算：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.(.).(),