卡爾曼濾波算法實現代碼卡爾曼濾波器簡介近來發(fā)現有些問題很多人都很感興趣。所以在這里希望能盡自己能力跟大家討論一些力所能及的算法。現在先討論一下卡爾曼濾波器，如果時間和能力允許，我還希望能夠寫寫其他的算法，例如遺傳算法，傅立葉變換，數字濾波，神經網絡，圖像處理等等。因為這里不能寫復雜的數學公式，所以也只能形象的描述。希望如果哪位是這方面的專家，歡迎討論更正。卡爾曼濾波器-KalmanFilter1． 什么是卡爾曼濾波器（WhatistheKalmanFilter?）在學習卡爾曼濾波器之前，首先看看為什么叫“卡爾曼”。跟其他著名的理論（例如傅立葉變換，泰勒級數等等）一樣，卡爾曼也是一個人的名字，而跟他們不同的是，他是個現代人！卡爾曼全名RudolfEmilKalman,匈牙利數學家，1930年出生于匈牙利首都布達佩斯。1953,1954年于麻省理工學院分別獲得電機工程學士及碩士學位。1957年于哥倫比亞大學獲得博士學位。我們現在要學習的卡爾曼濾波器，正是源于他的博士論文和1960年發(fā)表的論文《ANewApproachtoLinearFilteringandPredictionProblems》（線性濾波與預測問題的新方法）。如果對這編論文有興趣，可以到這里的地址下載：簡單來說，卡爾曼濾波器是一個“optimalrecursivedataprocessingalgorithm（最優(yōu)化自回歸數據處理算法）”。對于解決很大部分的問題，他是最優(yōu)，效率最高甚至是最有用的。他的廣泛應用已經超過30年，包括機器人導航，控制，傳感器數據融合甚至在軍事方面的雷達系統以及導彈追蹤等等。近年來更被應用于計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。2．卡爾曼濾波器的介紹（IntroductiontotheKalmanFilter）為了可以更加容易的理解卡爾曼濾波器，這里會應用形象的描述方法來講解，而不是像大多數參考書那樣羅列一大堆的數學公式和數學符號。但是，他的5條公式是其核心內容。結合現代的計算機，其實卡爾曼的程序相當的簡單，只要你理解了他的那5條公式。在介紹他的5條公式之前，先讓我們來根據下面的例子一步一步的探索。假設我們要研究的對象是一個房間的溫度。根據你的經驗判斷，這個房間的溫度是恒定的，也就是下一分鐘的溫度等于現在這一分鐘的溫度（假設我們用一分鐘來做時間單位）。假設你對你的經驗不是100%的相信，可能會有上下偏差幾度。我們把這些偏差看成是高斯白噪聲（WhiteGaussianNoise)，也就是這些偏差跟前后時間是沒有關系的而且符合高斯分配(GaussianDistribution)。另外，我們在房間里放一個溫度計，但是這個溫度計也不準確的，測量值會比實際值偏差。我們也把這些偏差看成是高斯白噪聲。好了，現在對于某一分鐘我們有兩個有關于該房間的溫度值：你根據經驗的預測值(系統的預測值)和溫度計的值(測量值)。下面我們要用這兩個值結合他們各自的噪聲來估算出房間的實際溫度值。假如我們要估算k時刻的是實際溫度值。首先你要根據k-1時刻的溫度值，來預測k時刻的溫度。因為你相信溫度是恒定的，所以你會得到k時刻的溫度預測值是跟k-1時刻一樣的，假設是23度，同時該值的高斯噪聲的偏差是5度(5是這樣得到的：如果k-1時刻估算出的最優(yōu)溫度值的偏差是3，你對自己預測的不確定度是4度，他們平方相加再開方，就是5)。然后，你從溫度計那里得到了k時刻的溫度值，假設是25度，同時該值的偏差是4度。由于我們用于估算k時刻的實際溫度有兩個溫度值，分別是23度和25度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的covariance來判斷。因為Kg'2=5'2/(5'2+4'2)，所以Kg=，我們可以估算出k時刻的實際溫度值是：23+*(25-23)=度。可以看出，因為溫度計的covariance比較小(比較相信溫度計)，所以估算出的最優(yōu)溫度值偏向溫度計的值。現在我們已經得到k時刻的最優(yōu)溫度值了，下一步就是要進入k+1時刻，進行新的最優(yōu)估算。到現在為止，好像還沒看到什么自回歸的東西出現。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優(yōu)值(度)的偏差。算法如下：((1-Kg)*5'2)'=。這里的5就是上面的k時刻你預測的那個23度溫度值的偏差，得出的就是進入k+1時刻以后k時刻估算出的最優(yōu)溫度值的偏差(對應于上面的3)。就是這樣，卡爾曼濾波器就不斷的把covariance遞歸，從而估算出最優(yōu)的溫度值。他運行的很快，而且它只保留了上一時刻的covariance。上面的Kg,就是卡爾曼增益(KalmanGain)。他可以隨不同的時刻而改變他自己的值，是不是很神奇！下面就要言歸正傳，討論真正工程系統上的卡爾曼。3． 卡爾曼濾波器算法(TheKalmanFilterAlgorithm)在這一部分，我們就來描述源于DrKalman的卡爾曼濾波器。下面的描述，會涉及一些基本的概念知識，包括概率(Probability),隨即變量(RandomVariable)，高斯或正態(tài)分配(GaussianDistribution)還有State-spaceModel等等。但對于卡爾曼濾波器的詳細證明，這里不能一一描述。首先，我們先要引入一個離散控制過程的系統。該系統可用一個線性隨機微分方程(LinearStochasticDifferenceequation)來描述：X(k)=AX(k-1)+BU(k)+W(k)再加上系統的測量值：Z(k)=HX(k)+V(k)上兩式子中，x(k)是k時刻的系統狀態(tài)，U(k)是k時刻對系統的控制量。A和B是系統參數，對于多模型系統，他們?yōu)榫仃嚒(k)是k時刻的測量值，H是測量系統的參數，對于多測量系統，H為矩陣。W(k)和V(k)分別表示過程和測量的噪聲。他們被假設成高斯白噪聲(WhiteGaussianNoise)，他們的covariance分別是Q,R(這里我們假設他們不隨系統狀態(tài)變化而變化)。對于滿足上面的條件(線性隨機微分系統，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最優(yōu)的信息處理器。下面我們來用他們結合他們的covariances來估算系統的最優(yōu)化輸出(類似上一節(jié)那個溫度的例子)。首先我們要利用系統的過程模型，來預測下一狀態(tài)的系統。假設現在的系統狀態(tài)是k，根據系統的模型，可以基于系統的上一狀態(tài)而預測出現在狀態(tài)：x(k|k-1)=Ax(k-1|k-1)+BU(k)………..(1)式(1)中，X(k|k-1)是利用上一狀態(tài)預測的結果，X(k-1|k-1)是上一狀態(tài)最優(yōu)的結果，U(k)為現在狀態(tài)的控制量，如果沒有控制量，它可以為0。到現在為止，我們的系統結果已經更新了，可是，對應于X(k|k-1)的covariance還沒更新。我們用P表示covariance：P(k|k-1)=AP(k-1|k-1)A'+Q………(2)式(2)中，P(k|k-1)是X(k|k-1)對應的covariance，P(kT|k-1)是X(kT|k-1)對應的covariance，A'表示A的轉置矩陣，Q是系統過程的covariance。式子1,2就是卡爾曼濾波器5個公式當中的前兩個，也就是對系統的預測。現在我們有了現在狀態(tài)的預測結果，然后我們再收集現在狀態(tài)的測量值。結合預測值和測量值，我們可以得到現在狀態(tài)(k)的最優(yōu)化估算值X(k|k):X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))………(3)其中Kg為卡爾曼增益(KalmanGain):Kg(k)=P(k|k-1)H'/(HP(k|k-1)H'+R)………(4)到現在為止，我們已經得到了k狀態(tài)下最優(yōu)的估算值X(k|k)。但是為了要另卡爾曼濾波器不斷的運行下去直到系統過程結束，我們還要更新k狀態(tài)下X(k|k)的covariance：P(k|k)=(I-Kg(k)H)P(k|k-1)………(5)其中I為1的矩陣，對于單模型單測量，1=1。當系統進入k+1狀態(tài)時，P(k|k)就是式子(2)的P(kT|k-1)。這樣，算法就可以自回歸的運算下去。卡爾曼濾波器的原理基本描述了，式子1，2，3，4和5就是他的5個基本公式。根據這5個公式，可以很容易的實現計算機的程序。下面，我會用程序舉一個實際運行的例子。。。4． 簡單例子(ASimpleExample)這里我們結合第二第三節(jié)，舉一個非常簡單的例子來說明卡爾曼濾波器的工作過程。所舉的例子是進一步描述第二節(jié)的例子，而且還會配以程序模擬結果。根據第二節(jié)的描述，把房間看成一個系統，然后對這個系統建模。當然，我們見的模型不需要非常地精確。我們所知道的這個房間的溫度是跟前一時刻的溫度相同的，所以A=1。沒有控制量，所以U(k)=0。因此得出：X(k|k-1)=X(k-1|k-1)………..(6)式子(2)可以改成：P(k|k-1)=P(k-1|k-1)+Q………(7)因為測量的值是溫度計的，跟溫度直接對應，所以H=1。式子3,4,5可以改成以下：X(k|k)=X(k|k-1)+Kg(k)(Z(k)-X(k|k-1))………(8)Kg(k)=P(k|k-1)/(P(k|k-1)+R)………(9)P(k|k)=(1-Kg(k))P(k|k-1)………(10)現在我們模擬一組測量值作為輸入。假設房間的真實溫度為25度，我模擬了200個測量值，這些測量值的平均值為25度，但是加入了標準偏差為幾度的高斯白噪聲(在圖中為藍線)。為了令卡爾曼濾波器開始工作，我們需要告訴卡爾曼兩個零時刻的初始值，是X(0|0)和P(0|0)。他們的值不用太在意，隨便給一個就可以了，因為隨著卡爾曼的工作，X會逐漸的收斂。但是對于P,—般不要取0,因為這樣可能會令卡爾曼完全相信你給定的X(0|0)是系統最優(yōu)的，從而使算法不能收斂。我選了X(0|0)=1度，P(0|0)=10。該系統的真實溫度為25度，圖中用黑線表示。圖中紅線是卡爾曼濾波器輸出的最優(yōu)化結果(該結果在算法中設置了Q=le-6,R=le-1)。最佳線性濾波理論起源于40年代美國科學家Wiener和前蘇聯科學家Ko^MorOp的研究工作，后人統稱為維納濾波理論。從理論上說，維納濾波的最大缺點是必須用到無限過去的數據，不適用于實時處理。為了克服這一缺點，60年代Kalman把狀態(tài)空間模型引入濾波理論，并導出了一套遞推估計算法，后人稱之為卡爾曼濾波理論。卡爾曼濾波是以最小均方誤差為估計的最佳準則，來尋求一套遞推估計的算法，其基本思想是：采用信號與噪聲的狀態(tài)空間模型，利用前一時刻地估計值和現時刻的觀測值來更新對狀態(tài)變量的估計，求出現時刻的估計值。它適合于實時處理和計算機運算。現設線性時變系統的離散狀態(tài)防城和觀測方程為：X(k)=F(k,k-1)?X(k-1)+T(k,k-1)?U(k—1)Y(k)=H(k)?X(k)+N(k)其中X(k)和Y(k)分別是k時刻的狀態(tài)矢量和觀測矢量F(k,k-1)為狀態(tài)轉移矩陣U(k)為k時刻動態(tài)噪聲T(k,k-1)為系統控制矩陣H(k)為k時刻觀測矩陣N(k)為k時刻觀測噪聲則卡爾曼濾波的算法流程為：預估計X(k)lF(k,k—1)?X(k-1)計算預估計協方差矩陣C(k廠二F(k,k—l)XC(k)XF(k,k—l)'+T(k,k—l)XQ(k)XT(k,k—l)'Q(k)=U(k)XU(k)'計算卡爾曼增益矩陣K(k)=C(k廠XH(k)'X[H(k)XC(k廠XH(k)'+R(k)廠(—1)R(k)=N(k)XN(k)'更新估計X(k)~=X(k廠+K(k)X[Y(k)—H(k)XX(k廠]計算更新后估計協防差矩陣C(k)~=[I—K(k)XH(k)]XC(k廠X[I—K(k)XH(k)]'+K(k)XR(k)XK(k)'X(k+1)=X(k)~C(k+1)=C(k)~重復以上步驟KalmanFilter2010-05-2921:13:49閱讀90評論0字號：大中小訂閱KalmanFilter是一個高效的遞歸濾波器，它可以實現從一系列的噪聲測量中，估計動態(tài)系統的狀態(tài)。廣泛應用于包含Radar、計算機視覺在內的等工程應用領域，在控制理論和控制系統工程中也是一個非常重要的課題。連同線性均方規(guī)劃，卡爾曼濾波器可以用于解決LQG(Linear-quadratic-Gaussiancontrol)問題。卡爾曼濾波器，線性均方歸化及線性均方高斯控制器，是大部分控制領域基礎難題的主要解決途徑。目錄1應用實例2命名和發(fā)展歷史3基本動態(tài)系統模型4卡爾曼濾波器預測更新不變量■■后驗估計協方差矩陣推導Kalman增益推導后驗誤差協方差矩陣簡化■8非線性濾波器擴展Kalman濾波UnscentedKalmanfilter9Kalman-Bucy濾波10應用11參見12參考文獻13外部鏈接1應用實例一個簡單的應用是估計物體的位置和速度；簡要描述如下：假設我們可以獲取一個物體的包含噪聲的一系列位置觀測數據，我們可以獲得此物體的精確速度和位置連續(xù)更新信息。例如，對于雷達來說，我們關心的是跟蹤目標，而目標的位置，速度，加速度的測量值是時刻含有誤差的，卡爾曼濾波器利用目標的動態(tài)信息，去掉噪聲影響，獲取目標此刻好的位置估計(濾波)，將來位置估計(預測)，也可以是過去位置估計的(插值或平滑)2命名和發(fā)展歷史這個濾波器以它的發(fā)明者而命名，但是在Kanlman之前，ThorvaldNicolaiThiele和PeterSwerling已經提出了類似的算法。StanleySchmidt首次實現了Kalman濾波器。在一次對NASAAmesResearchCenter訪問中，卡爾曼發(fā)現他的方法對于解決阿波羅計劃的軌跡預測很有用，后來阿波羅飛船導航電腦就使用了這種濾波器。這個濾波器可以追溯到Swerling(1958),Kalman(1960),Kalman和Bucy(1961)發(fā)表的論文。這個濾波器有時叫做Stratonovich-Kalman-Bucy濾波器。因為更為一般的非線性濾波器最初由Ruslan發(fā)明，而Stratonovich-Kalman-Bucy濾波器只是非線性濾波器的一個特例。事實上，1960年夏季，Kalman和Stratonovich在一個Moscow召開的會議中相遇，而作為非線性特例的線性濾波方程，早已經由Stratonovich在此以前發(fā)表了。在控制領域,Kalman濾波被稱為線性二次型估計，目前，卡爾曼濾波已經有很多不同的實現,有施密特擴展濾波器、信息濾波器以及一系列的Bierman和Thornton發(fā)明的平方根濾波器等，而卡爾曼最初提出的形式現在稱為簡單卡爾曼濾波器。也許最常見的卡爾曼濾波器應用是鎖相環(huán)，它在收音機、計算機和幾乎全部視頻或通訊設備中廣泛存在。■3基本動態(tài)系統模型Kalman濾波基于時域描述的線性動態(tài)系統，它的模型是MarkovChain，而MarkovChain建立在一個被高斯噪聲干擾的線性算子之上。系統的狀態(tài)可以用一個元素為實數的向量表示。隨著離散時間的增加，這個線性算子就會作用到當前狀態(tài)之上，產生一個新的狀態(tài)，并且會帶入一定的噪聲，同時一些已知的控制信息也會加入。同時另外一個受噪聲干擾的線性算子將產生這些隱含狀態(tài)的可見輸出。Kalman濾波可以被看作為類似隱馬爾科夫模型，它們的顯著不同點在于：隱狀態(tài)變量的取值空間是一個連續(xù)的空間，而離散狀態(tài)空間則不是；另為，隱馬爾科夫模型可以描述下一個狀態(tài)的一個任意分布，這也與應用于Kalman濾波器中的高斯噪聲模型相反。Kalman濾波器方程和隱馬爾科夫方程之間有很大的二重性，關于Kalman濾波方程和隱馬爾科夫方程之間二重性參看RoweisandGhahramani（1999）［4］。為了從一系列的噪聲觀測中，應用Kalman濾波估計觀測過程的內部狀態(tài)。我們必須把這個過程在Kalman濾波器的框架下建立模型，這就意味著，對于每一步k我們要定義矩陣理、旦丘、Q、兔、爲如下：KalmanFilter假設k時刻的真實狀態(tài)是從k-1時刻演化而來，符合下式血=F込X+呂戡+叭這里■理是作用在前一狀態(tài)的狀態(tài)轉移模型（狀態(tài)轉移矩陣）■耳是作用在控制向量上的控制輸入模型（輸入輸出矩陣）■ ■是過程噪聲，假設是均值為o的白噪聲，協方差為貝y：叫TOQ在k時刻，假設真實狀態(tài)琨的觀測，？丘滿足如下公式:比=盡心+吃其中丹丘是觀測模型（觀測矩陣），它把真實狀態(tài)映射到觀測空間，叫是觀測噪聲，假設它是均值是0,方差是尺尺的高斯白噪聲:KalmanFilter基本動態(tài)系統模型如圖（1）所示，圓圈代表向量，方塊代表矩陣，星號代表高斯噪聲，其協方差在右下方標出。初始狀態(tài)以及每一時刻的噪聲向量｛x0,w1,...,wk,v1...vk｝都為認為是互相獨立的。實際中，真實世界中動態(tài)系統并不是嚴格的符合此模型。但是Kalman模型是設計在噪聲過程工作的，一個近似的符合已經可以使這個濾波器非常有用了，更多復雜模型關于KalmanFilter模型的變種，將在下述中討論：■ ■ rnfltbijff.blDg.ie3jsaiHi圖(1)■4卡爾曼濾波器KalmanFilter是一個遞歸的估計，即只要獲知上一時刻的狀態(tài)估計和當前狀態(tài)的觀測就可以計算出當前狀態(tài)的估計，不同于其他的估計技術，Kalman濾波器不需要觀測或/和估計的歷史記錄，KalmanFilter是一個純粹的時域濾波器，而不像低通濾波器等頻域濾波器那樣需要在頻域中設計，然后轉換到時域中應用。下面，耳祀代表已知從m到n-1包括m時刻的觀測在n時刻的估計值卡爾曼濾波器的狀態(tài)由以下兩個變量表示：■X晰已知k時刻以前時刻觀測值，k時刻的狀態(tài)估計值■弓E誤差協方差矩陣，度量狀態(tài)估計的精度程度Kalman濾波包括兩個階段：預測和更新；在估計階段，濾波器應用上一狀態(tài)的估計做出對當前狀態(tài)的估計。在更新階段，濾波器利用在當前狀態(tài)的觀測值優(yōu)化預測階段的預測值，以獲的一個更精確的當前狀態(tài)的估計。預測狀態(tài)預測：估計協方差預測：■場卜】=比&冊祝+Q_i更新新息或測量余量新息協方差■風=H&PgH；+坨Kalman增益狀態(tài)估計更新狀態(tài)協方差更新使用上述公式計算幾丘僅在最優(yōu)卡爾曼增益的時候有效。使用其他增益公式要復雜一些，看見推導不變量如果模型準確，毛0和對口值將準確反映最初狀態(tài)的分布，那么下面所有不變量保持不變,所有估計的誤差均值為0：■這里珥燈表示1"的期望，而協方差矩陣則反映的估計的協方差■直二泅仇）■5實例考慮在一個無摩擦、無限長的直軌道上的一輛小車，它的初始位置在0點，但是它會隨機的受到沖擊作用，我們每隔右f測量一次小車的位置，但是這些測量數據不是很精確。我們想建立一個關于小車位置和速度的模型，這里我們描述如何建立這個模型，以及從這個模型出發(fā)如何推導出Kalman濾波器。因為小車沒有控制輸入，我們可以忽略塊和％。由于F，H，R和Q全是恒值，我們可以忽略時間下標。小車的位置和速度用線性空間可以描述如下：

這里匸表示速度，也就是位置對時間的微分。我們假設在時間間隔k-1和k之間，小車受到一個恒定的沖擊樂，樂服從均值為0，方差為"厘的正態(tài)分布，根據Newton動力學方程，可得到：二 +島Jt其中我們發(fā)現：Q=如至）=E[（G^G^]=G母/冒=的呼在每一時刻,我們獲取真實位置的?我們假設噪聲服噪聲干擾測量，假設測量噪聲服從均值為0,標準差為丫正態(tài)分布。尊二陸+羽其中H=[10]，叫叫警!麴我們可以得到足夠精度的初始狀態(tài)數據，所以我們可以初始化我們可以得到足夠精度的初始狀態(tài)數據，所以我們可以初始化如果初始位置和速度不是精確的知道，那么協方差矩陣應該初始化為一個對角線元素B為適當大小的矩陣如下：這樣與模型中已有信息相比，濾波器更趨向于使用首次的測量數據信息。■6推導后驗估計協方差矩陣推導首先開始不變量后驗估計協方差矩陣昂丘的推導:P碌=2式耳-和）帶入工腿定義，可場嚴由于誤差向量叫與其他不相關，所以場嚴由于誤差向量叫與其他不相關，所以弘.二如心-耳％）（忑-％1））十如乞叫）由協方差矩陣性質則珈=&-瓦砂沁-7』-心歸+瓦珈=&-瓦砂沁-7』-心歸+瓦g堿:使用不變量Pklk-1以及Rk的定義這一項可以寫作碌 代以山/—呵4）+朋鳳，此公式Josephform）對任意增益Kk的都成立，如果Kk最優(yōu)卡爾曼增益，則可以進一步簡化，見下文。Kalman增益推導，這等價于最小化后驗估計協方差矩陣馬用的Kalman濾波器是一個最小均方誤差估計器，先驗狀態(tài)誤差估計可表示為張—際我們最小化這個矢量幅度平方的期望值應口農—切』］，這等價于最小化后驗估計協方差矩陣馬用的跡，通過展開合并碌公式，可得跡，通過展開合并碌公式，可得冠二門hi"瓦丹占W弔層十晦（見-弓加1丹【+兔）層=張一耳％F冒-環(huán)腫：境+憨g當矩陣導數為0時，矩陣的跡取最小值,從這個式子解出Kalman增益疋這個增益就是最優(yōu)Kalman增益，應用它可以得到最小均方誤差。后驗誤差協方差矩陣簡化當應用上述最優(yōu)Kalman增益時，后驗誤差協方差可以得到簡化，在最優(yōu)Kalman增益兩邊同時乘以&煜，可得蔓鳳心二F%厲莊&，參見后驗誤差協方差公式展開張二仏二張二仏二1-瓦耳厲卜1二（』一瓦■趁城41這個公式的計算比較簡單，所以實際中總是使用這個公式，但是需注意這公式僅在最優(yōu)卡爾曼增益時它才成立。如果算術精度總是很低而導致數值穩(wěn)定性出現問題，或者特意使用非最優(yōu)卡爾曼增益，那么就不能使用這個簡化；必須使用上面導出的后驗誤差協方差公式。■7信息濾波在信息濾波器（逆方差濾波器）中，協方差估計和狀態(tài)估計將會被信息矩陣和信息向量所取代，它們的定義如下：凡總=抵％類似的預測協方差和預測狀態(tài)也有等價的信息形式，定義如下:'加-1='加-1=頃-1同樣測量協方差和測量向量定義為：信息更新現在變成一個加和形式：N九R=九—+遲知j'-i信息濾波器的主要優(yōu)點在于N和測量數據都可以用于濾波，簡單的通過信息矩陣和信息向量的加和。為了預測信息濾波器，信息矩陣和信息向量必須變換到它們的等價狀態(tài)空間，或者應用下述信息空間更新：這里F和Q必須可逆。■8非線性濾波器擴展Kalman濾波估計過程如以上所述，卡爾曼濾波器估計一個線性隨機差分方程描述的離散時間過程的狀態(tài)變量葢占,但是如果被估計的過程和（或）觀測變量與過程的關系不時線性關系。那該如何處理呢？一些很有趣和成功的Kalman濾波器應用就是處理這些情況的。將期望和方差線性化的卡爾曼濾波器稱作擴展卡爾曼濾波器（ExtendedKalmanFilter）,簡稱EKF。同泰勒級數類似，面對非線性關系時，我們可以通過求過程和量測方程的偏導來線性化并計算當前估計，為了實現這個目的，我們必須修改上面的一些描述，我們假設過程仍具有狀態(tài)向量応代，但其狀態(tài)方程已變?yōu)榉蔷€性隨機差分方程的形式。觀測變量為N=曲（竝心）這里隨機變量乂和％分別為過程噪聲和觀測噪聲。差分方程式中的非線性函數f將過去k-1時刻狀態(tài)與現在k時刻狀態(tài)聯系起來。在測量方程中，輸入函數uk和零均值過程噪聲wk是它的參數。非線性函數h反映了狀態(tài)變量xk和觀測變量zk的關系。實際中我們并不知道每一時刻噪聲wk和vk各自真實值，但是我們可以在假設他們不存在的前提下，近似估計狀態(tài)向量和測量向量：這里玉是相對于前一時刻k的后驗狀態(tài)估計。有一點非常重要，那就是擴展卡爾曼濾波器的一個基本缺陷：離散隨機變量的分布（或連續(xù)隨機變量的密度）在經過非線性系統轉化后不再是正態(tài)的了。擴展卡爾曼濾波器其實就是一個通過線性化而達到漸進最優(yōu)貝葉斯決策的特殊狀態(tài)估計器°[Julier96]中描述了一項有趣的研究，Julier設計了擴展卡爾曼濾波器的一種變體，使得通過非線性轉換后的隨機變量仍具有正態(tài)分布特性。濾波器的計算原型為了估計一個具有非線性差分和量測關系的過程，我們先給出式和式的一個新的線性化表示：心F 一3+阿其中：xk和zk是狀態(tài)向量和觀測向量的真值；“和來自式和式，是狀態(tài)向量和觀測向量的近似值;4“是k時刻狀態(tài)向量的后驗估計;隨機變量wk和vk表示過程激勵噪聲和觀測噪聲。A是f對x的偏導的雅可比矩陣：W是f對w的偏導的雅可比矩陣：H是h對x的偏導的雅可比矩陣：V是h對v的偏導的雅可比矩陣：

高斯：如果一個噪聲，它的幅度分布服從，而它的功率譜密度又是均勻分布的，則稱它為高斯白噪聲。和散粒噪聲是高斯白噪聲。所謂高斯白噪聲中的高斯是指概率分布是正態(tài)函數， 而白噪聲是指它的二階矩不相關，一階矩為常數，是指先后信號在時間上的相關性。這是考查一個信號的兩個不同方面的問題。短波信道存在多徑時延、多普勒頻移和擴散、高斯白噪聲干擾等復雜現象。為了測試短波通信設備的性能，通常需要進行大量的外場實驗。相比之下，信道模擬器能夠在實驗室環(huán)境下進行類似的性能測試，而且測試費用少、可重復性強，可以縮短設備的研制周期。所以自行研制信道模擬器十分必要。程序1這個程序就是你要的。#include""#include""#include""#include""#include""defineMAX_N3000/*這個值為N可以定義的最大長度*/defineN100 /*產生隨機序列的點數，注意不要大于MAX_N*//*產生均勻分布的隨機變量*/voidranda(float*x,intnum);/*產生/*產生瑞利分布的隨機變量*/voidrandr(float*x,intnum);/*產生/*產生標準高斯分布的隨機變量*/voidrandn(float*x,intnum);/*產生 的隨機變量*/voidrandl(float*x,floata,floatb,intnum);voidfshow(char*name,float*x,intnum);main()floatx[N];inti;/*randa(&x,N);randr(&x,N);randl(&x,10,10,N);*/randn(&x,N);/*此時x[N]就是所需要的高斯分布的序列*//*顯示該序列*/fshow("x",&x,N);getch();}voidranda(float*x,intnum){inti;structtimestime;unsignedseed;gettime(&stime);seed=**;srand(seed);for(i=0;i<num;i++){x[i]=rand();x[i]=x[i]/32768;}}voidrandr(float*x,intnum){floatx1[MAX_N];inti;structtimestime;unsignedseed;gettime(&stime);seed=**;srand(seed);for(i=0;i<num;i++){x1[i]=rand();x[i]=x1[i]/32768;x[i]=sqrt(-2*log(x[i]));}}voidrandn(float*x,intnum){floatx1[MAX_N],x2[MAX_N];inti;structtimestime;unsignedseed;gettime(&stime);seed=**;srand(seed);for(i=0;i<num;i++){x1[i]=rand();x2[i]=rand();x1[i]=x1[i]/32768;x2[i]=x2[i]/32768;x[i]=sqrt(-2*log(x1[i]))*cos(x2[i]*M_PI);}}voidrandl(float*x,floata,floatb,intnum){floatx1[MAX_N],x2[MAX_N];floattemp[MAX_N];inti;structtimestime;unsignedseed;gettime(&stime);seed=**;srand(seed);for(i=0;i<num;i++){x1[i]=rand();x2[i]=rand();x1[i]=x1[i]/32768;x2[i]=x2[i]/32768;temp[i]=sqrt(-2*log(x1[i]))*cos(x2[i]*M_PI);x2[i]=sqrt(-2*log(x1[i]))*sin(x2[i]*M_PI);x1[i]=temp[i];x[i]=sqrt((a+x1[i])*(a+x1[i])+(b+x2[i])*(b+x2[i]));}}voidfshow(char*name,float*x,intnum){inti,sign,L;floattemp;printf("\n");printf(name);printf("=");L=6;/*按照每行6個數據的格式顯示*/for(i=0;i<num;i++){temp=i/L;sign=temp;if((i-sign*L)==0)printf("\n");if(x[i]>0)printf("%f ",x[i]);elseprintf("%f ",x[i]);}}程序2以下程序加入了圖形顯示的效果，因此更加直觀，你可以參考一下。/*作者Leo_nanjing時間功能生成各種分布的隨機變量,并顯示*/#include""#include""#include""#include""#include""#defineMAX_N3000#defineN1000voidranda(float*x,intnum);voidrandr(float*x,intnum);voidrandn(float*x,intnum);voidrandl(float*x,floata,floatb,intnum);voidfshow(char*name,float*x,intnum);/*用于圖形顯示的部分*/voidinit_graphic(unsignedcolor);voidplotxy(float*x,float*y,intnum,intmode);voidplot(float*y,intnum,intmode);floatmax(float*x,intnum);floatmin(float*x,intnum);/*畫出該隨機序列的分布函數曲線*/voidplotpdf(float*x,intnum,intpart,intmode);main(){floatx[N];inti;randn(&x,N);fshow("x",&x,N);getch();/*以下為圖形顯示部分*/init_graphic(0);/*顯示隨機序列*/plot(&x,N,1);getch();/*顯示其分布函數*/plotpdf(&x,N,20,0);getch();}voidranda(float*x,intnum){inti;structtimestime;unsignedseed;gettime(&stime);seed=**;srand(seed);for(i=0;i<num;i++){x[i]=rand();x[i]=x[i]/32768;}}voidrandr(float*x,intnum){floatx1[MAX_N];inti;structtimestime;unsignedseed;gettime(&stime);seed=**;srand(seed);for(i=0;i<num;i++){x1[i]=rand();x[i]=x1[i]/32768;x[i]=sqrt(-2*log(x[i]));}}voidrandn(float*x,intnum){floatx1[MAX_N],x2[MAX_N];inti;structtimestime;unsignedseed;gettime(&stime);seed=**;srand(seed);for(i=0;i<num;i++){x1[i]=rand();x2[i]=rand();x1[i]=x1[i]/32768;x2[i]=x2[i]/32768;x[i]=sqrt(-2*log(x1[i]))*cos(x2[i]*M_PI);}}voidrandl(float*x,floata,floatb,intnum){floatx1[MAX_N],x2[MAX_N];floattemp[MAX_N];inti;structtimestime;unsignedseed;gettime(&stime);seed=**;srand(seed);for(i=0;i<num;i++){x1[i]=rand();x2[i]=rand();x1[i]=x1[i]/32768;x2[i]=x2[i]/32768;temp[i]=sqrt(-2*log(x1[i]))*cos(x2[i]*M_PI);x2[i]=sqrt(-2*log(x1[i]))*sin(x2[i]*M_PI);x1[i]=temp[i];x[i]=sqrt((a+x1[i])*(a+x1[i])+(b+x2[i])*(b+x2[i]));}}voidfshow(char*name,float*x,intnum){inti,sign,L;floattemp;printf("\n");printf(name);printf("=");L=6;for(i=0;i<num;i++){temp=i/L;sign=temp;if((i-sign*L)==0)printf("\n");if(x[i]>0)printf("%f",x[i]);elseprintf("%f",x[i]);}}/*以下為圖形顯示的函數*/voidinit_graphic(unsignedcolor){intgraphicdriver,graphicmode;graphicdriver=DETECT;graphicmode=1;initgraph(&graphicdriver,&graphicmode,"E:\\turboc2\\");setbkcolor(color);}voidplotxy(float*x,float*y,intnum,intmode){inti;floatmax_x,max_y,min_x,min_y;floatx0,y0,x1,y1;clrscr(0);cleardevice();setbkcolor(0);max_x=max(x,num);max_y=max(y,num);min_x=min(x,num);min_y=min(y,num);setlinestyle(0,2,1);line(65,35,65,445);line(65,445,575,445);setlinestyle(3,0,1);line(65,35,575,35);line(575,35,575,445);setlinestyle(0,2,1);if(max_x==min_x)x0=320;elsex0=(x[0]-min_x)*500/(max_x-min_x)+70;if(max_y==min_y)y0=240;elsey0=480-((y[0]-min_y)*400/(max_y-min_y)+40);if(mode==0)circle(x0,y0,2);for(i=1;i<num;i++){if(max_x==min_x)x1=320;elsex1=(x[i]-min_x)*500/(max_x-min_x)+70;if(max_y==min_y)y1=240;elsey1=480-((y[i]-min_y)*400/(max_y-min_y)+40);if(mode==0)circle(x1,y1,2);line(x0,y0,x1,y1);x0=x1;y0=y1;}printf("\n\n");printf("%f",max_y);printf("\n\n\n\n\n\n\n\n\n\n");printf("\n\n\n");printf("%f",(max_y+min_y)/2);printf("\n\n\n\n\n\n\n\n\n\n");printf("\n\n");printf("%f",min_y);printf("\n%f",min_x);printf(" ");printf("%f",(m