clc

clear all

close all

format long g

FILE_NAME = ‘D:/data6/RxCOM6_20190528gps.dat’;

fid = fopen(FILE_NAME,‘rb’);

if(fid == -1)

error(‘open file failure!’);

end

cntL_GGA = 0;

%cntL_RMC = 0;

Failure_cnt = 0;

line_cnt = 1;

GGA_line_cnt = 0; %文件中GGA所在行

RMC_line_cnt = 0;

while(1)

Line = fgetl(fid);

if(Line == -1)

break;

end

if(strncmp(Line,'$GPGGA',6))

cntL_GGA = cntL_GGA + 1; %GGA信息计数

Line = strrep(Line,',',' ');

GGA_temp = sscanf(Line, '%*s %f %f %*s %f %*s %d %d %f %f %*s %f', [1, inf]);

GGA_array(cntL_GGA,:) = GGA_temp;

%高斯6度投影

iPI =pi/180;

ZoneWide = 6; %6度带宽

a = 6378245;

f = 1/298.257223563; %WGS84坐标系参数

ProjNo = floor((floor(GGA_temp(3)/100)+rem(GGA_temp(3),100)/60)/ZoneWide);%投影带号计算

longitude0 = ProjNo * ZoneWide + ZoneWide / 2;

longitude0 = longitude0 * iPI;

latitude0 = 0;

longitude1 = (floor(GGA_temp(3)/100)+rem(GGA_temp(3),100)/60) * iPI; %经度转换为弧度

latitude1 = (floor(GGA_temp(2)/100)+rem(GGA_temp(2),100)/60) * iPI; %纬度转换为弧度

e2 = 2 * f - f * f;

ee = e2 * (1 - e2);

NN = a /sqrt(1 -e2*sin(latitude1)*sin(latitude1));

T = tan(latitude1) *tan(latitude1);

C = ee * cos(latitude1) * cos(latitude1);

A = (longitude1 - longitude0) * cos(latitude1);

%参数转换

M = a * ((1 - e2 / 4 - 3 * e2 * e2 / 64 - 5 * e2 * e2 * e2 / 256) * latitude1 - ...

(3 * e2 / 8 + 3 * e2 * e2 / 32 + 45 * e2 * e2 * e2 / 1024) *...

sin(2 * latitude1) + (15 * e2 * e2 / 256 + 45 * e2 * e2 * e2 / 1024) * ...

sin(4 * latitude1) - (35 * e2 * e2 * e2 / 3072) * sin(6 * latitude1));

xval = NN * (A + (1 - T + C) * A * A * A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee) * A * A * A * A * A / 120);

yval = M + NN *tan(latitude1) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 + (61 - 58 * T + T * T + 600 * C - 330 * ee) * A * A * A * A * A * A / 720);

GGA_array(cntL_GGA,2)= xval+500000;

GGA_array(cntL_GGA,3)= yval;

% %经纬度转空间直角坐标

% a=6378137;

% f=1/298.257223563;

% b = a * (1 - f);

% e = sqrt(aa-bb)/a;

% N = a / sqrt(1 - e * e * sin((floor(GGA_temp(2)/100)+…

% rem(GGA_temp(2),100)/60) *pi/ 180) * sin((floor(GGA_temp(2)/100)+rem(GGA_temp(2),100)/60) * pi / 180));

%

% %GGA语句中的以度/分表示的经纬度转换成度表示

% GGA_array(cntL_GGA,2) = (N +GGA_temp(7))*cos((floor(GGA_temp(2)/100)+rem(GGA_temp(2),100)/60)pi/180)…

% cos((floor(GGA_temp(3)/100)+rem(GGA_temp(3),100)/60)*pi/180);

% GGA_array(cntL_GGA,3) = (N +GGA_temp(7))*cos((floor(GGA_temp(2)/100)+rem(GGA_temp(2),100)/60)pi/180)…

% sin((floor(GGA_temp(3)/100)+rem(GGA_temp(3),100)/60)*pi/180);

% %GGA语句中的以度/分表示的经纬度转换成度表示

