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

附錄：kalman濾波（起源、發(fā)展、原理、應(yīng)用）1、Kalman濾波起源及發(fā)展1960年，匈牙利數(shù)學(xué)家卡爾曼發(fā)表了一篇關(guān)于離散數(shù)據(jù)線(xiàn)性濾波遞推算法的論文，這意味著卡爾曼濾波的誕生。斯坦利.施密特（StanleySchmidt）首次實(shí)現(xiàn)了卡爾曼濾波器，卡爾曼在NASA埃姆斯研究中心訪(fǎng)問(wèn)時(shí)，發(fā)現(xiàn)他的方法對(duì)于解決阿波羅計(jì)劃的軌道預(yù)測(cè)很有用，后來(lái)阿波羅飛船的導(dǎo)航電腦使用了這種濾波器。關(guān)于這種濾波器的論文由Swerling（1958）、Kalman（1960）與KalmanandBucy（1961）發(fā)表.卡爾曼濾波是一種有著相當(dāng)廣泛應(yīng)用的濾波方法，但它既需要假定系統(tǒng)是線(xiàn)性的，又需要認(rèn)為系統(tǒng)中的各個(gè)噪聲與狀態(tài)變量均呈高斯分布，而這兩條并不總是確切的假設(shè)限制了卡爾曼濾波器在現(xiàn)實(shí)生活中的應(yīng)用。擴(kuò)展卡爾曼濾波器EKF）極大地拓寬了卡爾曼濾波的適用范圍。EKF的基本思路是，假定卡爾曼濾濾對(duì)當(dāng)前系統(tǒng)狀態(tài)估計(jì)值非常接近于其真實(shí)值，于是將非線(xiàn)性函數(shù)在當(dāng)前狀態(tài)估計(jì)值處進(jìn)行臺(tái)勞展開(kāi)并實(shí)現(xiàn)線(xiàn)性化。另一種非線(xiàn)性卡爾曼濾波叫線(xiàn)性化卡爾曼濾波。它與EKF的主要區(qū)別是前者將非線(xiàn)函數(shù)在濾波器對(duì)當(dāng)前系統(tǒng)狀態(tài)的最優(yōu)估計(jì)值處線(xiàn)性化，而后者因?yàn)轭A(yù)先知道非線(xiàn)性系統(tǒng)的實(shí)際運(yùn)行狀態(tài)大致按照所要求、希望的軌跡變化，所以這些非線(xiàn)性化函數(shù)在實(shí)際狀態(tài)處的值可以表達(dá)為在希望的軌跡處的臺(tái)勞展開(kāi)式，從而完成線(xiàn)性化。不敏卡爾曼濾波器（UKF）是針對(duì)非線(xiàn)性系統(tǒng)的一種改進(jìn)型卡爾曼濾波器。UKF處理非線(xiàn)性系統(tǒng)的基本思路在于不敏變換，而不敏變換從根本上講是一種描述高斯隨機(jī)變量在非線(xiàn)性化變換后的概率分布情況的方法。不敏卡爾曼濾波認(rèn)為，與其將一個(gè)非線(xiàn)性化變換線(xiàn)性化、近似化，還不如將高斯隨機(jī)變量經(jīng)非線(xiàn)性變換后的概率分布情況用高斯分布來(lái)近似那樣簡(jiǎn)單，因而不敏卡爾曼濾波算法沒(méi)有非線(xiàn)性化這一步驟。在每一定位歷元，不敏卡爾曼濾波器按照一套公式產(chǎn)生一系列樣點(diǎn)，每一樣點(diǎn)均配有一個(gè)相應(yīng)的權(quán)重，而這些帶權(quán)的樣點(diǎn)被用來(lái)完整地描述系統(tǒng)狀態(tài)向量估計(jì)值的分布情況，它們替代了原先卡爾曼濾波器中的狀態(tài)向量估計(jì)值及協(xié)方差。不敏卡爾曼濾器讓這些樣點(diǎn)一一經(jīng)歷非線(xiàn)性狀態(tài)方程與測(cè)量方程，然后再將這些經(jīng)非線(xiàn)性變換后的樣點(diǎn)按照它們的權(quán)重而綜合出對(duì)當(dāng)前時(shí)刻的系統(tǒng)狀態(tài)向量估計(jì)值。多態(tài)自適應(yīng)（MMA）卡爾曼濾波器是一種受到廣泛關(guān)注的濾波器，它由好多個(gè)并聯(lián)、同時(shí)運(yùn)行的卡爾曼濾波器組成。在這組卡爾曼濾波器中，每一個(gè)濾波器對(duì)未知的濾波參數(shù)分別做出相互不同的假設(shè)，然后各自按照自己的模型假設(shè)進(jìn)行濾波計(jì)算，而多態(tài)自適應(yīng)濾波器最后將它們對(duì)系統(tǒng)狀態(tài)的各個(gè)估計(jì)值進(jìn)行加權(quán)，并以此作為最優(yōu)估計(jì)值輸出。

