GPS编程

开发平台：

Visual C++

gps_dr_EKF.m：源码内容

							clear all;
clc;
format long ;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%555
%采集数据
Re=6367650; 
g=9.81;               %重力加速度
dt=0.02;              %采样时间0.02秒 
lon0=39.15/180*pi;
lat0=116.06666667/180*pi;
gps=load('E:hejKalmanGPS_.txt');
dr=load('E:hejKalmanDR_.txt');
ekf=load('E:hejKalmanekf.txt');%6000组数据
i=6000/50;     
j=0.02:0.02:i;      
figure
plot(j,dr(:,1),'b',j,gps(:,1),'r',j,ekf(:,1),'y'),xlabel('/t'),ylabel('东向位置');
legend('edr','egps','eKalman');
figure 
plot(j,dr(:,2),'b',j,gps(:,2),'r',j,ekf(:,2),'y');,xlabel('/t'),ylabel('北向位置');
legend('ndr','ngps','nKalman');
figure 
plot(j,dr(:,3),'b',j,gps(:,3),'r',j,ekf(:,3),'y');,xlabel('/t'),ylabel('东向速度');
legend('vedr','vegps','veKalman');
figure 
plot(j,dr(:,4),'b',j,gps(:,4),'r',j,ekf(:,4),'y');,xlabel('/t'),ylabel('北向速度');
legend('vndr','vngps','vnKalman');
figure 
plot(j,dr(:,5)*180/pi,'b'),xlabel('/t'),ylabel('航向角');

						