GPS-/HW5.m at main · pyopyojihun/GPS- · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171

%2) 문제 해결
%2-1) Matlab 코드

%% 작업공간 비우기
close all; clearvars;

%% 참값 정의 - 각자 함수 [xyz2gd] 작성 필요
truePos = [-3026338.575 4067414.201 3857230.279];
trueLLH = xyz2gd(truePos);

%% 추정 결과 c1(gs) | c2(X) | c3(Y) | c4(Z)
out = [ 104412.005  -3026341.961    4067417.404     3857232.962 ;
        104413.005  -3026341.933    4067417.255     3857232.942 ;
        104414.005  -3026341.908    4067417.299     3857232.891 ;
        104415.005  -3026341.877    4067417.276     3857232.843 ;
        104416.005  -3026341.836    4067417.204     3857232.691 ;
        104417.005  -3026341.869    4067417.211     3857232.657 ;
        104418.005  -3026341.892    4067417.322     3857232.746 ;
        104419.005  -3026341.855    4067417.209     3857232.581 ;
        104420.005  -3026341.762    4067417.018     3857232.373 ;
        104421.005  -3026341.681    4067416.973     3857232.225 ;
        104422.005  -3026341.748    4067417.059     3857232.379 ;
        104423.005  -3026341.762    4067416.934     3857232.365 ;
        104424.005  -3026341.729    4067416.966     3857232.351 ;
        104425.005  -3026341.726    4067416.937     3857232.369 ;
        104426.005  -3026341.754    4067417.073     3857232.453 ;
        104427.005  -3026341.787    4067416.962     3857232.441 ;
        104428.005  -3026341.768    4067416.909     3857232.442 ;
        104429.005  -3026341.782    4067416.805     3857232.478 ;
        104430.005  -3026341.814    4067416.885     3857232.446 ;
        104431.005  -3026341.783    4067416.896     3857232.367 ;
        104432.005  -3026341.741    4067416.952     3857232.387 ;
        104433.005  -3026341.715    4067416.968     3857232.406 ;
        104434.005  -3026341.650    4067417.059     3857232.373 ;
        104435.005  -3026341.751    4067417.029     3857232.340 ;
	    104436.005  -3026341.672    4067416.888     3857232.363 ;
        104437.005  -3026341.696    4067416.939     3857232.458 ;
	    104438.005  -3026341.687    4067416.955     3857232.365 ;
        104439.005  -3026341.635    4067416.923     3857232.345 ;
        104440.005  -3026341.560    4067416.910     3857232.319 ;
        104441.005  -3026341.548    4067416.932     3857232.364];

%% dXYZ 계산 - 각자 함수 [xyz2topo] 작성 필요
dXYZ = out(:,2:4) - truePos;
dNEV = xyz2topo(dXYZ, trueLLH(1), trueLLH(2));

%% 수평, 수직 오차 그래프

subplot(2, 1, 1) % 그래프를 나타내는 화면을 두개(2*1)로 나눈다. 그 중 1번째
x = dNEV(:, 2); % x = dE 값으로 한다.
y = dNEV(:, 1); % y = dN 값으로 한다.
plot(x, y, 'rx') % 빨간색 '*'을 사용하여 그래프 그리기
xlabel('dE'); ylabel('dN') % y축 제목
title('수평 오차 그래프') % 그래프 제목
grid on

subplot(2, 1, 2) % 두개로 나눈 화면 중 두번
xx = out(:, 1); % xx = time 값으로 한다.
yy = dNEV(:, 3); % yy = dV 값으로 한다.
plot(xx, yy, 'bx') % 파란색 '*'을 이용해서 그래프를 그린다.
xlabel('t'); ylabel('dV')
title('수직 오차 그래프')
grid on

% %% 수평 및 수직 오차 그래프그리기
% subplot(4,1,1)
% plot(dNEV(:,1),'ro')
% title('N 방향 수평오차')
% subplot(4,1,2)
% plot(dNEV(:,2),'bo')
% title('E 방향 수평오차')
% subplot(4,1,3)
% plot(dNEV(:,3),'go')
% title('V 방향 수직오차 ')
% subplot(4,1,4)
% plot(dNEV(:,1),dNEV(:,2),'bo')
% title('2차원 수평오차')


