-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
test_geodetic.c
241 lines (174 loc) · 6.92 KB
/
test_geodetic.c
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
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
/*
http://en.wikipedia.org/wiki/Geodetic_system
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_double.h"
#include "math/pprz_geodetic_float.h"
#include "math/pprz_geodetic_int.h"
//#define DEBUG 1
#define CM_OF_M(_m) ((_m)*1e2)
#define M_OF_CM(_cm) ((_cm)/1e2)
#define EM7RAD_OF_RAD(_r) (_r*1e7)
#define RAD_OF_EM7RAD(_r) (_r/1e7)
static void test_floats(void);
static void test_doubles(void);
static void test_enu_of_ecef_int(void);
static void test_ned_to_ecef_to_ned(void);
static void test_enu_to_ecef_to_enu( void );
/*
* toulouse lat 43.6052765, lon 1.4427764, alt 180.123019274324 -> x 4624497.0 y 116475.0 z 4376563.0
*/
int main(int argc, char** argv) {
test_floats();
test_doubles();
// test_enu_of_ecef_int();
// test_ned_to_ecef_to_ned();
// test_enu_to_ecef_to_enu();
return 0;
}
static void test_floats(void) {
printf("\n--- enu_of_ecef float ---\n");
// struct LlaCoor_f ref_coor;
// ref_coor.lat = RAD_OF_DEG(43.605278);
// ref_coor.lon = RAD_OF_DEG(1.442778);
// ref_coor.alt = 180.0;
struct EcefCoor_f ref_coor = { 4624497.0 , 116475.0, 4376563.0};
printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z);
struct LtpDef_f ltp_def;
ltp_def_from_ecef_f(<p_def, &ref_coor);
printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt);
struct EcefCoor_f my_ecef_point = ref_coor;
struct EnuCoor_f my_enu_point;
enu_of_ecef_point_f(&my_enu_point, <p_def, &my_ecef_point);
printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n",
my_ecef_point.x, my_ecef_point.y, my_ecef_point.z,
my_enu_point.x, my_enu_point.y, my_enu_point.z );
printf("\n");
}
static void test_doubles(void) {
printf("\n--- enu_of_ecef double ---\n");
// struct LlaCoor_f ref_coor;
// ref_coor.lat = RAD_OF_DEG(43.605278);
// ref_coor.lon = RAD_OF_DEG(1.442778);
// ref_coor.alt = 180.0;
struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0};
printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z);
struct LtpDef_d ltp_def;
ltp_def_from_ecef_d(<p_def, &ref_coor);
printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt);
struct EcefCoor_d my_ecef_point = ref_coor;
struct EnuCoor_d my_enu_point;
enu_of_ecef_point_d(&my_enu_point, <p_def, &my_ecef_point);
printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n",
my_ecef_point.x, my_ecef_point.y, my_ecef_point.z,
my_enu_point.x, my_enu_point.y, my_enu_point.z );
printf("\n");
}
static void test_enu_of_ecef_int(void) {
printf("\n--- enu_of_ecef int ---\n");
struct EcefCoor_f ref_coor_f = { 4624497.0 , 116475.0, 4376563.0};
struct LtpDef_f ltp_def_f;
ltp_def_from_ecef_f(<p_def_f, &ref_coor_f);
struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_f.x)),
rint(CM_OF_M(ref_coor_f.y)),
rint(CM_OF_M(ref_coor_f.z))};
printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z);
struct LtpDef_i ltp_def_i;
ltp_def_from_ecef_i(<p_def_i, &ref_coor_i);
printf("lla0 : (%d %d %d) (%f,%f,%f)\n", ltp_def_i.lla.lat, ltp_def_i.lla.lon, ltp_def_i.lla.alt,
DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)),
DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)),
M_OF_CM((double)ltp_def_i.lla.alt));
#define STEP 1000.
#define RANGE 100000.
double sum_err = 0;
struct FloatVect3 max_err;
FLOAT_VECT3_ZERO(max_err);
struct FloatVect3 offset;
for (offset.x=-RANGE; offset.x<=RANGE; offset.x+=STEP) {
for (offset.y=-RANGE; offset.y<=RANGE; offset.y+=STEP) {
for (offset.z=-RANGE; offset.z<=RANGE; offset.z+=STEP) {
struct EcefCoor_f my_ecef_point_f = ref_coor_f;
VECT3_ADD(my_ecef_point_f, offset);
struct EnuCoor_f my_enu_point_f;
enu_of_ecef_point_f(&my_enu_point_f, <p_def_f, &my_ecef_point_f);
#if DEBUG
printf("ecef to enu float : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n",
my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z,
my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z );
#endif
struct EcefCoor_i my_ecef_point_i = { rint(CM_OF_M(my_ecef_point_f.x)),
rint(CM_OF_M(my_ecef_point_f.y)),
rint(CM_OF_M(my_ecef_point_f.z))};;
struct EnuCoor_i my_enu_point_i;
enu_of_ecef_point_i(&my_enu_point_i, <p_def_i, &my_ecef_point_i);
#if DEBUG
// printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z);
printf("ecef to enu int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n",
M_OF_CM((double)my_ecef_point_i.x),
M_OF_CM((double)my_ecef_point_i.y),
M_OF_CM((double)my_ecef_point_i.z),
M_OF_CM((double)my_enu_point_i.x),
M_OF_CM((double)my_enu_point_i.y),
M_OF_CM((double)my_enu_point_i.z));
#endif
float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x);
if (fabs(ex) > max_err.x) max_err.x = fabs(ex);
float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y);
if (fabs(ey) > max_err.y) max_err.y = fabs(ey);
float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z);
if (fabs(ez) > max_err.z) max_err.z = fabs(ez);
sum_err += ex*ex + ey*ey + ez*ez;
}
}
}
double nb_samples = (2*RANGE / STEP + 1) * (2*RANGE / STEP + 1) * (2*RANGE / STEP + 1);
printf("enu_of_ecef int/float comparison:\n");
printf("error max (%f,%f,%f) m\n", max_err.x, max_err.y, max_err.z );
printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples );
printf("\n");
}
void test_ned_to_ecef_to_ned( void ) {
#if 0
struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0};
printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z);
struct LtpDef_d ltp_def;
ltp_def_from_ecef_d(<p_def, &ref_coor);
struct EcefCoor_d ecef_p1 = ref_coor;
struct NedCoor_d ned_p1;
ned_of_ecef_point_d(&ned_p1, <p_def, &ecef_p1);
printf("ecef to ned : (%f,%f,%f) -> (%f,%f,%f)\n",
ecef_p1.x, ecef_p1.y, ecef_p1.z,
ned_p1.x, ned_p1.y, ned_p1.z );
struct EcefCoor_d ecef_p2;
ecef_of_ned_point_d(&ecef_p2, <p_def, &ned_p1);
printf("ned to ecef : (%f,%f,%f) -> (%f,%f,%f)\n",
ned_p1.x, ned_p1.y, ned_p1.z,
ecef_p2.x, ecef_p2.y, ecef_p2.z);
printf("\n");
#endif
}
void test_enu_to_ecef_to_enu( void ) {
struct EcefCoor_f ref_coor = { 4624497.0 , 116475.0, 4376563.0};
printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z);
struct LtpDef_f ltp_def;
ltp_def_from_ecef_f(<p_def, &ref_coor);
struct EcefCoor_f ecef_p1 = ref_coor;
struct EnuCoor_f enu_p1;
enu_of_ecef_point_f(&enu_p1, <p_def, &ecef_p1);
printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n",
ecef_p1.x, ecef_p1.y, ecef_p1.z,
enu_p1.x, enu_p1.y, enu_p1.z );
#if 0
struct EcefCoor_f ecef_p2;
ecef_of_enu_point_f(&ecef_p2, <p_def, &enu_p1);
printf("enu to ecef : (%f,%f,%f) -> (%f,%f,%f)\n",
enu_p1.x, enu_p1.y, enu_p1.z,
ecef_p2.x, ecef_p2.y, ecef_p2.z);
printf("\n");
#endif
}