1 簡介

ITS(智能交通系統(tǒng))是一個開放的復雜巨系統(tǒng),是新興的交叉學科研究領域。ITS是將先進的信息處理技術、數(shù)據(jù)通信技術、電子控制技術以及計算機處理技術等有效的綜合運用于整個運輸管理體系,從而建立起大范圍內(nèi)、全方位發(fā)揮作用的實時、準確、高效的管理運輸系統(tǒng)。而車輛導航系統(tǒng)時ITS應用研究的一個主要方面,是集定位系統(tǒng)、地理信息系統(tǒng)、數(shù)據(jù)庫查詢系統(tǒng)、數(shù)字蜂窩通訊技術等于一體的綜合系統(tǒng)。近年來,如何使GPS定位技術與INS慣性導航系統(tǒng)更好的結合起來已經(jīng)成為車輛導航系統(tǒng)研究中的一個重要內(nèi)容。 本文首先介紹了GPS定位的基本原理。然后重點研究的內(nèi)容是通過信息融合,使得在定位衛(wèi)星信號接收差的場合工作更可靠。在GPS/INS組合導航系統(tǒng)中利用卡爾曼濾波去估計系統(tǒng)的各種狀態(tài),并用狀態(tài)的估計值去校正系統(tǒng)?？柭鼮V波算法的基本思想是：采用信號與噪聲的狀態(tài)空間模型,利用前一時刻的估計值和現(xiàn)在時刻的觀測值來更新對狀態(tài)變量的估計,求現(xiàn)在時刻的估計值。它是一種遞推估計算法,通過處理一系列帶有誤差的實際測量數(shù)據(jù)而得到的物理參數(shù)的最佳估算。最大的特點是能夠剔除隨機干擾噪聲,從而獲取逼近真實情況的有用信息。最后將本文提出的算法與現(xiàn)有的算法進行比較,經(jīng)過Kalman濾波處理后的軌跡比原始軌跡更平滑,精度也有效提高,在GPS中斷的情況下,軌跡的連續(xù)性也得到了保證。

2 部分代碼

