gps濾波演算法

『壹』 卡爾曼濾波理解與實現

本文為離散卡爾曼濾波演算法的一 一個簡明教程，從演算法思想、實現過程、理論推導和程序實現四個方面闡述和分析了卡爾曼濾波演算法。

XU Ruilin完成本教程主要部分的編寫，WANG Xuejun完成第3節的編寫，ZHU Ximin完成2.2節的編寫，WEN Shuhan完成2.3節的編寫，MAO Bo完成全文整理、修訂和排版。

卡爾曼濾波（Kalman Filtering）及其一系列的優化和改進演算法是目前在求解運動狀態推算問題上最為普遍和高效的方法。 魯道夫·卡爾曼 （Rudolf Emil Kalman） 在NASA埃姆斯研究中心訪問時，發現他的方法適用於解決阿波羅計劃的軌跡預測問題。阿波羅飛船的導航電腦就是使用這種濾波器進行軌跡預測。

卡爾曼濾波尤其適用於動態系統，這種方法對於內存要求極低而運算速度快，且能夠保持較好的計算精度，這使得這種方法非常適合解決實時問題和應用於嵌入式系統，也就是說，卡爾曼濾波天然的適用於解決艦艇指控系統的航跡推算問題。在接下來的內容里，我們將逐步領會卡爾曼濾波的這些絕佳特點。

不過，現在我們先從復雜的艦艇航跡推算問題中解脫出來，從一個更加熟悉和簡單的問題中來理解這個濾波演算法的思想、過程和演算法。

假設有一輛無人車WALL-E，需要導引它從A點到達B點，共有兩種手段（ 圖1 ）：

顯然，兩種方法都有一定的誤差。如果單獨採用某一種方法進行定位，WALL-E在誤差的影響下將無法到達B點。因此，需要將兩種方法結合起來，得到一個更加精確的結果，這就是卡爾曼濾波要解決的問題。

卡爾曼濾波方法如何看待我們的問題呢？在探究這個問題之前，我們先對問題進行抽象，並用數學語言來描述我們的問題。

我們用矢量 來描述WALL-E的運動狀態，這個列矢量 包括位置矢量 和速度矢量 兩個分量。在WALL-E的問題上，我們現在不知道位置 和速度 的准確值，但是知道WALL-E的運動模型滿足 狀態方程 ，定位的方法，也即觀測WALL-E運動狀態的方法滿足 觀測方程 . 當然，我們也知道，這兩種方法都存在一定的誤差 ，那麼我們的問題就可以轉化為一個優化問題——

在這一優化問題中，目標函數是要使預測（估計）誤差最小，同時約束於估計方法 和 的條件下。在卡爾曼濾波中，我們的估計原則（也就是最小化估計誤差的原則）是 最小方差無偏估計 [1] ，我們將通過後面的過程分析來說明這一點。

在我們正式開始引入公式分析卡爾曼濾波問題之前，我們還必須解決一個問題------把連續的線性系統離散化，也就是將連續時域問題轉化為時間序列問題。當然，目前我們只討論線性系統的情況，關於非線性系統問題，我們有擴展卡爾曼濾波（Extended Kalman Filtering, EKF）和無跡卡爾曼濾波（Unscented Kalman Filtering, UKF）兩種方法來求解。

補充內容------連續線性時變系統的離散化
設連續線性時變系統的時域狀態方程為

若采樣周期為 ，則從時刻 到時刻 ，有

令 ， ，則離散化的狀態方程為

通過對線性系統的離散化處理，我們現在可以考慮每一個時刻WALL-E的運動狀態。接下來，我們將用 來表示在 時刻運動狀態的最優估計值；用 表示用 時刻對 時刻的狀態預測值；用 表示對 時刻綜合預測和觀測兩種方法的最優估計值。

在估計WALL-E位置的問題上，假定我們已經知道它是勻速直線運動，WALL-E身上還攜帶有一個GPS感測器可以提供它的位置信息，WALL-E在前進過程中可能會遇到一些情況，比如停止前進或是受到風的影響。

加入我們已知的是WALL-E上一個時刻的最佳估計狀態，即k-1時刻的位置和速度，要求的是下一時刻即k時刻的最佳估計狀態，即k時刻的位置和速度，我們可以發現有兩種方法可以得到它的k時刻的狀態：

