Matlab分析NMEA0183报文源代码（含高斯6度带投影计算）

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);