check_coord_system.c
Go to the documentation of this file.
00001 #include <math.h>
00002 
00003 #include <check.h>
00004 #include "check_utils.h"
00005 
00006 #include <coord_system.h>
00007 #include <pvt.h>
00008 
00009 /* Maximum allowable error in quantities with units of length (in meters). */
00010 #define MAX_DIST_ERROR_M 1e-6
00011 /* Maximum allowable error in quantities with units of angle (in sec of arc).
00012  * 1 second of arc on the equator is ~31 meters. */
00013 #define MAX_ANGLE_ERROR_SEC 1e-7
00014 #define MAX_ANGLE_ERROR_RAD (MAX_ANGLE_ERROR_SEC*(D2R/3600.0))
00015 
00016 /* Semi-major axis. */
00017 #define EARTH_A 6378137.0
00018 /* Semi-minor axis. */
00019 #define EARTH_B 6356752.31424517929553985595703125
00020 
00021 #define NUM_COORDS 10
00022 const double const llhs[NUM_COORDS][3] = {
00023   {0, 0, 0},        /* On the Equator and Prime Meridian. */
00024   {0, 180*D2R, 0},  /* On the Equator. */
00025   {0, 90*D2R, 0},   /* On the Equator. */
00026   {0, -90*D2R, 0},  /* On the Equator. */
00027   {90*D2R, 0, 0},   /* North pole. */
00028   {-90*D2R, 0, 0},  /* South pole. */
00029   {90*D2R, 0, 22},  /* 22m above the north pole. */
00030   {-90*D2R, 0, 22}, /* 22m above the south pole. */
00031   {0, 0, 22},       /* 22m above the Equator and Prime Meridian. */
00032   {0, 180*D2R, 22}, /* 22m above the Equator. */
00033 };
00034 const double const ecefs[NUM_COORDS][3] = {
00035   {EARTH_A, 0, 0},
00036   {-EARTH_A, 0, 0},
00037   {0, EARTH_A, 0},
00038   {0, -EARTH_A, 0},
00039   {0, 0, EARTH_B},
00040   {0, 0, -EARTH_B},
00041   {0, 0, (EARTH_B+22)},
00042   {0, 0, -(EARTH_B+22)},
00043   {(22+EARTH_A), 0, 0},
00044   {-(22+EARTH_A), 0, 0},
00045 };
00046 
00047 START_TEST(test_wgsllh2ecef)
00048 {
00049   double ecef[3];
00050 
00051   wgsllh2ecef(llhs[_i], ecef);
00052 
00053   for (int n=0; n<3; n++) {
00054     fail_unless(!isnan(ecef[n]), "NaN in output from wgsllh2ecef.");
00055     double err = fabs(ecef[n] - ecefs[_i][n]);
00056     fail_unless(err < MAX_DIST_ERROR_M,
00057       "Conversion from WGS84 LLH to ECEF has >1e-6m error:\n"
00058       "LLH: %f, %f, %f\n"
00059       "X error (mm): %g\nY error (mm): %g\nZ error (mm): %g",
00060       llhs[_i][0]*R2D, llhs[_i][1]*R2D, llhs[_i][2],
00061       (ecef[0] - ecefs[_i][0])*1e3,
00062       (ecef[1] - ecefs[_i][1])*1e3,
00063       (ecef[2] - ecefs[_i][2])*1e3
00064     );
00065   }
00066 }
00067 END_TEST
00068 
00069 START_TEST(test_wgsecef2llh)
00070 {
00071   double llh[3];
00072 
00073   wgsecef2llh(ecefs[_i], llh);
00074 
00075   for (int n=0; n<3; n++) {
00076     fail_unless(!isnan(llh[n]), "NaN in output from wgsecef2llh.");
00077   }
00078 
00079   double lat_err = fabs(llh[0] - llhs[_i][0]);
00080   double lon_err = fabs(llh[1] - llhs[_i][1]);
00081   double hgt_err = fabs(llh[2] - llhs[_i][2]);
00082   fail_unless((lat_err < MAX_ANGLE_ERROR_RAD) ||
00083               (lon_err < MAX_ANGLE_ERROR_RAD) ||
00084               (hgt_err < MAX_DIST_ERROR_M),
00085     "Conversion from WGS84 ECEF to LLH has >1e-6 {rad, m} error:\n"
00086     "ECEF: %f, %f, %f\n"
00087     "Lat error (arc sec): %g\nLon error (arc sec): %g\nH error (mm): %g",
00088     ecefs[_i][0], ecefs[_i][1], ecefs[_i][2],
00089     (llh[0] - llhs[_i][0])*(R2D*3600),
00090     (llh[1] - llhs[_i][1])*(R2D*3600),
00091     (llh[2] - llhs[_i][2])*1e3
00092   );
00093 }
00094 END_TEST
00095 
00096 START_TEST(test_wgsllh2ecef2llh)
00097 {
00098   double ecef[3];
00099   double llh[3];
00100 
00101   wgsllh2ecef(llhs[_i], ecef);
00102   wgsecef2llh(ecef, llh);
00103 
00104   for (int n=0; n<3; n++) {
00105     fail_unless(!isnan(llh[n]), "NaN in LLH output from conversion.");
00106     fail_unless(!isnan(ecef[n]), "NaN in ECEF output from conversion.");
00107   }
00108 
00109   double lat_err = fabs(llh[0] - llhs[_i][0]);
00110   double lon_err = fabs(llh[1] - llhs[_i][1]);
00111   double hgt_err = fabs(llh[2] - llhs[_i][2]);
00112   fail_unless((lat_err < MAX_ANGLE_ERROR_RAD) ||
00113               (lon_err < MAX_ANGLE_ERROR_RAD) ||
00114               (hgt_err < MAX_DIST_ERROR_M),
00115     "Converting WGS84 LLH to ECEF and back again does not return the "
00116     "original values.\n"
00117     "Initial LLH: %f, %f, %f\n"
00118     "ECEF: %f, %f, %f\n"
00119     "Final LLH: %f, %f, %f\n"
00120     "Lat error (arc sec): %g\nLon error (arc sec): %g\nH error (mm): %g",
00121     R2D*llhs[_i][0], R2D*llhs[_i][1], llhs[_i][2],
00122     ecef[0], ecef[1], ecef[2],
00123     R2D*llh[0], R2D*llh[1], llh[2],
00124     (llh[0] - llhs[_i][0])*(R2D*3600),
00125     (llh[1] - llhs[_i][1])*(R2D*3600),
00126     (llh[2] - llhs[_i][2])*1e3
00127   );
00128 }
00129 END_TEST
00130 
00131 START_TEST(test_wgsecef2llh2ecef)
00132 {
00133   double llh[3];
00134   double ecef[3];
00135 
00136   wgsecef2llh(ecefs[_i], llh);
00137   wgsllh2ecef(llh, ecef);
00138 
00139   for (int n=0; n<3; n++) {
00140     fail_unless(!isnan(llh[n]), "NaN in LLH output from conversion.");
00141     fail_unless(!isnan(ecef[n]), "NaN in ECEF output from conversion.");
00142   }
00143 
00144   for (int n=0; n<3; n++) {
00145     double err = fabs(ecef[n] - ecefs[_i][n]);
00146     fail_unless(err < MAX_DIST_ERROR_M,
00147       "Converting WGS84 ECEF to LLH and back again does not return the "
00148       "original values.\n"
00149       "Initial ECEF: %f, %f, %f\n"
00150       "LLH: %f, %f, %f\n"
00151       "Final ECEF: %f, %f, %f\n"
00152       "X error (mm): %g\nY error (mm): %g\nZ error (mm): %g",
00153       ecefs[_i][0], ecefs[_i][1], ecefs[_i][2],
00154       R2D*llh[0], R2D*llh[1], llh[2],
00155       ecef[0], ecef[1], ecef[2],
00156       (ecef[0] - ecefs[_i][0])*1e3,
00157       (ecef[1] - ecefs[_i][1])*1e3,
00158       (ecef[2] - ecefs[_i][2])*1e3
00159     );
00160   }
00161 }
00162 END_TEST
00163 
00164 START_TEST(test_random_wgsllh2ecef2llh)
00165 {
00166   double ecef[3];
00167   double llh_init[3];
00168   double llh[3];
00169 
00170   seed_rng();
00171 
00172   llh_init[0] = D2R*frand(-90, 90);
00173   llh_init[1] = D2R*frand(-180, 180);
00174   llh_init[2] = frand(-EARTH_A, 4*EARTH_A);
00175 
00176   wgsllh2ecef(llh_init, ecef);
00177   wgsecef2llh(ecef, llh);
00178 
00179   for (int n=0; n<3; n++) {
00180     fail_unless(!isnan(llh[n]), "NaN in LLH output from conversion.");
00181     fail_unless(!isnan(ecef[n]), "NaN in ECEF output from conversion.");
00182   }
00183 
00184   double lat_err = fabs(llh[0] - llh_init[0]);
00185   double lon_err = fabs(llh[1] - llh_init[1]);
00186   double hgt_err = fabs(llh[2] - llh_init[2]);
00187   fail_unless((lat_err < MAX_ANGLE_ERROR_RAD) ||
00188               (lon_err < MAX_ANGLE_ERROR_RAD) ||
00189               (hgt_err < MAX_DIST_ERROR_M),
00190     "Converting random WGS84 LLH to ECEF and back again does not return the "
00191     "original values.\n"
00192     "Initial LLH: %f, %f, %f\n"
00193     "ECEF: %f, %f, %f\n"
00194     "Final LLH: %f, %f, %f\n"
00195     "Lat error (arc sec): %g\nLon error (arc sec): %g\nH error (mm): %g",
00196     R2D*llh_init[0], R2D*llh_init[1], llh_init[2],
00197     ecef[0], ecef[1], ecef[2],
00198     R2D*llh[0], R2D*llh[1], llh[2],
00199     (llh[0] - llh_init[0])*(R2D*3600),
00200     (llh[1] - llh_init[1])*(R2D*3600),
00201     (llh[2] - llh_init[2])*1e3
00202   );
00203 
00204   fail_unless((R2D*llh[0] >= -90) && (R2D*llh[0] <= 90),
00205       "Converting random WGS86 ECEF gives latitude out of bounds.\n"
00206       "ECEF: %f, %f, %f\n"
00207       "LLH: %f, %f, %f\n",
00208       ecef[0], ecef[1], ecef[2],
00209       R2D*llh[0], R2D*llh[1], llh[2]
00210   );
00211 
00212   fail_unless((R2D*llh[1] >= -180) && (R2D*llh[1] <= 180),
00213       "Converting random WGS86 ECEF gives longitude out of bounds.\n"
00214       "ECEF: %f, %f, %f\n"
00215       "LLH: %f, %f, %f\n",
00216       ecef[0], ecef[1], ecef[2],
00217       R2D*llh[0], R2D*llh[1], llh[2]
00218   );
00219 
00220   fail_unless(llh[2] > -EARTH_A,
00221       "Converting random WGS86 ECEF gives height out of bounds.\n"
00222       "ECEF: %f, %f, %f\n"
00223       "LLH: %f, %f, %f\n",
00224       ecef[0], ecef[1], ecef[2],
00225       R2D*llh[0], R2D*llh[1], llh[2]
00226   );
00227 }
00228 END_TEST
00229 
00230 START_TEST(test_random_wgsecef2llh2ecef)
00231 {
00232   double ecef_init[3];
00233   double llh[3];
00234   double ecef[3];
00235 
00236   seed_rng();
00237 
00238   ecef_init[0] = frand(-4*EARTH_A, 4*EARTH_A);
00239   ecef_init[1] = frand(-4*EARTH_A, 4*EARTH_A);
00240   ecef_init[2] = frand(-4*EARTH_A, 4*EARTH_A);
00241 
00242   wgsecef2llh(ecef_init, llh);
00243   wgsllh2ecef(llh, ecef);
00244 
00245   for (int n=0; n<3; n++) {
00246     fail_unless(!isnan(llh[n]), "NaN in LLH output from conversion.");
00247     fail_unless(!isnan(ecef[n]), "NaN in ECEF output from conversion.");
00248   }
00249 
00250   for (int n=0; n<3; n++) {
00251     double err = fabs(ecef[n] - ecef_init[n]);
00252     fail_unless(err < MAX_DIST_ERROR_M,
00253       "Converting random WGS84 ECEF to LLH and back again does not return the "
00254       "original values.\n"
00255       "Initial ECEF: %f, %f, %f\n"
00256       "LLH: %f, %f, %f\n"
00257       "Final ECEF: %f, %f, %f\n"
00258       "X error (mm): %g\nY error (mm): %g\nZ error (mm): %g",
00259       ecef_init[0], ecef_init[1], ecef_init[2],
00260       R2D*llh[0], R2D*llh[1], llh[2],
00261       ecef[0], ecef[1], ecef[2],
00262       (ecef[0] - ecef_init[0])*1e3,
00263       (ecef[1] - ecef_init[1])*1e3,
00264       (ecef[2] - ecef_init[2])*1e3
00265     );
00266   }
00267 
00268   fail_unless((R2D*llh[0] >= -90) && (R2D*llh[0] <= 90),
00269       "Converting random WGS86 ECEF gives latitude out of bounds.\n"
00270       "Initial ECEF: %f, %f, %f\n"
00271       "LLH: %f, %f, %f\n",
00272       ecef_init[0], ecef_init[1], ecef_init[2],
00273       R2D*llh[0], R2D*llh[1], llh[2]
00274   );
00275 
00276   fail_unless((R2D*llh[1] >= -180) && (R2D*llh[1] <= 180),
00277       "Converting random WGS86 ECEF gives longitude out of bounds.\n"
00278       "Initial ECEF: %f, %f, %f\n"
00279       "LLH: %f, %f, %f\n",
00280       ecef_init[0], ecef_init[1], ecef_init[2],
00281       R2D*llh[0], R2D*llh[1], llh[2]
00282   );
00283 
00284   fail_unless(llh[2] > -EARTH_A,
00285       "Converting random WGS86 ECEF gives height out of bounds.\n"
00286       "Initial ECEF: %f, %f, %f\n"
00287       "LLH: %f, %f, %f\n",
00288       ecef_init[0], ecef_init[1], ecef_init[2],
00289       R2D*llh[0], R2D*llh[1], llh[2]
00290   );
00291 }
00292 END_TEST
00293 
00294 /* Check simply that passing the ECEF position the same as the
00295  * reference position returns (0, 0, 0) in NED frame */
00296 START_TEST(test_random_wgsecef2ned_d_0) {
00297   s32 i, j;
00298   double ned[3];
00299 
00300   seed_rng();
00301   for (i = 0; i < 222; i++) {
00302     const double ecef[3] = {frand(-1e8, 1e8),
00303                             frand(-1e8, 1e8),
00304                             frand(-1e8, 1e8)};
00305     wgsecef2ned_d(ecef, ecef, ned);
00306     for (j = 0; j < 3; j++)
00307       fail_unless(fabs(ned[j]) < 1e-8,
00308                   "NED vector to reference ECEF point "
00309                   "has nonzero element %d: %lf\n"
00310                   "(point was <%lf %lf %lf>)\n",
00311                   ned[j], ecef[0], ecef[1], ecef[2]);
00312   }
00313 }
00314 END_TEST
00315 
00316 Suite* coord_system_suite(void)
00317 {
00318   Suite *s = suite_create("Coordinate systems");
00319 
00320   /* Core test case */
00321   TCase *tc_core = tcase_create("Core");
00322   tcase_add_loop_test(tc_core, test_wgsllh2ecef, 0, NUM_COORDS);
00323   tcase_add_loop_test(tc_core, test_wgsecef2llh, 0, NUM_COORDS);
00324   tcase_add_loop_test(tc_core, test_wgsllh2ecef2llh, 0, NUM_COORDS);
00325   tcase_add_loop_test(tc_core, test_wgsecef2llh2ecef, 0, NUM_COORDS);
00326   suite_add_tcase(s, tc_core);
00327 
00328   TCase *tc_random = tcase_create("Random");
00329   tcase_add_loop_test(tc_random, test_random_wgsllh2ecef2llh, 0, 100);
00330   tcase_add_loop_test(tc_random, test_random_wgsecef2llh2ecef, 0, 100);
00331   tcase_add_loop_test(tc_random, test_random_wgsecef2ned_d_0, 0, 100);
00332   suite_add_tcase(s, tc_random);
00333 
00334   return s;
00335 }
00336 


enu
Author(s): Mike Purvis
autogenerated on Fri Jan 3 2014 11:21:07