基于卡尔曼滤波的GPS数据处理研究

首先，研究卡尔曼滤波算法， 进行一维和二维数据的滤波仿真处理，接着使用GPS模块记录车辆的行驶估计，然后对记录的轨迹数据进行卡尔曼滤波处理，分析滤波效果。结果表明，采用卡尔曼滤波的地图匹配实现方案能有效提高车辆GPS位置的定位精度。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   交互多模Kalman滤波在目标跟踪中的应用、GPS测试的折线路段
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ImmKalman


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
clc
[GGATimeLATNLONESTATUSQUANTITYHDOPAltSMAltEma] = textread（‘宿舍——裕民大门22.txt‘‘%s %f %f %s %f %s %f %f %f %f %s %f %s %s‘‘delimiter‘ ‘‘）; %#ok

LAT=LAT（LAT~=0）; 
LON=LON（LON~=0）;

lat = floor（LAT/100） + （LAT - floor（LAT/100）*100）/60;   %换算纬度
lon = floor（LON/100） + （LON - floor（LON/100）*100）/60;   %换算经度

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
T=2;
M=5;
N=900/T;
N1=400/T;
N2=600/T;
N3=610/T;
N4=660/T;
Delta=100;
Rx=zeros（N1）;
Ry=zeros（N1）;
Zx=zeros（NM）;  
Zy=zeros（NM）;



t=2:T:400;
% x0=2000+0*t‘;
x0=lat（t/2:）;
%y0=10000-15*t‘;
y0=lon（t/2:）;


t=402:T:600;
% x1=x0（N1）+0.075*（（t‘-400）.^2）/2;
x1=lat（t/2:）;
% y1=y0（N1）-15*（t‘-400）+0.075*（（t‘-400）.^2）/2;
y1=lon（t/2:）;

t=602:T:610;
%vx=0.075*（600-400）;
% x2=x1（N2-N1）+vx*（t‘-600）;
x2 = lat（t/2:）;
% y2=y1（N2-N1）+0*t‘;
y2 = lon（t/2:）;

t=612:T:660;
%x3=x2（N3-N2）+（vx*（t‘-610）-0.3*（（t‘-610）.^2）/2）;
x3 = lat（t/2:）;
% y3=y2（N3-N2）-0.3*（（t‘-610）.^2）/2;
y3 = lon（t/2:）;

t=662:T:900;
% vy=-0.3*（660-610）;
% x4=x3（N4-N3）+0*t‘;
x4 = lat（t/2:）;
% y4=y3（N4-N3）+vy*（t‘-660）;
y4 = lon（t/2:）;


Rx=[x0;x1;x2;x3;x4];
Ry=[y0;y1;y2;y3;y4];


Mt_Est_Px=zeros（MN）;
Mt_Est_Py=zeros（MN）;

% nx=randn（NM）*Delta;
% ny=randn（NM）*Delta;
% Zx=Rx*ones（1M）+nx;
% Zy=Ry*ones（1M）+ny;

Zx=Rx*ones（1M）;
Zy=Ry*ones（1M）;

for m=1:M
    Mt_Est_Px（m1）=Zx（1m）;
    Mt_Est_Py（m1）=Zx（2m）;
    xn（1）=Zx（1m）;
    xn（2）=Zx（2m）;
    yn（1）=Zy（1m）;
    yn（2）=Zy（2m）;
    phi=[1T00;0100;001T;0001];
    h=[1000;0010];
    g=[T/20;10;0T/2;01];
    q=[Delta^20;0Delta^2];
    vx=（Zx（2）-Zx（1m））/2;
    vy=（Zy（2）-Zy（1m））/2;
    x_est=[Zx（2m）;vx;Zy（2m）;vy];
    p_est=[Delta^2Delta^2/T00;Delta^2/T2*Delta^2/（T^2）00;
        00Delta^2Delta^2/T;00Delta^2/T2*Delta^2/（T^2）];
    Mt_Est_Px（m2）=x_est（1）;
    Mt_Est_Py（m2）=x_est（3）;
    for r=3:N
        z=[Zx（rm）;Zy（rm）];
        if r<20
            x_pre=phi*x_est;
            p_pre=phi*p_est*phi‘;
            k=p_pre*h‘*inv（h*p_pre*h‘+q）;
            x_est=x_pre+k*（z-h*x_pre）;
            p_est=（eye（4）-k*h）*p_pre;
            xn（r）=x_est（1）;
            yn（r）=x_est（3）;
            Mt_Est_Px（mr）=x_est（1）;
            Mt_Est_Py（mr）=x_est（3）;
        else
            if r==20
                X_est=[x_est;0;0];
                P_est=p_est;
                P_est（66）=0;
                for i=1:3
                    Xn_est{i1}=X_est;
                    Pn_est{i1}=P_est;
                end
                u=[0.80.10.1];
            end
            [X_estP_estXn_estPn_estu]=IMM（Xn_estPn_estTzDeltau）;
            xn（r）=X_est（1）;
         

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件    8223212  2015-11-21 17:50  29、基于卡尔曼滤波的GPS数据处理研究\BaiDuMapDemo.rar

     文件      60891  2015-11-21 17:49  29、基于卡尔曼滤波的GPS数据处理研究\BaiDuMap_Winform.rar

     文件    3602352  2017-03-29 21:59  29、基于卡尔曼滤波的GPS数据处理研究\参考文献\GPS定位算法及其在智能公交中的应用.pdf

     文件    1849515  2017-03-29 16:33  29、基于卡尔曼滤波的GPS数据处理研究\参考文献\一种基于GPS采集处理系统的研究_马健.caj

     文件     669184  2017-03-27 09:24  29、基于卡尔曼滤波的GPS数据处理研究\参考文献\基于卡尔曼滤波的目标跟踪研究.doc

     文件    1164370  2017-03-29 19:16  29、基于卡尔曼滤波的GPS数据处理研究\参考文献\基于嵌入式的农机辅助导航系统研究_于占宝.caj

     文件    4227164  2017-03-29 16:21  29、基于卡尔曼滤波的GPS数据处理研究\参考文献\嵌入式农机GPS导航及变量施肥控制技术的研究_王鹏.caj

     文件   13069312  2017-06-29 15:11  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.doc

     文件        314  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\colorschememapping.xml

     文件       9012  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\filelist.xml

     文件       5120  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\header.htm

     文件        645  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image001.wmz

     文件       1064  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image002.gif

     文件        511  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image003.wmz

     文件        200  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image004.gif

     文件        474  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image005.wmz

     文件        199  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image006.gif

     文件        471  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image007.wmz

     文件        139  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image008.gif

     文件        471  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image009.wmz

     文件        214  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image010.gif

     文件        471  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image011.wmz

     文件        199  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image012.gif

     文件        560  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image013.wmz

     文件        342  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image014.gif

     文件        524  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image015.wmz

     文件        216  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image016.gif

     文件        513  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image017.wmz

     文件        545  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image018.wmz

     文件        311  2017-06-29 16:47  29、基于卡尔曼滤波的GPS数据处理研究\基于卡尔曼滤波的GPS数据处理研究.files\image019.gif

............此处省略289个文件信息