一種是通過WALL-E設定程序計算得到下一秒的狀態，比如現在設定是勻速直線運動，那麼下一秒的速度應該是恆定不變的，而位置則是在上一秒位置的基礎上加上時間乘以速度即一秒內走過的路程，但是現實生活中並不是理想的，機器人會受到摩擦力、風力等的影響，當然也可能會有頑皮的小孩擋住他前進的道路，這些因素使得WALL-E在k時的真實狀態與我們計算得到的數據有所不同。

另一種是通過WALL-E所攜帶的GPS來確定它的位置，因為GPS是測量出的就是WALL-E的實時狀態，因此它比較准確。但是GPS測量k時刻的狀態有兩個問題，一是GPS只能測出WALL-E的位置，而測不出它的速度；二是GPS感測器測量的時候也會有儀器的誤差，只能說它是比較准確的，比較接近真實值的。

那麼接下來問題來了，我們如何得到k時刻WALL-E的真實狀態呢？

我們將第一種方法得到的狀態值稱為預測值，第二種方法得到的狀態值稱為測量值，對汽車的最佳估計就是將這兩部分信息結合起來，盡量的去逼近k時刻的真實值。

下面再深入一些思考，怎麼將這兩部分結合起來？

在初始時間k-1， 是WALL-E的最佳估計值，WALL-E其實可以是估計值附近的任何位置，並且這種不確定性由該概率密度函數描述。WALL-E最有可能在這個分布的平均值附近。在下一個時間，估計的不確定性增加，用一個更大的方差表示，這是因為在時間步驟k-1和k之間，WALL-E可能收到了風力的影響，或者腳可能向前滑了一點，因此，它可能已經行進了與模型預測的距離不同的距離。

WALL-E位置的另一個信息來源來自測量，方差表示誤差測量的不確定性，真正的位置同樣可以是平均值附近的任何位置。

預測值和測量值，對WALL-E的最佳估計是將這兩部分信息結合起來，將兩個概率函數相乘得到另一個高斯函數，該估計值的方差小於先前估計值，並且該概率密度函數的平均值為我們提供了WALL-E位置的最佳估計。

以下，我們將進行e的運算推導

設：

則有實際目標變數的表達式：

數學模型中目標變數的表達式：

實際模型中測量變數的表達式：

數學模型中測量變數的表達式：

將目標變數的實際值和估計值相減：

將上述方程帶入誤差e的表達式，我們可得出誤差e的解析解：

從推導結果中我們不難看出，估計值和實際值的誤差隨時間呈指數形式變化，當（F-KH）<1時，隨著時間的推移，會無限趨近於零，也就是意味著估計值和實際值相吻合。這就是為什麼卡爾曼濾波器可以完美預測出目標狀態值的原理。

在估計WALL-E位置的問題上，我們不知道位置 和速度 的准確值，但是我們可以給出一個估計區間（ 圖5.a ）。卡爾曼濾波假設所有的變數是隨機的且符合高斯分布（正態分布）。每個變數有一個均值 和一個方差 （ 圖5.b ）。而 圖5.c 則表示速度和位置是相關的。

假如我們已知上一個狀態的位置值，現在要預測下一個狀態的位置值。如果我們的速度值很高，我們移動的距離會遠一點。相反，如果速度慢，WALL-E不會走的很遠。這種關系在跟蹤系統狀態時很重要，它給了我們更多的信息：一個觀測值告訴我們另一個觀測值可能是什麼樣子。這就是卡爾曼濾波的目的------從所有不確定信息中提取有價值的信息。

根據數理統計知識，我們知道這種兩個觀測值（隨機變數）之間的關系可以通過一個協方差矩陣

描述（ 圖6 ）。

我們假設系統狀態的分布為 高斯分布（正態分布） ，所以在 時刻我們需要兩個信息：最佳預估值 及其協方差矩陣 （如式(2)所示）。

下一步，我們需要通過 時刻的狀態來預測 時刻的狀態。請注意，我們不知道狀態的准確值，但是我們的預測函數並不在乎，它僅僅是對 時刻所有可能值的范圍進行預測轉移，然後得出一個k時刻新值的范圍。在這個過程中，位置 和速度 的變化為

我們可以通過一個狀態轉移矩陣 來描述這個轉換關系

同理，我們更新協方差矩陣 為

到目前為止，我們考慮的都是勻速運動的情況，也就是系統沒有對WALL-E的運動狀態進行控制的情況。那麼，如果系統對WALL-E進行控制，例如發出一些指令啟動或者制動輪子，對這些額外的信息，我們可以通過一個向量 來描述這些信息，並將其添加到我們的預測方程里作為一個修正。假如我們通過發出的指令得到預期的加速度 ，運動狀態方程就更新為

