gps.m

GPS的matlab程序终端轨迹估计

% Jason Seelye
% Senior Project: Autonomous Vehicle with GPS Navigation
% 1/12/01
% ------------------------------------------------------
% This program takes a log file (*.log) input from the user
% and displays a graph of the longitude and lattitude in meters so that
% you may see the path traveled.  Origin of graph is always the starting point
% and the start and end points are labeled.  Program also allows you to select
% two points and it calculates the distance between them. The file must 
% be logged from Ashtech's Evaluate program (written for version 5.04) but may
% work for others.  if the file is G8___001.log then you must
% enter whole file with extension.  This program also creates
% a file called temp1.txt in the directory the *.log file is in.
% This file contains all the "$PASHR,POS" ascii strings from the
% previously loaded *.log file.

% When Lattitude is 40.6995 degrees
% Length of a Degree of Latitude: 111048 meters/deg (1850.8 meters/minute)
%											 364331 feet	    (30.8467 meters/second)
%
% Length of a Degree of Longitude: 84516 meters/deg (1408.6 meters/minute)
%											  277284 feet		 (23.4767 meters/second)

flag=1;
while flag==1,   file = input('What file do you want to plot? ','s');
   FID=fopen(file,'r');
   if FID==-1
      flag=1;
   else
      flag=0;
   end
end
FID2=fopen('temp1.txt','w');
   
while feof(FID)==0,
   line=fgetl(FID);
   length1=max(size(line));
   if length1>52
      if line(8)=='P'
         if line(9)=='O'
            if line(10)=='S'
               fprintf(FID2,'%s\n',line);
            end
         end
      end
   end
end

ST=fclose(FID);
fclose(FID2);
[header,type,raw,sv,utc,lat,latdir,long,longdir,a,b,c,d,e,f,g,h,i,j]=textread...
   ('temp1.txt','%s %s %u %u %f %f %c %f %c %s %s %s %s %s %s %s %s %s %s','delimiter',',');
lat=lat/100;
long=long/100;
latdeg=floor(lat);
longdeg=floor(long);
lat=(lat-latdeg)*100;
long=(long-longdeg)*100;

if longdir=='W'
   long=long*-1;
end
if latdir=='S'
   lat=lat*-1;
end
j=(latdeg+lat/60);
[z,t]=calc(j);
xcf=(sum(t)/(length(t)))/60;
ycf=(sum(z)/(length(z)))/60;
x=(longdeg*60+long);			%Convert to minutes
y=(latdeg*60+lat);			%Convert to minutes
lengthx=length(x);
lengthy=length(y);
x1=x*xcf;
y1=y*ycf;
dx = (x-x(1))*xcf;
dy = (y-y(1))*ycf;
avgx=sum(x1)/length(x1);
avgy=sum(y1)/length(y1);
longdev=sqrt(sum((x1-avgx).^2)/(length(x)-1))
latdev=sqrt(sum((y1-avgy).^2)/(length(y)-1))
%longdev=1/(lengthx-1)*sum((0-xmean)^2)
%latdev=1/(lengthy-1)*sum((0-ymean)^2)
horzdev=sqrt(longdev^2+latdev^2)

distance=sqrt((x1(1)-x1(lengthx))...
   ^2+(y1(1)-y1(lengthy))^2) %Distance from start to finish
figure(1)
plot(dx,dy)
axis equal
grid on
xlabel('Longitude in Meters')
ylabel('Latitude in Meters')
title('GPS Data where North is at top')
text(0,0,'START')
text(dx(length(x)),dy(length(y)),'END')
[a,b]=ginput(2);
sqrt((b(1)-b(2))^2+(a(1)-a(2))^2)
%lat=lat*111048;
%long=long*84516;
%figure(2)
%plot(long,lat)