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

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

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

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

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

6、： （2-7）計算一步預測估計均方誤差： （2-8）為了更形象地說明卡爾曼濾波的原理，下面給出卡爾曼濾波的系統模型框圖：輸入量測量噪聲過程噪聲觀測量延時狀態變量系統圖1 卡爾曼濾波的系統模型3、Kalman濾波應用之一：基于VDLL的GPS信號跟蹤算法2圖2 VDLL的基本結構系統的狀態量為用戶狀態( 位置、速度、用戶鐘差、用戶鐘漂移) ，觀測量為所有衛星通道的碼環鑒相器輸出組成的矢量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。假設用戶為勻加速運動，可寫出狀態方程如下 （3-2）其中wk-1 為狀態擾動噪聲。 導航濾波器把k- 1 時刻的狀態和對應的衛星位置信息反饋給本地信號生成模塊，產生超前、即時與滯后碼序列，分別與k時刻的1ms 衛星信號的同相支路與正交支路做相關運算，即圖2 中的“積分存儲”模塊。為了去除對信號幅度變化的敏感性和對載波環路鎖定的依賴性，碼環鑒相器選擇歸一化的超前碼與滯后碼相關能量差( 第i 顆衛星) （3-3）該碼環鑒相器在沒有噪聲的理想情況下，當超前碼與滯后碼間距

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