coord_system.c
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010 Swift Navigation Inc.
00003  * Contact: Fergus Noble <fergus@swift-nav.com>
00004  *
00005  * This source is subject to the license found in the file 'LICENSE' which must
00006  * be be distributed together with this source. All other rights reserved.
00007  *
00008  * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
00009  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
00010  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
00011  */
00012 
00013 #include <math.h>
00014 
00015 #include "linear_algebra.h"
00016 #include "coord_system.h"
00017 
00057 void wgsllh2ecef(const double const llh[3], double ecef[3]) {
00058   double d = WGS84_E * sin(llh[0]);
00059   double N = WGS84_A / sqrt(1. - d*d);
00060 
00061   ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
00062   ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
00063   ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
00064 }
00065 
00092 void wgsecef2llh(const double const ecef[3], double llh[3]) {
00093   /* Distance from polar axis. */
00094   const double p = sqrt(ecef[0]*ecef[0] + ecef[1]*ecef[1]);
00095 
00096   /* Compute longitude first, this can be done exactly. */
00097   if (p != 0)
00098     llh[1] = atan2(ecef[1], ecef[0]);
00099   else
00100     llh[1] = 0;
00101 
00102   /* If we are close to the pole then convergence is very slow, treat this is a
00103    * special case. */
00104   if (p < WGS84_A*1e-16) {
00105     llh[0] = copysign(M_PI_2, ecef[2]);
00106     llh[2] = fabs(ecef[2]) - WGS84_B;
00107     return;
00108   }
00109 
00110   /* Caluclate some other constants as defined in the Fukushima paper. */
00111   const double P = p / WGS84_A;
00112   const double e_c = sqrt(1. - WGS84_E*WGS84_E);
00113   const double Z = fabs(ecef[2]) * e_c / WGS84_A;
00114 
00115   /* Initial values for S and C correspond to a zero height solution. */
00116   double S = Z;
00117   double C = e_c * P;
00118 
00119   /* Neither S nor C can be negative on the first iteration so
00120    * starting prev = -1 will not cause and early exit. */
00121   double prev_C = -1;
00122   double prev_S = -1;
00123 
00124   double A_n, B_n, D_n, F_n;
00125 
00126   /* Iterate a maximum of 10 times. This should be way more than enough for all
00127    * sane inputs */
00128   for (int i=0; i<10; i++)
00129   {
00130     /* Calculate some intermmediate variables used in the update step based on
00131      * the current state. */
00132     A_n = sqrt(S*S + C*C);
00133     D_n = Z*A_n*A_n*A_n + WGS84_E*WGS84_E*S*S*S;
00134     F_n = P*A_n*A_n*A_n - WGS84_E*WGS84_E*C*C*C;
00135     B_n = 1.5*WGS84_E*S*C*C*(A_n*(P*S - Z*C) - WGS84_E*S*C);
00136 
00137     /* Update step. */
00138     S = D_n*F_n - B_n*S;
00139     C = F_n*F_n - B_n*C;
00140 
00141     /* The original algorithm as presented in the paper by Fukushima has a
00142      * problem with numerical stability. S and C can grow very large or small
00143      * and over or underflow a double. In the paper this is acknowledged and
00144      * the proposed resolution is to non-dimensionalise the equations for S and
00145      * C. However, this does not completely solve the problem. The author caps
00146      * the solution to only a couple of iterations and in this period over or
00147      * underflow is unlikely but as we require a bit more precision and hence
00148      * more iterations so this is still a concern for us.
00149      *
00150      * As the only thing that is important is the ratio T = S/C, my solution is
00151      * to divide both S and C by either S or C. The scaling is chosen such that
00152      * one of S or C is scaled to unity whilst the other is scaled to a value
00153      * less than one. By dividing by the larger of S or C we ensure that we do
00154      * not divide by zero as only one of S or C should ever be zero.
00155      *
00156      * This incurs an extra division each iteration which the author was
00157      * explicityl trying to avoid and it may be that this solution is just
00158      * reverting back to the method of iterating on T directly, perhaps this
00159      * bears more thought?
00160      */
00161 
00162     if (S > C) {
00163       C = C / S;
00164       S = 1;
00165     } else {
00166       S = S / C;
00167       C = 1;
00168     }
00169 
00170     /* Check for convergence and exit early if we have converged. */
00171     if (fabs(S - prev_S) < 1e-16 && fabs(C - prev_C) < 1e-16) {
00172       break;
00173     } else {
00174       prev_S = S;
00175       prev_C = C;
00176     }
00177   }
00178 
00179   A_n = sqrt(S*S + C*C);
00180   llh[0] = copysign(1.0, ecef[2]) * atan(S / (e_c*C));
00181   llh[2] = (p*e_c*C + fabs(ecef[2])*S - WGS84_A*e_c*A_n) / sqrt(e_c*e_c*C*C + S*S);
00182 }
00183 
00192 static void ecef2ned_matrix(const double ref_ecef[3], double M[3][3]) {
00193   double hyp_az, hyp_el;
00194   double sin_el, cos_el, sin_az, cos_az;
00195 
00196   /* Convert reference point to spherical earth centered coordinates. */
00197   hyp_az = sqrt(ref_ecef[0]*ref_ecef[0] + ref_ecef[1]*ref_ecef[1]);
00198   hyp_el = sqrt(hyp_az*hyp_az + ref_ecef[2]*ref_ecef[2]);
00199   sin_el = ref_ecef[2] / hyp_el;
00200   cos_el = hyp_az / hyp_el;
00201   sin_az = ref_ecef[1] / hyp_az;
00202   cos_az = ref_ecef[0] / hyp_az;
00203 
00204   M[0][0] = -sin_el * cos_az;
00205   M[0][1] = -sin_el * sin_az;
00206   M[0][2] = cos_el;
00207   M[1][0] = -sin_az;
00208   M[1][1] = cos_az;
00209   M[1][2] = 0.0;
00210   M[2][0] = -cos_el * cos_az;
00211   M[2][1] = -cos_el * sin_az;
00212   M[2][2] = -sin_el;
00213 }
00214 
00215 
00234 void wgsecef2ned(const double ecef[3], const double ref_ecef[3],
00235                  double ned[3]) {
00236   double M[3][3];
00237 
00238   ecef2ned_matrix(ref_ecef, M);
00239   matrix_multiply(3, 3, 1, (double *)M, ecef, ned);
00240 }
00241 
00256 void wgsecef2ned_d(const double ecef[3], const double ref_ecef[3],
00257                    double ned[3]) {
00258   double tempv[3];
00259   vector_subtract(3, ecef, ref_ecef, tempv);
00260   wgsecef2ned(tempv, ref_ecef, ned);
00261 }
00262 
00263 
00281 void wgsned2ecef(const double ned[3], const double ref_ecef[3],
00282                  double ecef[3]) {
00283   double M[3][3], M_transpose[3][3];
00284   ecef2ned_matrix(ref_ecef, M);
00285   matrix_transpose(3, 3, (double *)M, (double *)M_transpose);
00286   matrix_multiply(3, 3, 1, (double *)M_transpose, ned, ecef);
00287 }
00288 
00301 void wgsned2ecef_d(const double ned[3], const double ref_ecef[3],
00302                    double ecef[3]) {
00303   double tempv[3];
00304   wgsned2ecef(ned, ref_ecef, tempv);
00305   vector_add(3, tempv, ref_ecef, ecef);
00306 }
00307 
00308 
00325 void wgsecef2azel(const double ecef[3], const double ref_ecef[3],
00326                   double* azimuth, double* elevation) {
00327   double ned[3];
00328 
00329   /* Calculate the vector from the reference point in the local North, East,
00330    * Down frame of the reference point. */
00331   wgsecef2ned_d(ecef, ref_ecef, ned);
00332 
00333   *azimuth = atan2(ned[1], ned[0]);
00334   /* atan2 returns angle in range [-pi, pi], usually azimuth is defined in the
00335    * range [0, 2pi]. */
00336   if (*azimuth < 0)
00337     *azimuth += 2*M_PI;
00338 
00339   *elevation = asin(-ned[2]/vector_norm(3, ned));
00340 }
00341 


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