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; %% -----------------------3차원 궤적set(h,'name', ['3위 궤적'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({'3차원 궤적', 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; %% --------------------------지상 항로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);