%% 각각의 오차 및 오차 통계 구하기

fprintf(1,"deltaX : %10.8f deltaY : %10.8f deltaZ : %10.8f \n", dXYZ(:,1),dXYZ(:,2),dXYZ(:,3) )
fprintf(1,"deltaN : %10.8f deltaE : %10.8f deltaV : %10.8f \n", dNEV(:,1),dNEV(:,2),dNEV(:,3) )

%% 각 오차의 평균값 및 표준편차 값
fprintf(1,"deltaX의 평균값 :  %10.8f\n", mean(dXYZ(:,1)))
fprintf(1,"deltaY의 평균값 :  %10.8f\n", mean(dXYZ(:,2)))
fprintf(1,"deltaZ의 평균값 :  %10.8f\n", mean(dXYZ(:,3)))
fprintf(1,"deltaX의 표준편차 :  %10.8f\n", std(dXYZ(:,1)))
fprintf(1,"deltaY의 표준편차 :  %10.8f\n", std(dXYZ(:,2)))
fprintf(1,"deltaZ의 표준편차 :  %10.8f\n", std(dXYZ(:,3)))

fprintf(1,"deltaN의 평균값 :  %10.8f\n", mean(dNEV(:,1)))
fprintf(1,"deltaE의 평균값 :  %10.8f\n", mean(dNEV(:,2)))
fprintf(1,"deltaV의 평균값 :  %10.8f\n", mean(dNEV(:,3)))
fprintf(1,"deltaN의 표준편차 :  %10.8f\n", std(dNEV(:,1)))
fprintf(1,"deltaE의 표준편차 :  %10.8f\n", std(dNEV(:,2)))
fprintf(1,"deltaV의 표준편차 :  %10.8f\n", std(dNEV(:,3)))

%% 각 오차의 최댓값, 최솟값


fprintf(1,"delta X, delta Y, delta Z 각각의 최댓값과 최솟값\n")
fprintf(1,"최댓값: %10.8f \n",max(dXYZ))
fprintf(1,"최소값: %10.8f \n",min(dXYZ))

fprintf(1,"delta N, delta E, delta V 각각의 최댓값과 최솟값\n")
fprintf(1,"최댓값: %10.8f\n ",max(dNEV))
fprintf(1,"최소값: %10.8f\n ",min(dNEV))


%XYZ 좌표계를 측지좌표계(geodetic)로 바꾸는 xyz2gd 함수는 다음과 같다.

function [gd] = xyz2gd(xyz)

%% Input
X = xyz(1); Y = xyz(2); Z = xyz(3);

%% GRS80: a = 6378137.0; f = 1/298.257222101;
a = 6378137.0; f = 1/298.257223563;
b = a*(1. - f);
aSQ = a^2;
bSQ = b^2;
eSQ = (aSQ - bSQ)/aSQ;

%% Computation of Longitude
Lon = atan2(Y,X)*180/pi;

if Lon > 180.
    Lon = Lon - 360.;
elseif Lon < -180.
    Lon = Lon + 360.;
end

%% Iterative Computations of Latitude and Height
p = sqrt(X^2 + Y^2); %STEP 1
q = 0;

Phi0 = atan2(Z*inv(1 - eSQ),p); %STEP 2

while (q ~= 1)
    N0 = aSQ/sqrt(aSQ*(cos(Phi0))^2 + bSQ*(sin(Phi0))^2); %STEP 3


    h = p/cos(Phi0)-N0; %STEP 4
    Phi = atan2(Z*inv(1 - eSQ*(N0/(N0 + h))),p); %STEP 5

    if abs(Phi - Phi0) <= 1e-13 %STEP 6
        break;
    else
        Phi0 = Phi;
    end
end

Lat = Phi*180/pi;

%% Output
gd = [Lat Lon h];
end