引入矩陣表示為

式中 稱為控制矩陣， 稱為控制向量（例如加速度 ）。當然，如果沒有任何外界動力影響的系統，可以忽略這一部分。

我們增加另一個細節，假如我們的預測轉換矩陣不是100%准確呢，會發生什麼？如果狀態只會根據系統自身特性演變，那樣將不會有任何問題。如果所有外界作用力對系統的影響可以被計算得十分准確，那樣也不會有任何問題。但是如果有些外力我們無法預測，例如我們在跟蹤一個四軸飛行器，它會受到風力影響；或者在跟蹤一個輪式機器人，輪子可能會打滑，地面上的突起會使它減速。我們無法跟蹤這些因素，而這些不確定事件發生時，預測方程將會失靈。因此，我們將這些不確定性統一建模，在預測方程中增加一個不確定項。

通過這種方式，使得原始狀態中的每一個點可以都會預測轉換到一個范圍，而不是某個確定的點（ 圖7.a ）。 可以這樣描述------ 中的每個點移動到一個符合方差 的高斯分布里（ 圖7.b ）。換言之，我們把這些不確定因素描述為方差為 的高斯雜訊，並用 表示。這樣就會產生一個新的高斯分布，方差不同，但是均值相同（ 圖7.c ）。

通過對 的疊加擴展，得到完整的預測轉換方程為

新的預測轉換方程只是引入了已知的系統控制因素。新的不確定性可以通過之前的不確定性計算得到。到這里，我們得到了一個模糊的估計范圍------通過 和 描述的范圍。

我們之前的工作仍然是在使用運動模型一種方法來估計系統的狀態，現在，我們要把另一種方法，也就是觀測（本問題中為GPS定位）考慮進來，以進一步修正對運動狀態的估計（ 圖8 ）。

我們用矩陣 來描述觀測方法的作用，於是有

再加入觀測雜訊 ，觀測方程為

從控制論的角度出發，我們定義新息（也即觀測值與預測值的誤差）為

當然我們也知道，觀測本身也會存在誤差，比如本問題中的GPS定位精度僅有10m. 因此，我們用矩陣 來描述這種不確定性（ 圖10 及 圖11.a ）。

這時，我們新息的協方差為

現在我們需要把兩種方法得到的可能性融合起來（ 圖11.b ）。對於任何狀態，有兩個可能性：1. 感測器的觀測值更接近系統真實狀態；2. 模型推算的估計值更接近系統真實狀態。如果有兩個相互獨立的獲取系統狀態的方式，並且我們想知道兩者都准確的概率值，於是我們可以通過加權來解決更相信誰的問題（ 圖11.c ）。

我們現在知道，系統模型的狀態預測 與對系統的狀態觀測 服從高斯分布，把這個問題抽象一下就是——

根據我們的一個估計准則------ 最小方差估計 ，那麼這個問題可以轉化為優化問題求解

求導數（差分）得

則 ，從而

當維度高於一維時，我們用矩陣來描述，有

這里的 稱為 卡爾曼增益 （Kalman Gain），也就是我們在解決更信任哪種方法時的偏向程度。

如果我們從兩個獨立的維度估計系統狀態，那麼根據系統模型的預測為

通過感測器的觀測為

我們結合著兩種方法得到

由 可知，卡爾曼增益為

將 約去（ 中也含有 項），得

此時的卡爾曼增益實際為

我們最後再來驗證一下 估計的無偏性 ——

這里我們設 時刻的真值為 ，由於

由於 （ 從初值而來的無偏傳遞性 ）可知 ，即卡爾曼濾波滿足無偏估計准則。顯然，其中要求系統雜訊和觀測雜訊是不相關、零期望的白雜訊，且是線性系統，初始時刻的狀態估計是無偏的。當這些條件不能滿足時，卡爾曼濾波的估計結果是有偏的。

到這里，我們已經獲得了卡爾曼濾波的全部要素。我們可以把整個過程總結為3個基本假設

假設一 和 都是零均值高斯白雜訊，也即 ，

假設二 與 無關，也即

假設三 系統初值 的均值和方差已知，且 與 均不相關。

以及5個基本方程 方程一 狀態預測

方程二 協方差預測

方程三 卡爾曼增益

『貳』 哪位大神有GPS與捷聯慣導組合導航的卡爾曼濾波演算法的matlab模擬程序