%GPS/INS組合導航%量測信號： ? 位置%INS輸出數(shù)據(jù)由simulink計算得出clearclc%得到軌跡信號load ode500Re ? ? = 6378245; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? %地球長半徑%真實軌跡a_R ? = yout(:,1:3);v_R ? ?= yout(:,4:6);p_R ? = yout(:,7:9);%加噪聲后的INS計算結果a_ins = yout(:,10:12);v_ins = yout(:,13:15);p_ins = yout(:,16:18);quat ?= yout(:,19:22); ? ?%姿態(tài)四元數(shù)Fn ? ?= yout(:,23:25); ? %地理系下的比力%慣導相關的噪聲統(tǒng)計數(shù)據(jù)Q_wg ?= (0.04/(57*3600))^2; ? ? ? ? %陀螺馬氏過程Q_wr ?= (0.01/(57*3600))^2; ? ? ? ?%陀螺白噪聲Q_wa ?= (1e-3)^2; ? %加計馬氏過程Q ? ? = diag([Q_wg Q_wg Q_wg, ?Q_wr Q_wr Q_wr, ?Q_wa Q_wa Q_wa]);Tg ? ? = 300*ones(3,1);Ta ? ? = 1000*ones(3,1);%得到帶誤差的GPS輸出信號p_gps_sample = p_R(1:10:end,:);n = size(p_gps_sample,1);p_error(:,1:2) ?= 30*randn(n,2)/Re; ? ?p_error(:,3) = 30*randn(n,1); ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?%位置誤差p_gps = p_gps_sample+p_error; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?%加入位置誤差R = diag(std(p_error).^2); ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? %計算測量噪聲方差R%卡爾曼濾波tao= 1; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?%濾波步長a_ins_sample ? ? = a_ins(1:10:end,:);v_ins_sample ? ? = v_ins(1:10:end,:);p_ins_sample ? ? = p_ins(1:10:end,:);a_R_sample ? ? ? = a_R(1:10:end,:);v_R_sample ? ? ? = v_R(1:10:end,:);p_R_sample ? ? ? = p_R(1:10:end,:);Dp= p_ins_sample-p_gps; ? ? ? ? ? ? ? ? ? ? ? ? ? %INS與GPS輸出的位置差值a = a_ins_sample; v = v_ins_sample;p = p_ins_sample;quat0 = quat(1:10:end,:);Fn0 = Fn(1:10:end,:);[Error_a, Error_v, Error_p, PP] = kalman_GPS_INS_position_sp_NFb(Dp, v, p, quat0, Fn0, Q, R, Tg, Ta, tao); ? %得到位置，速度誤差誤差估計值a_estimate ? ? ? = a(1:size(Error_a,1),:)-Error_a;v_estimate ? ? ? = v(1:size(Error_v,1),:)-Error_v;p_estimate ? ? ? = p(1:size(Error_p,1),:)-Error_p;n = size(p_estimate,1); ? %行數(shù)%位置誤差比較figure ?subplot(3,1,1)plot((1:n),(p_R_sample(1:n,1)-p(1:n,1))*6e6,'k',(1:n),(p_R_sample(1:n,1)-p_estimate(:,1))*6e6,'r') ?%黑線-濾波前的誤差 ? 紅線-濾波后的誤差xlabel('時間，單位s')subplot(3,1,2)plot((1:n),(p_R_sample(1:n,2)-p(1:n,2))*6e6,'k',(1:n),(p_R_sample(1:n,2)-p_estimate(:,2))*6e6,'r') ?%黑線-濾波前的誤差 ? 紅線-濾波后的誤差ylabel('位置誤差，單位m')subplot(3,1,3)plot((1:n),p_R_sample(1:n,3)-p(1:n,3),'k',(1:n),p_R_sample(1:n,3)-p_estimate(:,3),'r') ? ? ? ? ? ? ?%黑線-濾波前的誤差 ? 紅線-濾波后的誤差xlabel('黑線-濾波前的INS誤差 ? 紅線-濾波后的誤差')%速度誤差比較figure ?subplot(3,1,1)plot((1:n),v_R_sample(1:n,1)-v(1:n,1),'k',(1:n),v_R_sample(1:n,1)-v_estimate(:,1),'r') ? ? ? ? ? ? ? %黑線-濾波前的誤差 ? 紅線-濾波后的誤差xlabel('時間，單位s')subplot(3,1,2)plot((1:n),v_R_sample(1:n,2)-v(1:n,2),'k',(1:n),v_R_sample(1:n,2)-v_estimate(:,2),'r') ? ? ? ? ? ? ?%黑線-濾波前的誤差 ? 紅線-濾波后的誤差ylabel('速度誤差，單位m/s')subplot(3,1,3)plot((1:n),v_R_sample(1:n,3)-v(1:n,3),'k',(1:n),v_R_sample(1:n,3)-v_estimate(:,3),'r') ? ? ? ? ? ? ?%黑線-濾波前的誤差 ? 紅線-濾波后的誤差xlabel('黑線-濾波前的INS誤差 ? 紅線-濾波后的誤差')%位置誤差figure ?subplot(3,1,1)xlabel('時間，單位s')plot((1:n),(p_R_sample(1:n,1)-p_estimate(:,1))*6370000,'r') ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?%紅線-濾波后的誤差subplot(3,1,2)plot((1:n),(p_R_sample(1:n,2)-p_estimate(:,2))*6370000,'r') ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?%紅線-濾波后的誤差ylabel('位置誤差，單位m')subplot(3,1,3)plot((1:n),p_R_sample(1:n,3)-p_estimate(:,3),'r') ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?%紅線-濾波后的誤差xlabel('濾波后的位置誤差')%速度誤差figure ?subplot(3,1,1)plot((1:n),v_R_sample(1:n,1)-v_estimate(:,1),'r') ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?% 紅線-濾波后的誤差xlabel('時間，單位s')subplot(3,1,2)plot((1:n),v_R_sample(1:n,2)-v_estimate(:,2),'r') ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?%紅線-濾波后的誤差ylabel('速度誤差，單位m/s')subplot(3,1,3)plot((1:n),v_R_sample(1:n,3)-v_estimate(:,3),'r') ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?% 紅線-濾波后的誤差xlabel('濾波后的速度誤差')

3 仿真結果

4 參考文獻

[1]王欣明. GPS/INS組合導航系統(tǒng)中卡爾曼濾波算法的研究與實現(xiàn). Diss. 北京交通大學, 2012.

博主簡介：擅長智能優(yōu)化算法、神經(jīng)網(wǎng)絡預測、信號處理、元胞自動機、圖像處理、路徑規(guī)劃、無人機等多種領域的Matlab仿真，相關matlab代碼問題可私信交流。

部分理論引用網(wǎng)絡文獻，若有侵權聯(lián)系博主刪除。