卡爾曼濾波起源發(fā)展原理及應用

1、附錄：kalman濾波（起源、發(fā)展、原理、應用）1、Kalman濾波起源及發(fā)展1960年，匈牙利數(shù)學家卡爾曼發(fā)表了一篇關于離散數(shù)據(jù)線性濾波遞推算法的論文，這意味著卡爾曼濾波的誕生。斯坦利.施密特(Stanley Schmidt)首次實現(xiàn)了卡爾曼濾波器，卡爾曼在NASA埃姆斯研究中心訪問時，發(fā)現(xiàn)他的方法對于解決阿波羅計劃的軌道預測很有用，后來阿波羅飛船的導航電腦使用了這種濾波器。關于這種濾波器的論文由Swerling (1958)、Kalman (1960)與 Kalman and Bucy (1961)發(fā)表.卡爾曼濾波是一種有著相當廣泛應用的濾波方法，但它既需要假定系統(tǒng)是線性的，又需要認為系統(tǒng)

2、中的各個噪聲與狀態(tài)變量均呈高斯分布，而這兩條并不總是確切的假設限制了卡爾曼濾波器在現(xiàn)實生活中的應用。擴展卡爾曼濾波器（EKF）極大地拓寬了卡爾曼濾波的適用范圍。EKF的基本思路是，假定卡爾曼濾濾對當前系統(tǒng)狀態(tài)估計值非常接近于其真實值，于是將非線性函數(shù)在當前狀態(tài)估計值處進行臺勞展開并實現(xiàn)線性化。另一種非線性卡爾曼濾波叫線性化卡爾曼濾波。它與EKF的主要區(qū)別是前者將非線函數(shù)在濾波器對當前系統(tǒng)狀態(tài)的最優(yōu)估計值處線性化，而后者因為預先知道非線性系統(tǒng)的實際運行狀態(tài)大致按照所要求、希望的軌跡變化，所以這些非線性化函數(shù)在實際狀態(tài)處的值可以表達為在希望的軌跡處的臺勞展開式，從而完成線性化。不敏卡爾曼濾波器（U

3、KF）是針對非線性系統(tǒng)的一種改進型卡爾曼濾波器。UKF處理非線性系統(tǒng)的基本思路在于不敏變換，而不敏變換從根本上講是一種描述高斯隨機變量在非線性化變換后的概率分布情況的方法。不敏卡爾曼濾波認為，與其將一個非線性化變換線性化、近似化，還不如將高斯隨機變量經(jīng)非線性變換后的概率分布情況用高斯分布來近似那樣簡單，因而不敏卡爾曼濾波算法沒有非線性化這一步驟。在每一定位歷元，不敏卡爾曼濾波器按照一套公式產(chǎn)生一系列樣點，每一樣點均配有一個相應的權重，而這些帶權的樣點被用來完整地描述系統(tǒng)狀態(tài)向量估計值的分布情況，它們替代了原先卡爾曼濾波器中的狀態(tài)向量估計值及協(xié)方差。不敏卡爾曼濾器讓這些樣點一一經(jīng)歷非線性狀態(tài)方程

4、與測量方程，然后再將這些經(jīng)非線性變換后的樣點按照它們的權重而綜合出對當前時刻的系統(tǒng)狀態(tài)向量估計值。多態(tài)自適應（MMA）卡爾曼濾波器是一種受到廣泛關注的濾波器，它由好多個并聯(lián)、同時運行的卡爾曼濾波器組成。在這組卡爾曼濾波器中，每一個濾波器對未知的濾波參數(shù)分別做出相互不同的假設，然后各自按照自己的模型假設進行濾波計算，而多態(tài)自適應濾波器最后將它們對系統(tǒng)狀態(tài)的各個估計值進行加權，并以此作為最優(yōu)估計值輸出。2、Kalman原理卡爾曼濾波是基于狀態(tài)空間方法的一套遞推濾波算法，在狀態(tài)空間方法中，引入了狀態(tài)變量的概念。實際應用中，可以通過選取合適的狀態(tài)變量來體現(xiàn)系統(tǒng)的特征、特點和狀況的變化。卡爾曼濾波的模型

