Line data    Source code

       1             : /*
       2             :  * location_double.cpp
       3             :  *
       4             :  * This file is free software: you can redistribute it and/or modify it
       5             :  * under the terms of the GNU General Public License as published by the
       6             :  * Free Software Foundation, either version 3 of the License, or
       7             :  * (at your option) any later version.
       8             :  *
       9             :  * This file is distributed in the hope that it will be useful, but
      10             :  * WITHOUT ANY WARRANTY; without even the implied warranty of
      11             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
      12             :  * See the GNU General Public License for more details.
      13             :  *
      14             :  * You should have received a copy of the GNU General Public License along
      15             :  * with this program.  If not, see <http://www.gnu.org/licenses/>.
      16             :  */
      17             : 
      18             : /*
      19             :   this is for double precision functions related to the location structure
      20             :  */
      21             : 
      22             : #define AP_MATH_ALLOW_DOUBLE_FUNCTIONS 1
      23             : 
      24             : #include <AP_HAL/AP_HAL.h>
      25             : #include <stdlib.h>
      26             : #include "AP_Math.h"
      27             : #include "location.h"
      28             : 
      29             : /*
      30             :   these are not currently used. They should be moved to location_double.cpp if we do enable them in the future
      31             :  */
      32       34794 : void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef) {
      33       34794 :   double d = WGS84_E * sin(llh[0]);
      34       34794 :   double N = WGS84_A / sqrt(1 - d*d);
      35             : 
      36       34794 :   ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
      37       34794 :   ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
      38       34794 :   ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
      39       34794 : }
      40             : 
      41             : 
      42           0 : void wgsecef2llh(const Vector3d &ecef, Vector3d &llh) {
      43             :   /* Distance from polar axis. */
      44           0 :   const double p = sqrt(ecef[0]*ecef[0] + ecef[1]*ecef[1]);
      45             : 
      46             :   /* Compute longitude first, this can be done exactly. */
      47           0 :   if (!is_zero(p))
      48           0 :     llh[1] = atan2(ecef[1], ecef[0]);
      49             :   else
      50           0 :     llh[1] = 0;
      51             : 
      52             :   /* If we are close to the pole then convergence is very slow, treat this is a
      53             :    * special case. */
      54           0 :   if (p < WGS84_A * double(1e-16)) {
      55           0 :     llh[0] = copysign(M_PI_2, ecef[2]);
      56           0 :     llh[2] = fabs(ecef[2]) - WGS84_B;
      57           0 :     return;
      58             :   }
      59             : 
      60             :   /* Calculate some other constants as defined in the Fukushima paper. */
      61           0 :   const double P = p / WGS84_A;
      62           0 :   const double e_c = sqrt(1 - WGS84_E*WGS84_E);
      63           0 :   const double Z = fabs(ecef[2]) * e_c / WGS84_A;
      64             : 
      65             :   /* Initial values for S and C correspond to a zero height solution. */
      66           0 :   double S = Z;
      67           0 :   double C = e_c * P;
      68             : 
      69             :   /* Neither S nor C can be negative on the first iteration so
      70             :    * starting prev = -1 will not cause and early exit. */
      71           0 :   double prev_C = -1;
      72           0 :   double prev_S = -1;
      73             : 
      74             :   double A_n, B_n, D_n, F_n;
      75             : 
      76             :   /* Iterate a maximum of 10 times. This should be way more than enough for all
      77             :    * sane inputs */
      78           0 :   for (int i=0; i<10; i++)
      79             :   {
      80             :     /* Calculate some intermmediate variables used in the update step based on
      81             :      * the current state. */
      82           0 :     A_n = sqrt(S*S + C*C);
      83           0 :     D_n = Z*A_n*A_n*A_n + WGS84_E*WGS84_E*S*S*S;
      84           0 :     F_n = P*A_n*A_n*A_n - WGS84_E*WGS84_E*C*C*C;
      85           0 :     B_n = double(1.5) * WGS84_E*S*C*C*(A_n*(P*S - Z*C) - WGS84_E*S*C);
      86             : 
      87             :     /* Update step. */
      88           0 :     S = D_n*F_n - B_n*S;
      89           0 :     C = F_n*F_n - B_n*C;
      90             : 
      91             :     /* The original algorithm as presented in the paper by Fukushima has a
      92             :      * problem with numerical stability. S and C can grow very large or small
      93             :      * and over or underflow a double. In the paper this is acknowledged and
      94             :      * the proposed resolution is to non-dimensionalise the equations for S and
      95             :      * C. However, this does not completely solve the problem. The author caps
      96             :      * the solution to only a couple of iterations and in this period over or
      97             :      * underflow is unlikely but as we require a bit more precision and hence
      98             :      * more iterations so this is still a concern for us.
      99             :      *
     100             :      * As the only thing that is important is the ratio T = S/C, my solution is
     101             :      * to divide both S and C by either S or C. The scaling is chosen such that
     102             :      * one of S or C is scaled to unity whilst the other is scaled to a value
     103             :      * less than one. By dividing by the larger of S or C we ensure that we do
     104             :      * not divide by zero as only one of S or C should ever be zero.
     105             :      *
     106             :      * This incurs an extra division each iteration which the author was
     107             :      * explicityl trying to avoid and it may be that this solution is just
     108             :      * reverting back to the method of iterating on T directly, perhaps this
     109             :      * bears more thought?
     110             :      */
     111             : 
     112           0 :     if (S > C) {
     113           0 :       C = C / S;
     114           0 :       S = 1;
     115             :     } else {
     116           0 :       S = S / C;
     117           0 :       C = 1;
     118             :     }
     119             : 
     120             :     /* Check for convergence and exit early if we have converged. */
     121           0 :     if (fabs(S - prev_S) < double(1e-16) && fabs(C - prev_C) < double(1e-16)) {
     122             :       break;
     123             :     } else {
     124           0 :       prev_S = S;
     125           0 :       prev_C = C;
     126             :     }
     127             :   }
     128             : 
     129           0 :   A_n = sqrt(S*S + C*C);
     130           0 :   llh[0] = copysign(1.0, ecef[2]) * atan(S / (e_c*C));
     131           0 :   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);
     132             : }