2、Kalman原理卡爾曼濾波是基于狀態(tài)空間方法的一套遞推濾波算法，在狀態(tài)空間方法中，引入了狀態(tài)變量的概念。實(shí)際應(yīng)用中，可以通過(guò)選取合適的狀態(tài)變量來(lái)體現(xiàn)系統(tǒng)的特征、特點(diǎn)和狀況的變化?？柭鼮V波的模型包括狀態(tài)空間模型和觀測(cè)模型。狀態(tài)模型是反映狀態(tài)變化規(guī)律的模型，通過(guò)狀態(tài)方程來(lái)描寫(xiě)相鄰時(shí)刻的狀態(tài)轉(zhuǎn)移變化規(guī)律；觀測(cè)模型反映了實(shí)際觀測(cè)量與狀態(tài)變量之間的關(guān)系。Kalman濾波問(wèn)題就是聯(lián)合觀測(cè)信息及狀態(tài)轉(zhuǎn)移規(guī)律來(lái)得到系統(tǒng)狀態(tài)的最優(yōu)估計(jì)。假設(shè)動(dòng)態(tài)系統(tǒng)的狀態(tài)空間模型為(2-1)(2-2)x(t+1)=ox(t)+rw(t)(2-1)(2-2)Y(t)=HX(t)+V(t)其中，X(t)系統(tǒng)在時(shí)刻t的狀態(tài)Y(t)對(duì)狀態(tài)的觀測(cè)值W(t)系統(tǒng)噪聲，方差陣為QV(t)觀測(cè)噪聲，方差陣為R① 狀態(tài)轉(zhuǎn)移矩陣H 觀測(cè)矩陣r 系統(tǒng)噪聲驅(qū)動(dòng)矩陣卡爾曼濾波的計(jì)算流程為：計(jì)算狀態(tài)估計(jì)值：(2-3)(2-4)(2-5)(2-6)X(t+111+1)=X(t+111)+K(t+(2-3)(2-4)(2-5)(2-6)計(jì)算狀態(tài)一步預(yù)測(cè)：X(t+111)=QX(t11)計(jì)算新息：&(t+1)=Y(t+1)-HX(t+111)計(jì)算卡爾曼濾波增益：K(t+1)=P(t+111)Ht[HP(t+111)Ht+R]-1計(jì)算一步預(yù)測(cè)均方誤差:(2-7)(2-8(2-7)(2-8)圖1卡爾曼濾波的系統(tǒng)模型p(t+111)=①p(t11)Qt+rgrt計(jì)算一步預(yù)測(cè)估計(jì)均方誤差:P(t+111+1)=[I—K(t+1)H]P(t+111)為了更形象地說(shuō)明卡爾曼濾波的原理，下面給出卡爾曼濾波的系統(tǒng)模型框圖:3、Kalman濾波應(yīng)用之一：基于VDLL的GPS信號(hào)跟蹤算法⑵導(dǎo)航濾波器導(dǎo)航濾波器圖2VDLL的基本結(jié)構(gòu)附錄第3頁(yè)系統(tǒng)的狀態(tài)量為用戶(hù)狀態(tài)(位置、速度、用戶(hù)鐘差、用戶(hù)鐘漂移)，觀測(cè)量為所有衛(wèi)星通道的碼環(huán)鑒相器輸出組成的矢量D(X)X=(xyzbxyzb)tD(X)=(D(^t1)D(At2)…D(Atn))t (3-1)其中x,y,z，b,x,y,Z，b分別為在ECEF(EarthCenteredEarthFixed,地心坐標(biāo)系)下的用戶(hù)接收機(jī)三維位置和速度，b,b分別為接收機(jī)鐘差和鐘差變化率。預(yù)檢測(cè)積分時(shí)間T選擇為1ms,即濾波器迭代的k時(shí)刻與k+1時(shí)刻間隔1ms。假設(shè)用戶(hù)為勻加速運(yùn)動(dòng)，可寫(xiě)出狀態(tài)方程如下-1TI―Xk=0f (3-2)1-4 4」其中wk-1為狀態(tài)擾動(dòng)噪聲。導(dǎo)航濾波器把k-1時(shí)刻的狀態(tài)和對(duì)應(yīng)的衛(wèi)星位置信息反饋給本地信號(hào)生成模塊，產(chǎn)生超前、即時(shí)與滯后碼序列，分別與k時(shí)刻的1ms衛(wèi)星信號(hào)的同相支路與正交支路做相關(guān)運(yùn)算，即圖2中的“積分存儲(chǔ)”模塊。為了去除對(duì)信號(hào)幅度變化的敏感性和對(duì)載波環(huán)路鎖定的依賴(lài)性，碼環(huán)鑒相器選擇歸一化的超前碼與滯后碼相關(guān)能量差(第i顆衛(wèi)星)(3-3)D(金)='E2+QE2-IH-QLi，k IE2+QE2+IL2+QL(3-3)該碼環(huán)鑒相器在沒(méi)有噪聲的理想情況下，當(dāng)超前碼與滯后碼間距為1chip(碼片)時(shí)，在±0.5chip輸入誤差范圍內(nèi)，鑒相器輸出等于真實(shí)的跟蹤誤差。在實(shí)際應(yīng)用中，超前滯后碼的移位通常以采樣點(diǎn)為單位，1個(gè)chip的長(zhǎng)度不一定為整數(shù)個(gè)采樣點(diǎn)，但鑒相器仍然與輸入誤差成線(xiàn)性關(guān)系，所以式可寫(xiě)為D(At)=gAt+v (3-4)i,k ii,k i,k其中g(shù)i為待求的鑒相器特性在線(xiàn)性區(qū)的斜率，v.k為觀測(cè)噪聲。鑒相器輸入誤差atk是k時(shí)刻(接收機(jī)時(shí)刻)第i顆衛(wèi)星1ms長(zhǎng)度的采樣信號(hào)與本地即時(shí)碼之間的相位偏移，而本地即時(shí)碼的初始相位值等于k-1時(shí)刻對(duì)衛(wèi)星信號(hào)的相位估計(jì)。所以△".,k實(shí)質(zhì)上是k時(shí)刻衛(wèi)星信號(hào)的相位與k-1時(shí)刻的相位估計(jì)之差，等于k時(shí)刻信號(hào)傳輸延遲的真實(shí)值與k-1時(shí)刻信號(hào)傳輸延遲的估計(jì)值。k時(shí)刻接收到第i顆衛(wèi)星信號(hào)的理論測(cè)量延遲為T(mén)OC\o"1-5"\h\z1二 ; 7 ; 7 ;~ 7T=一\；(x -x)2+(y -y)2 +(Z -z)2 +b (3-5)i,k ck i,k k i,k ki,k k其中(xk,yk,zk)為k時(shí)刻用戶(hù)的三維位置坐標(biāo),(X、k,yitk，zirk)為k時(shí)刻第i顆衛(wèi)星的位置坐標(biāo),bk為k時(shí)刻的接收機(jī)鐘差°K-1時(shí)刻信號(hào)傳輸延遲的估計(jì)值可由k-1時(shí)刻的用戶(hù)狀態(tài)估計(jì)X(k-1Ik-1)和衛(wèi)星位置計(jì)算得到入 1 ： ■ — ■ — ■- ?t=—J(x —x)2+(y—x)2+(z—x)2+b(3-6)i,k—1 c k—1Ik—1 i,k—1 k—1Ik—1 i,k—1 k—1Ik—1 i,k—1 k—1Ik—1由(4))(6)式，可寫(xiě)出第i顆衛(wèi)星的觀測(cè)方程如下1二 - ； ；；I ~ 7廣D(X)=g{-,(x-x)2+(y-y)2+(z-z)2+b-b-kic%k i,k k i,k ki,kk k—1Ik—1] (3-7)(x -x)2+(y-x)2+(z -x )2