RflySimSDK v4.01
RflySimSDK说明文档
载入中...
搜索中...
未找到
rfly_wgs84.h
1#pragma once
2
3/*
4 * Header file for wgs_conversions.cpp
5 *
6 * Dan Pierce
7 * 2017-03-13
8 */
9#include <iostream>
10#include <math.h>
11#define M_PI_R 3.14159265358979323846
12
13typedef double array_type[3];
14typedef double matrix_type[3][3];
15
16/*! Primary class for wgs_conversions */
17class WgsConversions{
18
19 public:
20 WgsConversions(){}
21 ~WgsConversions(){}
22
23 //------------------------------------------------------------------------------------------------
24 // WgsConversions::enu2lla [Public] --- convert from (East,North,Up) to (Lat,Long,Alt)
25 //------------------------------------------------------------------------------------------------
26 bool enu2lla(double lla[3], const double enu[3], const double ref_lla[3]){
27 double ref_xyz[3],diff_xyz[3],xyz[3],R[3][3],Rt[3][3];
28
29 // First, calculate the xyz of reflat, reflon, refalt
30 if (!lla2xyz(ref_xyz,ref_lla))
31 return 0;
32
33 rot3d(R, ref_lla[0], ref_lla[1]);
34
35 transposeMatrix(Rt,R);
36
37 matrixMultiply(diff_xyz,Rt,enu);
38
39 xyz[0] = diff_xyz[0] + ref_xyz[0];
40 xyz[1] = diff_xyz[1] + ref_xyz[1];
41 xyz[2] = diff_xyz[2] + ref_xyz[2];
42
43 if(!xyz2lla(lla,xyz))
44 return 0;
45
46 return 1;
47 }
48
49 //------------------------------------------------------------------------------------------------
50 // WgsConversions::lla2enu [Public] --- convert from (Lat,Long,Alt) to (East,North,Up)
51 //------------------------------------------------------------------------------------------------
52 bool lla2enu(double enu[3], const double lla[3], const double ref_lla[3]){
53
54 double xyz[3];
55
56 if(!lla2xyz(xyz,lla))
57 return 0;
58
59 if(!xyz2enu(enu,xyz,ref_lla))
60 return 0;
61
62 return 1;
63 }
64
65 //------------------------------------------------------------------------------------------------
66 // WgsConversions::xyz2lla [Public] --- convert from (ECEF X, ECEF Y, ECEF Z) to (Lat,Long,Alt)
67 //------------------------------------------------------------------------------------------------
68 bool xyz2lla(double lla[3], const double xyz[3]){
69
70 //This dual-variable iteration seems to be 7 or 8 times faster than
71 //a one-variable (in latitude only) iteration. AKB 7/17/95
72
73 double A_EARTH = 6378137.0;
74 double flattening = 1.0 / 298.257223563;
75 double NAV_E2 = (2.0 - flattening) * flattening; // also e^2
76 double rad2deg = 180.0 / M_PI_R;
77
78 if ((xyz[0] == 0.0) & (xyz[1] == 0.0)) {
79 lla[1] = 0.0;
80 } else {
81 lla[1] = atan2(xyz[1], xyz[0]) * rad2deg;
82 }
83
84 if ((xyz[0] == 0.0) & (xyz[1] == 0.0) & (xyz[2] == 0.0)) {
85 std::cout << "WGS xyz at center of earth" << std::endl;
86 return 0;
87 } else {
88 // Make initial lat and alt guesses based on spherical earth.
89 double rhosqrd = xyz[0] * xyz[0] + xyz[1] * xyz[1];
90 double rho = sqrt(rhosqrd);
91 double templat = atan2(xyz[2], rho);
92 double tempalt = sqrt(rhosqrd + xyz[2] * xyz[2]) - A_EARTH;
93 double rhoerror = 1000.0;
94 double zerror = 1000.0;
95
96 int iter = 0; // number of iterations
97
98 // % Newton's method iteration on templat and tempalt makes
99 // % the residuals on rho and z progressively smaller. Loop
100 // % is implemented as a 'while' instead of a 'do' to simplify
101 // % porting to MATLAB
102
103 while ((abs(rhoerror) > 1e-6) | (abs(zerror) > 1e-6)) {
104 double slat = sin(templat);
105 double clat = cos(templat);
106 double q = 1.0 - NAV_E2 * slat*slat;
107 double r_n = A_EARTH / sqrt(q);
108 double drdl = r_n * NAV_E2 * slat * clat / q; // d(r_n)/d(latitutde)
109
110 rhoerror = (r_n + tempalt) * clat - rho;
111 zerror = (r_n * (1.0 - NAV_E2) + tempalt) * slat - xyz[2];
112
113 // % -- -- -- --
114 // % | drhoerror/dlat drhoerror/dalt | | a b |
115 // % Find Jacobian | |=| |
116 // % | dzerror/dlat dzerror/dalt | | c d |
117 // % -- -- -- --
118
119 double aa = drdl * clat - (r_n + tempalt) * slat;
120 double bb = clat;
121 double cc = (1.0 - NAV_E2)*(drdl * slat + r_n * clat);
122 double dd = slat;
123
124 //Apply correction = inv(Jacobian)*errorvector
125
126 double invdet = 1.0 / (aa * dd - bb * cc);
127 templat = templat - invdet * (+dd * rhoerror - bb * zerror);
128 tempalt = tempalt - invdet * (-cc * rhoerror + aa * zerror);
129
130 iter++;
131
132 if (iter>20){
133 std::cout << "xyz2lla could not converge" << std::endl;
134 return 0;
135 }
136 }
137
138 lla[0] = templat*rad2deg;
139 lla[2] = tempalt;
140 }
141 return 1;
142 }
143
144 //------------------------------------------------------------------------------------------------
145 // WgsConversions::lla2xyz [Public] --- convert from (Lat,Long,Alt) to (ECEF X, ECEF Y, ECEF Z)
146 //------------------------------------------------------------------------------------------------
147 bool lla2xyz(double xyz[3], const double lla[3]){
148
149 if ((lla[0] < -90.0) | (lla[0] > +90.0) | (lla[1] < -180.0) | (lla[1] > +360.0)){
150 std::cout << "WGS lat or WGS lon out of range" << std::endl;
151 return 0;
152 }
153
154 double A_EARTH = 6378137.0;
155 double flattening = 1.0/298.257223563;
156 double NAV_E2 = (2.0-flattening)*flattening; // also e^2
157 double deg2rad = M_PI_R/180.0;
158
159 double slat = sin(lla[0]*deg2rad);
160 double clat = cos(lla[0]*deg2rad);
161 double r_n = A_EARTH/sqrt(1.0 - NAV_E2*slat*slat);
162 xyz[0] = (r_n + lla[2])*clat*cos(lla[1]*deg2rad);
163 xyz[1] = (r_n + lla[2])*clat*sin(lla[1]*deg2rad);
164 xyz[2] = (r_n*(1.0 - NAV_E2) + lla[2])*slat;
165
166 return 1;
167 }
168
169 //------------------------------------------------------------------------------------------------
170 // WgsConversions::enu2xyz [Public] --- convert from (East,North,Up) to (ECEF X, ECEF Y, ECEF Z)
171 //------------------------------------------------------------------------------------------------
172 bool enu2xyz(double xyz[3], const double enu[3], const double ref_lla[3]){
173 double lla[3];
174
175 // first enu2lla
176 if(!enu2lla(lla,enu, ref_lla))
177 return 0;
178
179 // then lla2xyz
180 if(!lla2xyz(xyz,lla))
181 return 0;
182
183 return 1;
184 }
185
186 //------------------------------------------------------------------------------------------------
187 // WgsConversions::xyz2enu [Public] --- convert from (ECEF X, ECEF Y, ECEF Z) to (East,North,Up)
188 //------------------------------------------------------------------------------------------------
189 bool xyz2enu(double enu[3], const double xyz[3], const double ref_lla[3]){
190
191 double ref_xyz[3],diff_xyz[3],R[3][3];
192
193 // First, calculate the xyz of reflat, reflon, refalt
194 if (!lla2xyz(ref_xyz,ref_lla))
195 return 0;
196
197 //Difference xyz from reference point
198 diff_xyz[0] = xyz[0] - ref_xyz[0];
199 diff_xyz[1] = xyz[1] - ref_xyz[1];
200 diff_xyz[2] = xyz[2] - ref_xyz[2];
201
202 rot3d(R, ref_lla[0], ref_lla[1]);
203
204 matrixMultiply(enu,R,diff_xyz);
205
206 return 1;
207 }
208
209 //--------------------------------------------------------------------------------------------------------------
210 // WgsConversions::xyz2enu_vel [Public] --- convert velocities from (ECEF X, ECEF Y, ECEF Z) to (East,North,Up)
211 //--------------------------------------------------------------------------------------------------------------
212 void xyz2enu_vel(double enu_vel[3], const double xyz_vel[3], const double ref_lla[3]){
213
214 double R[3][3];
215
216 rot3d(R, ref_lla[0], ref_lla[1]);
217
218 matrixMultiply(enu_vel,R,xyz_vel);
219
220 }
221
222 //--------------------------------------------------------------------------------------------------------------
223 // WgsConversions::enu2xyz_vel [Public] --- convert velocities from (East,North,Up) to (ECEF X, ECEF Y, ECEF Z)
224 //--------------------------------------------------------------------------------------------------------------
225 void enu2xyz_vel(double xyz_vel[3], const double enu_vel[3], const double ref_lla[3]){
226
227 double R[3][3],Rt[3][3];
228
229 rot3d(R, ref_lla[0], ref_lla[1]);
230
231 transposeMatrix(Rt,R);
232
233 matrixMultiply(xyz_vel,Rt,enu_vel);
234
235 }
236
237 //--------------------------------------------------------------------------------------------------------------------------------
238 // WgsConversions::xyz2enu_cov [Public] --- convert position/velocity covariance from (ECEF X, ECEF Y, ECEF Z) to (East,North,Up)
239 //--------------------------------------------------------------------------------------------------------------------------------
240 void xyz2enu_cov(double enu_Cov[3][3], const double xyz_Cov[3][3], const double ref_lla[3]){
241
242 double R[3][3],Rt[3][3],Tmp[3][3];
243
244 rot3d(R, ref_lla[0], ref_lla[1]);
245
246 transposeMatrix(Rt,R);
247
248 matrixMultiply(Tmp,xyz_Cov,Rt); // Tmp = xyz_cov*R'
249
250 matrixMultiply(enu_Cov,R,Tmp); // enu_cov = R*xyz_cov*R'
251
252 }
253
254 void xyz2enu_cov(double enu_cov[9], const double xyz_cov[9], const double ref_lla[3]){
255
256 double xyz_Cov[3][3],enu_Cov[3][3];
257
258 for(int i=0;i<3;i++)
259 for(int j=0;j<3;j++)
260 xyz_Cov[i][j]=xyz_cov[3*i+j];
261
262 double R[3][3],Rt[3][3],Tmp[3][3];
263
264 rot3d(R, ref_lla[0], ref_lla[1]);
265
266 transposeMatrix(Rt,R);
267
268 matrixMultiply(Tmp,xyz_Cov,Rt);
269
270 matrixMultiply(enu_Cov,R,Tmp);
271
272 for(int i=0;i<3;i++)
273 for(int j=0;j<3;j++)
274 enu_cov[3*i+j] = enu_Cov[i][j];
275
276 }
277
278 //--------------------------------------------------------------------------------------------------------------------------------
279 // WgsConversions::enu2xyz_cov [Public] --- convert position/velocity covariance from (East,North,Up) to (ECEF X, ECEF Y, ECEF Z)
280 //--------------------------------------------------------------------------------------------------------------------------------
281 void enu2xyz_cov(double xyz_Cov[3][3], const double enu_Cov[3][3], const double ref_lla[3]){
282
283 double R[3][3],Rt[3][3],Tmp[3][3];
284
285 rot3d(R, ref_lla[0], ref_lla[1]);
286
287 transposeMatrix(Rt,R);
288
289 matrixMultiply(Tmp,enu_Cov,R);
290
291 matrixMultiply(xyz_Cov,Rt,Tmp);
292
293 }
294
295 void enu2xyz_cov(double xyz_cov[9], const double enu_cov[9], const double ref_lla[3]){
296
297 double xyz_Cov[3][3],enu_Cov[3][3];
298
299 for(int i=0;i<3;i++)
300 for(int j=0;j<3;j++)
301 enu_Cov[i][j]=enu_cov[3*i+j];
302
303 double R[3][3],Rt[3][3],Tmp[3][3];
304
305 rot3d(R, ref_lla[0], ref_lla[1]);
306
307 transposeMatrix(Rt,R);
308
309 matrixMultiply(Tmp,enu_Cov,R);
310
311 matrixMultiply(xyz_Cov,Rt,Tmp);
312
313 for(int i=0;i<3;i++)
314 for(int j=0;j<3;j++)
315 xyz_cov[3*i+j] = xyz_Cov[i][j];
316 }
317
318private:
319
320 //--------------------------------------------------------------------------------------------
321 // WgsConversions::enu2xyz [Private] --- return the 3D rotation matrix to/from ECEF/ENU frame
322 //--------------------------------------------------------------------------------------------
323 void rot3d(double R[3][3], const double reflat, const double reflon){
324
325 double R1[3][3],R2[3][3];
326
327 rot(R1, 90 + reflon, 3);
328 rot(R2, 90 - reflat, 1);
329
330 matrixMultiply(R, R2, R1);
331 }
332
333 //------------------------------------------------------------------------------------------------
334 // WgsConversions::matrixMultiply [Private] --- Multiply 3x3 matrix times another 3x3 matrix C=AB
335 //------------------------------------------------------------------------------------------------
336 void matrixMultiply(double C[3][3], const double A[3][3], const double B[3][3]){
337
338 C[0][0] = A[0][0] * B[0][0] + A[0][1] * B[1][0] + A[0][2] * B[2][0];
339 C[0][1] = A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1];
340 C[0][2] = A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2];
341 C[1][0] = A[1][0] * B[0][0] + A[1][1] * B[1][0] + A[1][2] * B[2][0];
342 C[1][1] = A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1];
343 C[1][2] = A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2];
344 C[2][0] = A[2][0] * B[0][0] + A[2][1] * B[1][0] + A[2][2] * B[2][0];
345 C[2][1] = A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1];
346 C[2][2] = A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2];
347
348 }
349
350 //------------------------------------------------------------------------------------------------
351 // WgsConversions::matrixMultiply [Private] --- Multiply 3x3 matrix times a 3x1 vector c=Ab
352 //------------------------------------------------------------------------------------------------
353 void matrixMultiply(double c[3], const double A[3][3], const double b[3]){
354
355 c[0] = A[0][0] * b[0] + A[0][1] * b[1] + A[0][2] * b[2];
356 c[1] = A[1][0] * b[0] + A[1][1] * b[1] + A[1][2] * b[2];
357 c[2] = A[2][0] * b[0] + A[2][1] * b[1] + A[2][2] * b[2];
358
359 }
360
361 //------------------------------------------------------------------------------------------------
362 // WgsConversions::transposeMatrix [Private] --- transpose a 3x3 matrix At = A'
363 //------------------------------------------------------------------------------------------------
364 void transposeMatrix(double At[3][3], const double A[3][3]){
365
366 At[0][0] = A[0][0];
367 At[0][1] = A[1][0];
368 At[0][2] = A[2][0];
369 At[1][0] = A[0][1];
370 At[1][1] = A[1][1];
371 At[1][2] = A[2][1];
372 At[2][0] = A[0][2];
373 At[2][1] = A[1][2];
374 At[2][2] = A[2][2];
375
376 }
377
378 //------------------------------------------------------------------------------------------------
379 // WgsConversions::rot [Private] --- rotation matrix
380 //------------------------------------------------------------------------------------------------
381 void rot(double R[3][3], const double angle, const int axis) {
382
383 double cang = cos(angle * M_PI_R / 180);
384 double sang = sin(angle * M_PI_R / 180);
385
386 if (axis == 1) {
387 R[0][0] = 1;
388 R[0][1] = 0;
389 R[0][2] = 0;
390 R[1][0] = 0;
391 R[2][0] = 0;
392 R[1][1] = cang;
393 R[2][2] = cang;
394 R[1][2] = sang;
395 R[2][1] = -sang;
396 } else if (axis == 2) {
397 R[0][1] = 0;
398 R[1][0] = 0;
399 R[1][1] = 1;
400 R[1][2] = 0;
401 R[2][1] = 0;
402 R[0][0] = cang;
403 R[2][2] = cang;
404 R[0][2] = -sang;
405 R[2][0] = sang;
406 } else if (axis == 3) {
407 R[2][0] = 0;
408 R[2][1] = 0;
409 R[2][2] = 1;
410 R[0][2] = 0;
411 R[1][2] = 0;
412 R[0][0] = cang;
413 R[1][1] = cang;
414 R[1][0] = -sang;
415 R[0][1] = sang;
416 }
417 }
418};
制作者 doxygen 1.14.0