navigation.c

Go to the documentation of this file.
00001 /**
00002 \file    navigation.c
00003 \brief   GNSS core 'c' function library: navigation functions.
00004 \author  Glenn D. MacGougan (GDM)
00005 \date    2007-11-29
00006 \since   2005-08-23
00007 
00008 \b "LICENSE INFORMATION" \n
00009 Copyright (c) 2007, refer to 'author' doxygen tags \n
00010 All rights reserved. \n
00011 
00012 Redistribution and use in source and binary forms, with or without
00013 modification, are permitted provided the following conditions are met: \n
00014 
00015 - Redistributions of source code must retain the above copyright
00016   notice, this list of conditions and the following disclaimer. \n
00017 - Redistributions in binary form must reproduce the above copyright
00018   notice, this list of conditions and the following disclaimer in the
00019   documentation and/or other materials provided with the distribution. \n
00020 - The name(s) of the contributor(s) may not be used to endorse or promote 
00021   products derived from this software without specific prior written 
00022   permission. \n
00023 
00024 THIS SOFTWARE IS PROVIDED BY THE CONTRIBUTORS ``AS IS'' AND ANY EXPRESS 
00025 OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
00026 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00027 DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
00028 INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00029 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
00030 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
00031 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
00032 LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 
00033 OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 
00034 SUCH DAMAGE.
00035 */
00036 
00037 #include <math.h>
00038 #include "constants.h"
00039 #include "geodesy.h"
00040 #include "navigation.h"
00041 
00042 void NAVIGATION_ComputeDerivativesOf_Range_WithRespectTo_XYZ( 
00043   const double x,     // User X coordinate WGS84 ECEF      [m]
00044   const double y,     // User Y coordinate WGS84 ECEF      [m]
00045   const double z,     // User Z coordinate WGS84 ECEF      [m]
00046   const double satX,  // Satellite X coordinate WGS84 ECEF [m]
00047   const double satY,  // Satellite Y coordinate WGS84 ECEF [m]
00048   const double satZ,  // Satellite Z coordinate WGS84 ECEF [m]  
00049   double* dPdx,       // Derivative of P wrt X             []
00050   double* dPdy,       // Derivative of P wrt Y             []
00051   double* dPdz,       // Derivative of P wrt Z             []    
00052   double* range )     // computed user to satellite range  [m]  
00053 {
00054   double dx;
00055   double dy;
00056   double dz;
00057          
00058   dx = satX - x;
00059   dy = satY - y;
00060   dz = satZ - z;
00061   
00062   *range  = sqrt( dx*dx + dy*dy + dz*dz );
00063   
00064   // dPdx = d/dx( sqrt( (x_s-x)^2 + (y_s-y)^2 + (z_s-z)^2 ) = -dx/range
00065   *dPdx   = -dx / (*range);
00066   *dPdy   = -dy / (*range); 
00067   *dPdz   = -dz / (*range);      
00068 }
00069 
00070 
00071 
00072 
00073 void NAVIGATION_ComputeDerivativesOf_Range_WithRespectToLatitudeLongitudeHeight( 
00074   const double latitude,  // User geodetic latitude                   [rad]
00075   const double longitude, // User geodetic longtiude                  [rad]
00076   const double height,    // User geodetic height                     [m]
00077   const double satX,      // Satellite X coordinate WGS84 ECEF        [m]
00078   const double satY,      // Satellite Y coordinate WGS84 ECEF        [m]
00079   const double satZ,      // Satellite Z coordinate WGS84 ECEF        [m]  
00080   double* dlat,           // d(P)/d(lat) but not in units of [m/rad], [m/m]
00081   double* dlon,           // d(P)/d(lon) but not in units of [m/rad], [m/m]
00082   double* dhgt,           // d(P)/d(hgt)                              [m/m]
00083   double* range )         // computed user to satellite range         [m]
00084 {
00085   double sinlat;
00086   double coslat; 
00087   double sinlon;
00088   double coslon;
00089   double dx;
00090   double dy;
00091   double dz;  
00092   double x; // User X position WGS84 ECEF [m]
00093   double y; // User Y position WGS84 ECEF [m]
00094   double z; // User Z position WGS84 ECEF [m]
00095   
00096   sinlat = sin(latitude);
00097   coslat = cos(latitude);
00098   sinlon = sin(longitude);
00099   coslon = cos(longitude);
00100 
00101   GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates(
00102     GEODESY_REFERENCE_ELLIPSE_WGS84,
00103     latitude,
00104     longitude,
00105     height,
00106     &x,
00107     &y, 
00108     &z );
00109   
00110   dx = satX - x;
00111   dy = satY - y;
00112   dz = satZ - z;
00113 
00114   // d/dx(P) = d/dx( sqrt( (x_s-x)^2 + (y_s-y)^2 + (z_s-z)^2 ) = -dx/range
00115   // likewise for d/dy, d/dz
00116   dx *= -1.0;
00117   dy *= -1.0;
00118   dz *= -1.0;
00119 
00120   *range  = sqrt( dx*dx + dy*dy + dz*dz );
00121 
00122   /*
00123   GEODESY_ComputePrimeVerticalRadiusOfCurvature( GEODESY_REFERENCE_ELLIPSE_WGS84, latitude, &N );
00124   GEODESY_ComputeMeridianRadiusOfCurvature( GEODESY_REFERENCE_ELLIPSE_WGS84, latitude, &M );  
00125 
00126   The derivatives of range with respect to lat and lon in [rad/m]
00127   lead to numeric instabilities in matrix inversion. Values in [m/rad] are huge!
00128   *dlat = (M + hgt) * ( -dx*sinlat*coslon - dy*sinlat*sinlon + dz*coslat ) / (*range);
00129   *dlon = (N + hgt) * ( -dx*coslat*sinlon + dy*coslat*coslon ) / (*range);  
00130   to scale dlat into a change in [m/m]
00131   *dlat /= (M + hgt)
00132   to scale dlon into a change in [m/m] 
00133   *dlon /= (N + hgt)*coslat
00134   
00135   This is effectively the same as rotating the derivatives with repect to xyz to the local frame
00136   This method requires a good approximation for the initial position when using this parametrization.
00137   */
00138   
00139   *dlat = ( -dx*sinlat*coslon - dy*sinlat*sinlon + dz*coslat ) / (*range);
00140   *dlon = ( -dx*sinlon        + dy*coslon                    ) / (*range);  
00141   *dhgt = (  dx*coslat*coslon + dy*coslat*sinlon + dz*sinlat ) / (*range);  
00142 }
00143 
00144 
00145 
00146 
00147 
00148 
00149 
00150 int NAVIGATION_PerformClosedFormPositionSolution_FromPseuodrangeMeasurements(
00151   double p1,          //!< 1st raw pseudorange measurement [m]
00152   double p2,          //!< 2nd raw pseudorange measurement [m]
00153   double p3,          //!< 3rd raw pseudorange measurement [m]
00154   double p4,          //!< 4th raw pseudorange measurement [m]
00155   double prc_satclk1, //!< 1st satellite clock corrections for psuedoranges [m]
00156   double prc_satclk2, //!< 2nd satellite clock corrections for psuedoranges [m]
00157   double prc_satclk3, //!< 3rd satellite clock corrections for psuedoranges [m]
00158   double prc_satclk4, //!< 4th satellite clock corrections for psuedoranges [m]
00159   double x1,          //!< 1st satellite X coordinates, WGS84 ECEF [m]
00160   double x2,          //!< 2nd satellite X coordinates, WGS84 ECEF [m]
00161   double x3,          //!< 3rd satellite X coordinates, WGS84 ECEF [m]
00162   double x4,          //!< 4th satellite X coordinates, WGS84 ECEF [m]
00163   double y1,          //!< 1st satellite Y coordinates, WGS84 ECEF [m]
00164   double y2,          //!< 2nd satellite Y coordinates, WGS84 ECEF [m]
00165   double y3,          //!< 3rd satellite Y coordinates, WGS84 ECEF [m]
00166   double y4,          //!< 4th satellite Y coordinates, WGS84 ECEF [m]
00167   double z1,          //!< 1st satellite Z coordinates, WGS84 ECEF [m]
00168   double z2,          //!< 2nd satellite Z coordinates, WGS84 ECEF [m]
00169   double z3,          //!< 3rd satellite Z coordinates, WGS84 ECEF [m]
00170   double z4,          //!< 4th satellite Z coordinates, WGS84 ECEF [m]
00171   double* latitude,   //!< The computed geodetic latitude [rad]
00172   double* longitude,  //!< The computed geodetic longitude [rad]
00173   double* height,     //!< The computed geodetic height [m]
00174   double* rx_clock_bias //!< The computed receiver clock bias [m]
00175   )
00176 {
00177   // difference values
00178   double x12, x13, x14; // 'xij' = 'xi' - 'xj' [m]
00179   double y12, y13, y14;
00180   double z12, z13, z14;
00181   double p21, p31, p41; // 'pij' = 'pi' - 'pj' [m]
00182 
00183   double k1, k2, k3; // coefficients
00184   double c1, c2, c3;
00185   double f1, f2, f3;
00186   double m1, m2, m3;
00187 
00188   double d1;   // clock bias, solution 1 [m]
00189   double d2;   // clock bias, solution 2 [m]
00190   double detA; // determinant of A
00191   double tmp1;
00192   double tmp2;
00193   double tmp3;
00194 
00195   double A[3][3];
00196   double D[3][3]; // D is A^-1 * detA
00197 
00198   typedef struct
00199   {
00200     double x;
00201     double y;
00202     double z;    
00203   } struct_SOLN;
00204 
00205   struct_SOLN s1; 
00206   struct_SOLN s2;
00207 
00208   // apply the satellite clock corrections
00209   p1 = p1 + prc_satclk1;
00210   p2 = p2 + prc_satclk2;
00211   p3 = p3 + prc_satclk3;
00212   p4 = p4 + prc_satclk4;  
00213   
00214   x12 = x1 - x2;
00215   x13 = x1 - x3;
00216   x14 = x1 - x4;
00217 
00218   y12 = y1 - y2;
00219   y13 = y1 - y3;
00220   y14 = y1 - y4;
00221   
00222   z12 = z1 - z2;
00223   z13 = z1 - z3;
00224   z14 = z1 - z4;
00225 
00226   p21 = p2 - p1;
00227   p31 = p3 - p1;
00228   p41 = p4 - p1;
00229 
00230   k1 = x12*x12 + y12*y12 + z12*z12 - p21*p21;
00231   k2 = x13*x13 + y13*y13 + z13*z13 - p31*p31;
00232   k3 = x14*x14 + y14*y14 + z14*z14 - p41*p41;
00233 
00234   A[0][0] = 2.0*x12;
00235   A[1][0] = 2.0*x13;
00236   A[2][0] = 2.0*x14;
00237 
00238   A[0][1] = 2.0*y12;
00239   A[1][1] = 2.0*y13;
00240   A[2][1] = 2.0*y14;
00241 
00242   A[0][2] = 2.0*z12;
00243   A[1][2] = 2.0*z13;
00244   A[2][2] = 2.0*z14;
00245 
00246   tmp1 = A[1][1]*A[2][2] - A[2][1]*A[1][2];
00247   tmp2 = A[1][0]*A[2][2] - A[2][0]*A[1][2];
00248   tmp3 = A[1][0]*A[2][1] - A[2][0]*A[1][1];
00249 
00250   detA = A[0][0]*tmp1 - A[0][1]*tmp2 + A[0][2]*tmp3;
00251 
00252   D[0][0] =  tmp1;
00253   D[1][0] = -tmp2;
00254   D[2][0] =  tmp3;
00255 
00256   D[0][1] = -A[0][1]*A[2][2] + A[2][1]*A[0][2];
00257   D[1][1] =  A[0][0]*A[2][2] - A[2][0]*A[0][2];
00258   D[2][1] = -A[0][0]*A[2][1] + A[2][0]*A[0][1];
00259 
00260   D[0][2] =  A[0][1]*A[1][2] - A[1][1]*A[0][2];
00261   D[1][2] = -A[0][0]*A[1][2] + A[1][0]*A[0][2];
00262   D[2][2] =  A[0][0]*A[1][1] - A[1][0]*A[0][1];
00263 
00264   c1 = (D[0][0]*p21 + D[0][1]*p31 + D[0][2]*p41) * 2.0 / detA;
00265   c2 = (D[1][0]*p21 + D[1][1]*p31 + D[1][2]*p41) * 2.0 / detA;
00266   c3 = (D[2][0]*p21 + D[2][1]*p31 + D[2][2]*p41) * 2.0 / detA;
00267 
00268   f1 = (D[0][0]*k1 + D[0][1]*k2 + D[0][2]*k3) / detA;
00269   f2 = (D[1][0]*k1 + D[1][1]*k2 + D[1][2]*k3) / detA;
00270   f3 = (D[2][0]*k1 + D[2][1]*k2 + D[2][2]*k3) / detA;
00271 
00272   m1 = c1*c1 + c2*c2 + c3*c3 - 1.0;
00273   m2 = -2.0*( c1*f1 + c2*f2 + c3*f3 );
00274   m3 = f1*f1 + f2*f2 + f3*f3;
00275 
00276   tmp1 = m2*m2 - 4.0*m1*m3;
00277   if( tmp1 < 0 )
00278   {
00279     // not good, there is no solution
00280     return FALSE;
00281   }
00282 
00283   d1 = ( -m2 + sqrt( tmp1 ) ) * 0.5 / m1;
00284   d2 = ( -m2 - sqrt( tmp1 ) ) * 0.5 / m1;
00285 
00286   // two solutions
00287   s1.x = c1*d1 - f1 + x1;
00288   s1.y = c2*d1 - f2 + y1;
00289   s1.z = c3*d1 - f3 + z1;
00290 
00291   s2.x = c1*d2 - f1 + x1;
00292   s2.y = c2*d2 - f2 + y1;
00293   s2.z = c3*d2 - f3 + z1;
00294 
00295   tmp1 = sqrt( s1.x*s1.x + s1.y*s1.y + s1.z*s1.z );
00296   tmp2 = sqrt( s2.x*s2.x + s2.y*s2.y + s2.z*s2.z );
00297 
00298   // choose the correct solution based
00299   // on whichever solutino is closest to 
00300   // the Earth's surface
00301   tmp1 = fabs( tmp1 - 6371000.0 );
00302   tmp2 = fabs( tmp2 - 6371000.0 );
00303 
00304   if( tmp2 < tmp1 )
00305   {
00306     s1 = s2;
00307     d1 = d2;
00308   }
00309 
00310   *rx_clock_bias = d1;
00311 
00312   GEODESY_ConvertEarthFixedCartesianToGeodeticCurvilinearCoordinates(
00313     GEODESY_REFERENCE_ELLIPSE_WGS84,
00314     s1.x, s1.y, s1.z,
00315     latitude, longitude, height );
00316 
00317   if( *height < -1500.0 || *height > 18000.0 )
00318   {
00319     // height is out of the likely range for terrestrial users
00320     return FALSE;
00321   }
00322   
00323   return TRUE;
00324 }
00325