【文章內容簡介】 考橢球面的距離。 空間直角坐標系 空間大地坐標系 針對定位的各種動態(tài)誤差的特性，對消除動態(tài)誤差尋求解決的方法主要集中在以下兩方面:（1）采用差分定位，組合定位的方法從接收機本身精度考慮，在經費允許的情況下選擇精度較高的GPS接收機。當然，這需要對成本和精度之間做出權衡。從定位方式考慮，可采用差分定位，但差分定位對硬件的依賴度很高，且更適合加入其他的輔助手段以達到高精度的要求。基于以上考慮，組合導航技術被引入，用來輔助GPS單獨導航，在信號微弱或是失鎖情況下提高更準確的定位信息，可以大大減小系統(tǒng)本身所帶來的各種誤差源。（2）濾波方法 在動態(tài)定位中，濾波技術是影響系統(tǒng)性能即定位精度的一個關鍵因素。對于接收機觀測所帶來的各種誤差，通過適當的濾波方法，可以大大減小誤差。由于卡爾曼濾波最初提出的濾波理論只適應線性系統(tǒng)，而許多導航定位系統(tǒng)的量測模型都具有不同程度的非線性，因而就要求人們去研究和探索適合于GPS導航系統(tǒng)特點的濾波理論和方法。本文利用卡爾曼濾波法和遞歸最小二次方算法對接收機接收的初始數據進行濾波處理，驗證了該方法對提高單機動態(tài)定位精度的有效性。第3章 濾波算法研究 信號分類濾波就是從混合在一起的諸多信號中提取出所需要的信號。信號是傳遞和運載信息的時間或空間函數。信號有兩類，其中一類信號的變化規(guī)律是既定的，可以表示為一個確定的時間函數或空間函數，如調幅廣播中的載波信號、階躍信號、脈寬固定的矩形脈沖信號等，它們都具有確定的頻譜，這類信號稱為確定性信號。另一類信號沒有既定的變化規(guī)律，在相同的初始條件和環(huán)境條件下，信號的每次實現都不一樣，如GPS的定位誤差、隨機海浪等，它們沒有確定的頻譜，這類信號稱為隨機信號。由于確定性信號具有確定的頻譜，所以可根據各信號頻帶的不同，設置具有相應頻率特性的濾波器，如低通、高通、帶通、帶阻濾波器，使有用信號無衰減地通過，使干擾信號受到抑制。這類濾波器可用物理方法實現，即模擬濾波器，也可用計算機通過算法實現，即數字濾波器。對確定性信號的濾波處理也稱常規(guī)濾波。隨機信號沒有確定的頻譜，無法用常規(guī)濾波提取或抑制信號，但隨機信號具有確定的功率譜。所以可根據有用信號和干擾信號的功率譜設計濾波器，維納濾波是解決此類問題的方法之一。但設計維納濾波器須作功率譜分解，只適用于被處理信號為平穩(wěn)的過程。而卡爾曼濾波將狀態(tài)空間的概念引入隨機估計理論中，將信號過程視為白噪聲作用下的一個線性系統(tǒng)的輸出，用狀態(tài)方程來描述這種輸入輸出關系。估計過程中利用系統(tǒng)狀態(tài)方程、觀測方程和白噪聲激勵的統(tǒng)計特性構成濾波算法，所以適用于隨機過程。 信號的估計準則 在實際的控制工程中，比如在航天控制領域和工業(yè)控制領域，為了對系統(tǒng)進行精確的控制，我們必須了解系統(tǒng)運行的準確狀態(tài)，但是有一些狀態(tài)是很難直接通過儀器儀表觀測，例如航空器的姿態(tài)信息，位置信息等，我們只能通過測量其它數據來間接估計系統(tǒng)狀態(tài)，因此必須要找到一種合適的計算方法來間接估計系統(tǒng)的狀態(tài)量，即信號估計準則。目前估計中常用的三類準則是直接誤差準則、誤差函數矩準則和直接概率準則。一個系統(tǒng)當前時刻的系統(tǒng)狀態(tài)取決于一下三種主要因素：（1）在設計控制系統(tǒng)時采用控制算法，例如在工業(yè)控制領域常用的 PID 算法、神經網絡算法等；（2）系統(tǒng)運行的初始狀態(tài)，比如在各種控制電路中，電容的初始值直接的影響系統(tǒng)最后的響應；（3）系統(tǒng)當前時刻以前的噪聲干擾，干擾有很多種，我們用來測量數據的電子儀器就會受到電子元器件熱噪聲的干擾，這一種隨時間劇烈變化，電子元器件的老化也會影響系統(tǒng)，這種干擾隨時間緩慢變化，也可以歸為噪聲信號，我們把這一類干擾信號統(tǒng)一稱為噪聲。 噪聲在實際工程中無法避免，它是隨機過程，無論使用何種方法都無法對它進行準確的估計，系統(tǒng)狀態(tài)由于受到了噪聲的影響，也可以認為是一種隨機過程，因此，我們不能夠完全準確的估計系統(tǒng)狀態(tài)的精確值，只能盡量提高精確度，例如使用精確的估計方法，使得估計狀態(tài)無窮的逼近真實狀態(tài)。當然，噪聲并不是完全沒有規(guī)律，它有一定的統(tǒng)計特性，無論是在時間域的分布特性，還是在頻域的功率譜特性，通過深入分析這些噪聲的統(tǒng)計特性，采取相應地信號估計理論來預測系統(tǒng)的狀態(tài)，使得狀態(tài)的估計值可以更加準確地接近狀態(tài)的真實值。 信號估計理論本質上是一種數據處理技術。對于工程中經常出現的高斯隨機過程，最優(yōu)估計準則就是線性最小方差估計。但是在實際工程中，經常采用卡爾曼濾波來對 GPS 系統(tǒng)數學模型進行數據處理，卡爾曼濾波器也是高斯過程最優(yōu)濾波的一種有效算法，其實卡爾曼濾波器本質上就是線性最小方差估計方法，線性最小方差估計的理論構成了卡爾曼濾波的全部基礎。假設系統(tǒng)運行的狀態(tài)向量為，觀測向量為，在運行時系統(tǒng)狀態(tài)向量X和觀測向量Z會受到高斯隨機噪聲和干擾的影響，因此也可以認為狀態(tài)向量X和觀測向量Z也是一種高斯隨機過程，線性最小方差估計就是要找到一種關于觀測向量線性的估計向量,使得狀態(tài)估計誤差的均方誤差矩陣 達到最小，則就是系統(tǒng)狀態(tài)向量X的線性最小方差估計。按照上述線性最小方差估計的定義，將代入采用配方的方法可以求出： （） （） （）這種線性估計也是無偏估計，即 （）上述方程就是我們在推導卡爾曼濾波公式過程中所需要的基本理論和方法。線性最小方差估計具有很多特殊的優(yōu)點，這些特點將在后面的卡爾曼濾波計算中可以得到體現，它們使得計算過程得以最大程度的簡化，因此，最小方差估計也被認為是線性估計方法中的最佳估計準則。（RLS）算法 遞歸最小二次方（RLS）算法，又稱LS算法。是一種基于梯度最速下降的搜索算法，它是沿誤差超曲面梯度(誤差超平面)最大的方向，向下搜索逐漸逼近誤差超曲面梯度為0的平衡點。這些平衡點可能是不穩(wěn)定的，也可能是局部極小點。通過不斷反向修正權值，最終達到誤差終值的要求?？疾橛^測信號： ()式中，s(t)是感興趣的信號；n(t)為噪聲；y(t)為含噪聲的觀測數據。該算法用指數加權的誤差平方和作為代價函數，即有 ()式中，加權因子，其作用是對離n時刻越近的誤差加比較大的權重，而對離n時刻越遠的誤差加比較小的權重。換句話說，對各個時刻的誤差具有一定的遺忘作用，故稱之為遺忘因子。從這個意義上講，=1相當于各時刻的誤差被“一視同仁”。即無任何遺忘功能。此時，指數加權的最小二乘方退化為一般的最小二乘方。反之，若=0，則只有現在時刻的誤差起作用，而過去時刻的誤差完全被遺忘，不起任何作用，在非平穩(wěn)環(huán)境中，為了跟蹤變化的系統(tǒng)，這兩個極端的遺忘因子值是不適合的。估計誤差定義為： （）式中代表時刻的期望響應。稱為濾波器在i時刻的后驗誤差，為i時刻的步長。因此，加權誤差平方和的完整表達式為: （）它是的函數。由易得，其解為 （）式中， （） （）的遞推公式： （）式中稱為增益向量。定義為： （）綜上所述，可以得到RLS算法如下：步驟1 初始化 ，其中是一個很小的值。步驟2 更新 n=1，2，...... 卡爾曼(Kalman)算法卡爾曼()博士在上個世紀六十年代提出了一種線性最優(yōu)估計濾波理論，這套理論直接以卡爾曼博士本人的名字直接命名，稱為卡爾曼濾波理論，目前,卡爾曼濾波理論在航空航天以及工業(yè)控制領域已經獲得了廣泛的應用。 由于運動體在運動過程中，其導航精度受到諸多因素的影響。在有些特殊情況，如太陽、大氣的擾動及接收機附近的強干擾影響下，當信噪比小至接收極限時，也會影響GPS的定位精度，甚至無法接收信號。還有地球不是平面體，是一個橢球體，參數計算也會帶來一定的誤差。位置的偏差對運動軌跡、運動方向的影響就比較大，不能反映真實的運動軌跡、運動方向，所以算法性能測試軟件必須采取一定方法減少定位誤差。GPS平滑濾波方法是消除GPS定位隨機誤差的一種重要方法，能有效提高衛(wèi)星定位精度。可假設GPS輸出數據中存在加性平穩(wěn)噪聲，因此可用卡爾曼濾波、自適應濾波和預測處理等方法來提高估計量的精度?？柭鼮V波(kalman)是一個不斷預測和修正的過程，無需存儲大量的觀測數據，當得到新的觀測值時，它只是根據前一個估計值和最后一個觀測數據來估計信號的當前值，特別適宜實時數據處理。卡爾曼濾波作為一種以最小均方誤差為準則的最佳線性估計或濾波，其在使用時并不限于平穩(wěn)隨機過程，也適用于隨機過程，這就使得其實際應用性得到很大的提高?？柭鼮V波從與被提取信號有關的量測量中通過算法估計出所需信號。其中被估計信號是由白噪聲激勵引起的隨機響應，激勵源與響應之間的傳遞結構(系統(tǒng)方程)已知，量測量與被估計量之間的函數關系(量測方程)也已知。估計過程中利用了如下信息:系統(tǒng)方程、量測方程、白噪聲激勵的統(tǒng)計特性、量測誤差的統(tǒng)計特性。由于所用信息都是時域內的量，所以卡爾曼濾波器是在時域內設計的，且適用于多維情況，這就完全避免了維納濾波器在頻域內設計遇到的限制和障礙，適用范圍遠比維納濾波器廣。因此，卡爾曼濾波有如下特點。①卡爾曼濾波處理的對象是隨機信號。②。③系統(tǒng)的白噪聲激勵和量測噪聲并不是需要濾除的對象，它們的統(tǒng)計特性正是估計過程中需要利用的信息。④算法是遞推的，且使用狀態(tài)空間法在時域內設計濾波器，所以卡爾曼濾波適用于對多維隨機過程的估計。⑤采用動力學方程即狀態(tài)方程描述被估計量的動態(tài)變化規(guī)律，被估計量的動態(tài)統(tǒng)計信息由激勵白噪聲的統(tǒng)計信息和動力學方程確定。由于激勵白噪聲是平穩(wěn)過程，動力學方程己知，所以被估計量既可以是平穩(wěn)的，也可以是非平穩(wěn)的，即卡爾曼濾波也適用于非平穩(wěn)過程。⑥卡爾曼濾波具有連續(xù)型和離散型兩類算法，離散型算法可直接在數字計算機上實現?？柭鼮V波技術是專門針對離散線性系統(tǒng)而提出來的，因此也被稱為離散卡爾曼濾波技術，現在所談論的卡爾曼濾波就是專指離散卡爾曼濾波。對于前面介紹的線性離散動態(tài)系統(tǒng)，卡爾曼濾波估計就是要通過觀測向量以及系統(tǒng)的初始狀態(tài)，來得到系統(tǒng)狀態(tài)向量的一個無偏估計，同時對 有著如下的要求：(1) 是系統(tǒng)初始向量以及觀測量，的線性函數。 (2) 狀態(tài)估計誤差的協(xié)方差矩陣最小。 因此從定義上看，正如前面一節(jié)提到的那樣，卡爾曼濾波技術實際上就是線性最小方差估計的其中一種。下面以線性離散系統(tǒng)為例來介紹卡爾曼濾波器的大致結構。設隨機線性離散系統(tǒng)的狀態(tài)方程和觀測方程為： （） （）式中：是維非奇異狀態(tài)一步轉移矩陣；是維系統(tǒng)過程噪聲輸入矩陣；是維觀測矩陣；是維系統(tǒng)過程噪聲序列；為維系統(tǒng)隨機觀測噪聲序列。系統(tǒng)的過程噪聲序列和觀測噪聲序列為零均值的白噪聲隨機序列，即： 其中是系統(tǒng)過程噪聲的階非負定方差矩陣，是觀測噪聲的 階對稱正定方差矩陣，的定義式為：： 卡爾曼濾波器的遞推公式系統(tǒng)狀態(tài)方程系統(tǒng)觀測方程噪聲統(tǒng)計特性濾波公式預測公式濾波增益濾波誤差協(xié)方差預測誤差協(xié)方差初始條件以上為離散型卡爾曼濾波基本方程，只要給定初值見和，根據K時刻的量測，就可以遞推計算得到K時刻的狀態(tài)估計(K=1，2，…)。根據上述的濾波公式，增益矩陣與初始均方誤差陣、動態(tài)噪聲協(xié)方差陣及觀測噪聲協(xié)方差陣之間的存在著下列關系:①當、 、(K=1，2，3…)同時乘以相同數量時，不變。②當增大時，變小，這從直觀上可以理解，因為如果觀測噪聲增大，則增益應該取的小一些，以減弱觀測噪聲的影響。③當變小或變小或兩者都變小時，變小。這從直觀上看也是自然的，因為變小表示初始估計較好，變小表示狀態(tài)轉移的隨機波動小，所以新加進的觀測對狀態(tài)預測的校正影響減弱，于是增益矩陣應當變小。，該濾波器的輸入是系統(tǒng)狀態(tài)的觀測值，輸出的是系統(tǒng)狀態(tài)的估計值。 卡爾曼濾波器結構圖： 卡爾曼濾波的兩個計算回路和兩個更新回路從圖中可明顯看出卡爾曼濾波具有兩個計算回路:增益計算回路和濾波計算回路，其中增益計算回路是獨立回路，而濾波計算回路依賴于增益計算回路。在一個濾波周期內，從卡爾曼濾波在使用系統(tǒng)信息和量測信息的先后次序來看，卡爾曼濾波具有兩個明顯的信息更新過程:時間更新過程和量測更新過程。狀態(tài)一步預測方程說明根據K1時刻的狀態(tài)估計預測K時刻狀態(tài)估計的方法，一步預測誤差方差對這種預測的質量優(yōu)劣作了定量描述。著兩式的計算中僅使用了與系統(tǒng)動態(tài)特性有關的信息，如一步轉移矩陣、噪聲輸入矩陣、過程噪聲方差矩陣等。從時間的推移過程來看，這兩式將時間從K1時刻推進到K時刻，描述了卡爾曼濾波的時間更新過程。卡爾曼濾波方程的其余諸式用來計算對時間更新值的修正量，該修正量由時間更新的質量優(yōu)劣()、量測信息的質量優(yōu)劣()、量測與狀態(tài)的關系()以及K時刻具體的量測值所確定，所有這些方程圍繞一個目的，即正確合理地利用量測，這一過程描述了卡爾曼濾波的量測更新過程。 交互式多模型（IMM）算法模型是IMM算法的基礎，在IMM算法中使用比較廣泛的有基于勻速直線（英文縮寫為C