5、包括狀態(tài)空間模型和觀測模型。狀態(tài)模型是反映狀態(tài)變化規(guī)律的模型，通過狀態(tài)方程來描寫相鄰時刻的狀態(tài)轉移變化規(guī)律；觀測模型反映了實際觀測量與狀態(tài)變量之間的關系。Kalman濾波問題就是聯(lián)合觀測信息及狀態(tài)轉移規(guī)律來得到系統(tǒng)狀態(tài)的最優(yōu)估計。假設動態(tài)系統(tǒng)的狀態(tài)空間模型為 （2-1） （2-2）其中，X(t) 系統(tǒng)在時刻t的狀態(tài)Y(t) 對狀態(tài)的觀測值W(t) 系統(tǒng)噪聲，方差陣為QV(t) 觀測噪聲，方差陣為R 狀態(tài)轉移矩陣H 觀測矩陣 系統(tǒng)噪聲驅動矩陣卡爾曼濾波的計算流程為：計算狀態(tài)估計值： （2-3）計算狀態(tài)一步預測： （2-4）計算新息： （2-5）計算卡爾曼濾波增益： （2-6）計算一步預測均方誤差

6、： （2-7）計算一步預測估計均方誤差： （2-8）為了更形象地說明卡爾曼濾波的原理，下面給出卡爾曼濾波的系統(tǒng)模型框圖：輸入量測量噪聲過程噪聲觀測量延時狀態(tài)變量系統(tǒng)圖1 卡爾曼濾波的系統(tǒng)模型3、Kalman濾波應用之一：基于VDLL的GPS信號跟蹤算法2圖2 VDLL的基本結構系統(tǒng)的狀態(tài)量為用戶狀態(tài)( 位置、速度、用戶鐘差、用戶鐘漂移) ，觀測量為所有衛(wèi)星通道的碼環(huán)鑒相器輸出組成的矢量D(X) X=(x y z b )TD(X)=(D(1) D(2) D(N)T （3-1）其中x，y，z，b，分別為在ECEF( Earth Centered Earth Fixed, 地心坐標系) 下的用戶接收

7、機三維位置和速度， b, 分別為接收機鐘差和鐘差變化率。預檢測積分時間T 選擇為1ms，即濾波器迭代的k時刻與k+1時刻間隔1ms。假設用戶為勻加速運動，可寫出狀態(tài)方程如下 （3-2）其中wk-1 為狀態(tài)擾動噪聲。 導航濾波器把k- 1 時刻的狀態(tài)和對應的衛(wèi)星位置信息反饋給本地信號生成模塊，產(chǎn)生超前、即時與滯后碼序列，分別與k時刻的1ms 衛(wèi)星信號的同相支路與正交支路做相關運算，即圖2 中的“積分存儲”模塊。為了去除對信號幅度變化的敏感性和對載波環(huán)路鎖定的依賴性，碼環(huán)鑒相器選擇歸一化的超前碼與滯后碼相關能量差( 第i 顆衛(wèi)星) （3-3）該碼環(huán)鑒相器在沒有噪聲的理想情況下，當超前碼與滯后碼間距

8、為1 chip( 碼片) 時，在±0.5 chip 輸入誤差范圍內(nèi)，鑒相器輸出等于真實的跟蹤誤差。在實際應用中，超前滯后碼的移位通常以采樣點為單位，1 個chip 的長度不一定為整數(shù)個采樣點，但鑒相器仍然與輸入誤差成線性關系，所以式可寫為 （3-4）其中gi 為待求的鑒相器特性在線性區(qū)的斜率， vi, k為觀測噪聲。鑒相器輸入誤差是k時刻( 接收機時刻) 第i顆衛(wèi)星1ms 長度的采樣信號與本地即時碼之間的相位偏移，而本地即時碼的初始相位值等于k- 1時刻對衛(wèi)星信號的相位估計。所以i, k實質(zhì)上是k時刻衛(wèi)星信號的相位與k - 1時刻的相位估計之差，等于k 時刻信號傳輸延遲的真實值與k- 1 時刻信號傳輸延遲的估計值。 k 時刻接收到第i顆衛(wèi)星信號的理論測量延遲為 （3-5）其中( xk , yk , zk ) 為k 時刻用戶的三維位置坐標,( xi, k , yi, k , zi, k ) 為k 時刻第i 顆衛(wèi)星的位置坐標,bk 為k 時刻的接收機鐘差。K-1 時刻信號傳輸延遲的估計值可由k-1 時刻的用戶狀態(tài)估計 ( k- 1 |k- 1) 和衛(wèi)星位置計算得到 （3-6）由( 4) ) ( 6) 式，可寫出第i 顆衛(wèi)