% GGA_array(cntL_GGA,2) = floor(GGA_temp(2)/100)+… %度(纬度)

% rem(GGA_temp(2),100)/60 ; %以度表示的分(纬度)

% GGA_array(cntL_GGA,3) = floor(GGA_temp(3)/100)+… %度(经度)

% rem(GGA_temp(3),100)/60 ; %以度表示的分(经度)

GGA_line_cnt(line_cnt) = 1;

end

%if(strncmp(Line,'$BDRMC',6))

%cntL_RMC = cntL_RMC + 1; %RMC信息计数

%Line = strrep(Line,',',' ');

%RMC_temp = sscanf(Line, '%*s %f %s %f %*s %f %*s %f %f %f %f %*s %f', [1, inf]);

%RMC_array(cntL_RMC,:) = RMC_temp;

%RMC_line_cnt(line_cnt) = 1;

%end

line_cnt = line_cnt + 1;

end

GGA_line_cnt = GGA_line_cnt’;

%RMC_line_cnt = RMC_line_cnt’;

%% ----------------------平面轨迹----------------------------

h=figure(1);set(h,‘name’,['平面轨迹 ’ FILE_NAME]);

plot(GGA_array(:,3),GGA_array(:,2),‘b.-’)

xlabel(‘Longitude [deg]’,‘fontsize’,12,‘fontweight’,‘bold’);

ylabel(‘Latitude [deg]’,‘fontsize’,12,‘fontweight’,‘bold’);

title({‘平面轨迹’,FILE_NAME},‘fontsize’,12,‘fontweight’,‘bold’);

grid;

%% -----------------------三维轨迹--------------------------

% h=figure(2);set(h,‘name’,['三纬轨迹 ’ FILE_NAME]);

% plot3(GGA_array(:,3),GGA_array(:,2),GGA_array(:,7),‘b.-’)

% xlabel(‘Longitude [deg]’,‘fontsize’,12,‘fontweight’,‘bold’);

% ylabel(‘Latitude [deg]’,‘fontsize’,12,‘fontweight’,‘bold’);

% zlabel(‘Height [m]’,‘fontsize’,12,‘fontweight’,‘bold’);

% title({‘三维轨迹’,FILE_NAME},‘fontsize’,12,‘fontweight’,‘bold’);

% grid;

%% -------------------------地速---------------------------------

%h=figure(3);set(h,‘name’,[‘地面速度’ FILE_NAME]);

%plot(RMC_array(:,5),‘b.-’);

% xlabel(‘Epoch [s]’,‘fontsize’,12,‘fontweight’,‘bold’);

% ylabel(‘SOG [knot]’,‘fontsize’,12,‘fontweight’,‘bold’);

% title({‘Speed of Ground’,FILE_NAME},‘fontsize’,12,‘fontweight’,‘bold’);

% grid;

%% --------------------------地面航向------------------------------

% h=figure(4);set(h,‘name’,[‘地面航向’ FILE_NAME]);

% plot(RMC_array(:,6),‘b.-’);

% xlabel(‘Epoch [s]’,‘fontsize’,12,‘fontweight’,‘bold’);

% ylabel(‘COG [knot]’,‘fontsize’,12,‘fontweight’,‘bold’);

% title({‘Course of Ground’,FILE_NAME},‘fontsize’,12,‘fontweight’,‘bold’);

% grid;

%% ----------------------------高度----------------------------

h=figure(5);set(h,‘name’,[‘高程’ FILE_NAME]);

plot(GGA_array(:,7),‘b.-’);

hold on;

plot(GGA_array(:,4),‘b.-’);

hold on;

plot(GGA_array(:,5),‘b.-’);

hold on;

xlabel(‘Epoch [s]’,‘fontsize’,12,‘fontweight’,‘bold’);

ylabel(‘Height[m]’,‘fontsize’,12,‘fontweight’,‘bold’);

title({‘height’,FILE_NAME},‘fontsize’,12,‘fontweight’,‘bold’);

grid;

fclose(fid);