在下面的模擬的代碼中，理想的觀測量不是真實數據，而是自生成的正弦波數據，在真實的應用場景中，應該是一系列的參考數據。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 卡爾曼濾波器在INS-GPS組合導航中應用模擬
% Author : lylogn
% Email : [email protected]
% Company: BUAA-Dep3
% Time : 2013.01.06
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 參考文獻:
% [1]. 鄧正隆. 慣導技術, 哈爾濱工業大學出版社.2006.
clear all;
%% 慣性-GPS組合導航模型參數初始化
we = 360/24/60/60*pi/180; %地球自轉角速度，弧度/s
psi = 10*pi/180; %psi角度 / 弧度
Tge = 0.12;
Tgn = 0.10;
Tgz = 0.10; %這三個參數的含義詳見參考文獻
sigma_ge=1;
sigma_gn=1;
sigma_gz=1;
%% 連續空間系統狀態方程
% X_dot(t) = A(t)*X(t) + B(t)*W(t)
A=[0 we*sin(psi) -we*cos(psi) 1 0 0 1 0 0;
-we*sin(psi) 0 0 0 1 0 0 1 0;
we*cos(psi) 0 0 0 0 1 0 0 1;
0 0 0 -1/Tge 0 0 0 0 0;
0 0 0 0 -1/Tgn 0 0 0 0;
0 0 0 0 0 -1/Tgz 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;]; %狀態轉移矩陣
B=[0 0 0 sigma_ge*sqrt(2/Tge) 0 0 0 0 0;
0 0 0 0 sigma_gn*sqrt(2/Tgn) 0 0 0 0;
0 0 0 0 0 sigma_gz*sqrt(2/Tgz) 0 0 0;]';%輸入控制矩陣
%% 轉化為離散時間系統狀態方程
% X(k+1) = F*X(k) + G*W(k)
T = 0.1;
[F,G]=c2d(A,B,T);
H=[1 0 0 0 0 0 0 0 0;
0 -sec(psi) 0 0 0 0 0 0 0;];%觀測矩陣
%% 卡爾曼濾波器參數初始化
t=0:T:50-T;
length=size(t,2);
y=zeros(2,length);
Q=0.5^2*eye(3); %系統雜訊協方差
R=0.25^2*eye(2); %測量雜訊協方差
y(1,:)=2*sin(pi*t*0.5);
y(2,:)=2*cos(pi*t*0.5);
Z=y+sqrt(R)*randn(2,length); %生成的含有雜訊的假定觀測值，2維
X=zeros(9,length); %狀態估計值，9維
X(:,1)=[0,0,0,0,0,0,0,0,0]'; %狀態估計初始值設定
P=eye(9); %狀態估計協方差
%% 卡爾曼濾波演算法迭代過程
for n=2:length
X(:,n)=F*X(:,n-1);
P=F*P*F'+ G*Q*G';
Kg=P*H'/(H*P*H'+R);
X(:,n)=X(:,n)+Kg*(Z(:,n)-H*X(:,n));
P=(eye(9,9)-Kg*H)*P;
end
%% 繪圖代碼
figure(1)
plot(y(1,:))
hold on;
plot(y(2,:))
hold off;
title('理想的觀測量');
figure(2)
plot(Z(1,:))
hold on;
plot(Z(2,:))
hold off;
title('帶有雜訊的觀測量');
figure(3)
plot(X(1,:))
hold on;
plot(X(2,:))
hold off;
title('濾波後的觀測量');

『叄』 卡爾曼濾波演算法的發展歷史如何

全球定位系統（GPS）是新一代的精密衛星導航定位系統。由於其全球性、全天候以及連續實時三維定位等特點，在軍事和民用領域得到了廣泛的發展。近年來，隨著科學技術的發展，GPS導航和定位技術已向高精度、高動態的方向發展。但是由於GPS定位包含許多誤差源，尤其是測量隨機誤差和衛星的幾何位置誤差，使定位精度受到影響。利用傳統的方法很難消除。而GPS動態濾波是消除GPS定位隨機誤差的重要方法，即利用特定的濾波方法消除各種隨機誤差，從而提高GPS導航定位精度。 經典的最優濾波包括：Wiener濾波和Kalman濾波。由於Wiener濾波採用頻域法，作用受到限制；而Kalman濾波採用時域狀態空間法，適合於多變數系統和時變系統及非平穩隨機過程，且由於其遞推特點容易在計算機上實現，因此得到了廣泛的應用。為此，本文對Kalman濾波方法進行了深入的研究，並取得了一些成果。 本文首先概述了GPS的組成、應用及最新動態。在此基礎上介紹了GPS的導航定位原理，給出了衛星可見性演算法、選星演算法及定位演算法。然後介紹了卡爾曼濾波的基本原理，在此基礎上對動態用戶的飛行軌跡進行了模擬，對「singer」模型下的8狀態和11狀態卡爾曼濾波演算法進行了模擬分析，同時對「當前」統計模型下11狀態卡爾曼濾波演算法進行了模擬分析，並對濾波前後的定位精度進行了比較。在此基礎上，就如何提高濾波器的動態性能作者提出了改進演算法，即自適應卡爾曼濾波演算法、帶漸消因子的優化演算法及改進的優化演算法，並分別進行了模擬分析。最後作者將卡爾曼濾波演算法分別應用於GPS/DR和GPS/INS組合導航定位系統中，並分別對這兩種系統進行了建模和模擬分析，取得了較理想的結果。 本文的研究工作，對改進傳統的濾波方法有一定的參考和應用價值，並對卡爾曼濾波方法在提高GPS動態導航定位精度方面的應用起到積極的促進作用。

『肆』 GPS定位產生偏離，怎麼破

GPS處於樹木遮擋、高樓林立、高架橋以及隧道和地下停車場等弱信號環境中的時候，發送的位置信息跟實際位置是有所偏差的。您是做一個什麼應用呢？車載定位導航的話，可以選擇一款採用能滿足日益增長的車載導航對弱信號環境的高精度定位需求，即使在隧道、車庫等環境下也能為車輛提供高精度定位的定位導航模塊，像SKYLAB採用GNSS/INS組合導航定位技術的車載組合導航模塊SKM-4DX，充分利用慣性導航系統和衛星導航系統優點，基於最優估計演算法—卡爾曼濾波演算法融合兩種導航演算法，獲得最優的導航結果；尤其是當衛星導航系統無法工作時，利用慣性導航系統使得導航系統繼續工作，保證導航系統的正常工作，提高了系統的穩定性和可靠性。

『伍』 車載導航怎麼調音效

一般的2.1、5.1音響裡面的0.1這個聲道就是指超重低音，通常是用來接有源低音炮或者無源低音炮的，營造更好的視聽效果。低音不用調，其他適當降一點，最後調音量。

車載導航是利用車載GPS（全球定位系統）配合電子地圖來進行的，它能方便且准確地告訴駕駛者去往目的地的最短或者最快路徑，是駕駛員的好幫手。

位於地球上空的同步衛星最初是用於軍事和航空導航。美國政府在80年代時放寬了對同步衛星的使用限制，為其後來的廣泛採用，打開了一個新天地。隨後而來的商用通訊衛星，更是大大的增加了通訊衛星的准確性和覆蓋度。

系統功能

構成車載定位導航系統的各個部分：

（1）能夠導航的電子地圖。

（2）地圖數據的搜索和處理。

（3）定位模塊。

（4）地圖配比。

（5）規定行走路徑。

（6）路徑引導。

（7）地圖實時顯示模塊 。

（8）人機交互介面。

（9）無線通信。

功能模塊簡介

GPS導航衛星准確定位、最佳路徑搜索、自動語音提示。

導航模式多種目的輸入，五種以上路徑檢索定製模式，站點設置模式，地圖定位模式。

全國地圖提供全國數千城市電子地圖、提供最新地圖支持，導航更精確讓您暢行無阻。

路徑規劃提供出發地和目的地的路徑規劃，設置經由地和迴避地功能。

語音提示親切真人語音提示，行駛中自動播報道路名稱。

音樂欣賞視聽享受、解除疲勞。

文件管理強大的資源管理功能。

『陸』 什麼是INS/GPS組合導航系統

組合導航是指綜合各種導航設備，由監視器和計算機進行控制的導航系統。INS/GPS組合導航系統是指基於GPS衛星導航系統和慣性導航系統的組合導航系統。

組合導航系統模塊充分利用GNSS衛星導航系統和慣性導航系統優點，基於最優估計算——卡爾曼濾波演算法融合兩種導航演算法，獲得最優的導航結果；尤其是當衛星導航系統無法工作時，利用慣性導航系統使得導航系統繼續工作，保證導航系統的正常工作，提高了系統的穩定性和可靠性。

組合導航模塊SKM-4DX採用GNSS(BDS/GPS系統聯合定位)/INS（慣性導航）組合導航定位技術，憑借高精度六軸慣性器件和成熟的慣性演算法，無需里程計或速度信號接入，且無嚴格安裝要求，即使在隧道、車庫等弱信號環境下也能為車輛提供高精度的定位模塊。