pvt.c
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2010 Swift Navigation Inc.
00003  * Contact: Henry Hallam <henry@swift-nav.com>
00004  *          Matt Peddie <peddie@alum.mit.edu>
00005  *
00006  * This source is subject to the license found in the file 'LICENSE' which must
00007  * be be distributed together with this source. All other rights reserved.
00008  *
00009  * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
00010  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
00011  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
00012  */
00013 
00014 #include <math.h>
00015 #include <string.h>
00016 #include <stdio.h>
00017 
00018 #include "constants.h"
00019 #include "linear_algebra.h"
00020 #include "coord_system.h"
00021 #include "track.h"
00022 
00023 #include "pvt.h"
00024 
00025 static double vel_solve(double rx_vel[],
00026                         const u8 n_used,
00027                         const navigation_measurement_t nav_meas[n_used],
00028                         const double G[n_used][4],
00029                         const double X[4][n_used])
00030 {
00031   /* Velocity Solution
00032    *
00033    * G and X matrices already exist from the position
00034    * solution loop through valid measurements.  Here we form satellite
00035    * velocity and pseudorange rate vectors -- it's the same
00036    * prediction-error least-squares thing, but we do only one step.
00037   */
00038 
00039   double tempvX[n_used];
00040   double pdot_pred;
00041 
00042   for (u8 j = 0; j < n_used; j++) {
00043     /* Calculate predicted pseudorange rates from the satellite velocity
00044      * and the geometry matix G which contains normalised line-of-sight
00045      * vectors to the satellites.
00046      */
00047     pdot_pred = -vector_dot(3, G[j], nav_meas[j].sat_vel);
00048 
00049     /* The residual is due to the user's motion. */
00050     tempvX[j] = -nav_meas[j].doppler * GPS_C / GPS_L1_HZ - pdot_pred;
00051   }
00052 
00053   /* Use X to map our pseudorange rate residuals onto the Jacobian update.
00054    *
00055    *   rx_vel[j] = X[j] . tempvX[j]
00056    */
00057   matrix_multiply(4, n_used, 1, (double *) X, (double *) tempvX, (double *) rx_vel);
00058 
00059   /* Return just the receiver clock bias. */
00060   return rx_vel[3];
00061 }
00062 
00063 void compute_dops(const double H[4][4],
00064                   const double pos_ecef[3],
00065                   dops_t *dops)
00066 {
00067   double H_pos_diag[3];
00068   double H_ned[3];
00069 
00070   dops->gdop = dops->pdop = dops->tdop = dops->hdop = dops->vdop = 0;
00071 
00072   /* PDOP is the norm of the position elements of tr(H) */
00073   for (u8 i=0; i<3; i++) {
00074     dops->pdop += H[i][i];
00075     /* Also get the trace of H position states for use in HDOP/VDOP
00076      * calculations.
00077      */
00078     H_pos_diag[i] = H[i][i];
00079   }
00080   dops->pdop = sqrt(dops->pdop);
00081 
00082   /* TDOP is like PDOP but for the time state. */
00083   dops->tdop = sqrt(H[3][3]);
00084 
00085   /* Calculate the GDOP -- ||tr(H)|| = sqrt(PDOP^2 + TDOP^2) */
00086   dops->gdop = sqrt(dops->tdop*dops->tdop + dops->pdop*dops->pdop);
00087 
00088   /* HDOP and VDOP are Horizontal and Vertical; we need to rotate the
00089    * PDOP into NED frame and then take the separate components.
00090    */
00091   wgsecef2ned(H_pos_diag, pos_ecef, H_ned);
00092   dops->vdop = sqrt(H_ned[2]*H_ned[2]);
00093   dops->hdop = sqrt(H_ned[0]*H_ned[0] + H_ned[1]*H_ned[1]);
00094 }
00095 
00096 
00097 /* This function is the key to GPS solution, so it's commented
00098  * liberally.  It does a single step of a multi-dimensional
00099  * Newton-Raphson solution for the variables X, Y, Z (in ECEF) plus
00100  * the clock offset for each receiver used to make pseudorange
00101  * measurements.  The steps involved are roughly the following:
00102  *
00103  *     1. Account for the Earth's rotation during transmission
00104  *
00105  *     2. Estimate the ECEF position for each satellite measured using
00106  *     the downloaded ephemeris
00107  *
00108  *     3. Compute the Jacobian of pseudorange versus estimated state.
00109  *     There's no explicit differentiation; it's done symbolically
00110  *     first and just coded as a "line of sight" vector.
00111  *
00112  *     4. Get the inverse of the Jacobian times its transpose.  This
00113  *     matrix is normalized to one, but it tells us the direction we
00114  *     must move the state estimate during this step.
00115  *
00116  *     5. Multiply this inverse matrix (H) by the transpose of the
00117  *     Jacobian (to yield X).  This maps the direction of our state
00118  *     error into a direction of pseudorange error.
00119  *
00120  *     6. Multiply this matrix (X) by the error between the estimated
00121  *     (ephemeris) position and the measured pseudoranges.  This
00122  *     yields a vector of corrections to our state estimate.  We apply
00123  *     these to our current estimate and recurse to the next step.
00124  *
00125  *     7. If our corrections are very small, we've arrived at a good
00126  *     enough solution.  Solve for the receiver's velocity (with
00127  *     vel_solve) and do some bookkeeping to pass the solution back
00128  *     out.
00129  */
00130 static double pvt_solve(double rx_state[],
00131                         const u8 n_used,
00132                         const navigation_measurement_t nav_meas[n_used],
00133                         double H[4][4])
00134 {
00135   double p_pred[n_used];
00136 
00137   /* Vector of prediction errors */
00138   double omp[n_used];
00139 
00140   /* G is a geometry matrix tells us how our pseudoranges relate to
00141    * our state estimates -- it's the Jacobian of d(p_i)/d(x_j) where
00142    * x_j are x, y, z, Δt. */
00143   double G[n_used][4];
00144   double Gtrans[4][n_used];
00145   double GtG[4][4];
00146 
00147   /* H is the square of the Jacobian matrix; it tells us the shape of
00148      our error (or, if you prefer, the direction in which we need to
00149      move to get a better solution) in terms of the receiver state. */
00150 
00151   /* X is just H * Gtrans -- it maps our pseudoranges onto our
00152    * Jacobian update */
00153   double X[4][n_used];
00154 
00155   double tempv[3];
00156   double los[3];
00157   double xk_new[3];
00158   double tempd;
00159   double correction[4];
00160 
00161   for (u8 j=0; j<4; j++) {
00162     correction[j] = 0.0;
00163   }
00164 
00165   for (u8 j = 0; j < n_used; j++) {
00166     /* The satellite positions need to be corrected for Earth's rotation during
00167      * the signal time of flight. */
00168     /* TODO: Explain more about how this corrects for the Sagnac effect. */
00169 
00170     /* Magnitude of range vector converted into an approximate time in secs. */
00171     vector_subtract(3, rx_state, nav_meas[j].sat_pos, tempv);
00172     double tau = vector_norm(3, tempv) / GPS_C;
00173 
00174     /* Rotation of Earth during time of flight in radians. */
00175     double wEtau = GPS_OMEGAE_DOT * tau;
00176 
00177     /* Apply linearlised rotation about Z-axis which will adjust for the
00178      * satellite's position at time t-tau. Note the rotation is through
00179      * -wEtau because it is the ECEF frame that is rotating with the Earth and
00180      * hence in the ECEF frame free falling bodies appear to rotate in the
00181      * opposite direction.
00182      *
00183      * Making a small angle approximation here leads to less than 1mm error in
00184      * the satellite position. */
00185     xk_new[0] = nav_meas[j].sat_pos[0] + wEtau * nav_meas[j].sat_pos[1];
00186     xk_new[1] = nav_meas[j].sat_pos[1] - wEtau * nav_meas[j].sat_pos[0];
00187     xk_new[2] = nav_meas[j].sat_pos[2];
00188 
00189     /* Line of sight vector. */
00190     vector_subtract(3, xk_new, rx_state, los);
00191 
00192     /* Predicted range from satellite position and estimated Rx position. */
00193     p_pred[j] = vector_norm(3, los);
00194 
00195     /* omp means "observed minus predicted" range -- this is E, the
00196      * prediction error vector (or innovation vector in Kalman/LS
00197      * filtering terms).
00198      */
00199     omp[j] = nav_meas[j].pseudorange - p_pred[j];
00200 
00201     /* Construct a geometry matrix.  Each row (satellite) is
00202      * independently normalized into a unit vector. */
00203     for (u8 i=0; i<3; i++) {
00204       G[j][i] = -los[i] / p_pred[j];
00205     }
00206 
00207     /* Set time covariance to 1. */
00208     G[j][3] = 1;
00209 
00210   } /* End of channel loop. */
00211 
00212   /* Solve for position corrections using batch least-squares.  When
00213    * all-at-once least-squares estimation for a nonlinear problem is
00214    * mixed with numerical iteration (not time-series recursion, but
00215    * iteration on a single set of measurements), it's basically
00216    * Newton's method.  There's a reasonably clear explanation of this
00217    * in Wikipedia's article on GPS.
00218    */
00219 
00220   /* Gt := G^{T} */
00221   matrix_transpose(n_used, 4, (double *) G, (double *) Gtrans);
00222   /* GtG := G^{T} G */
00223   matrix_multiply(4, n_used, 4, (double *) Gtrans, (double *) G, (double *) GtG);
00224   /* H \elem \mathbb{R}^{4 \times 4} := GtG^{-1} */
00225   matrix_inverse(4, (const double *) GtG, (double *) H);
00226   /* X := H * G^{T} */
00227   matrix_multiply(4, 4, n_used, (double *) H, (double *) Gtrans, (double *) X);
00228   /* correction := X * E (= X * omp) */
00229   matrix_multiply(4, n_used, 1, (double *) X, (double *) omp, (double *) correction);
00230 
00231   /* Increment ecef estimate by the new corrections */
00232   for (u8 i=0; i<3; i++) {
00233     rx_state[i] += correction[i];
00234   }
00235 
00236   /* Set the Δt estimates according to this solution */
00237   for (u8 i=3; i<4; i++) {
00238     rx_state[i] = correction[i];
00239   }
00240 
00241   /* Look at the magnintude of the correction to see if
00242    * the solution has converged yet.
00243    */
00244   tempd = vector_norm(3, correction);
00245   if(tempd > 0.001) {
00246     /* The solution has not converged, return a negative value to
00247      * indicate that we should continue iterating.
00248      */
00249     return -tempd;
00250   }
00251 
00252   /* The solution has converged! */
00253 
00254   /* Perform the velocity solution. */
00255   vel_solve(&rx_state[4], n_used, nav_meas, (const double (*)[4]) G, (const double (*)[n_used]) X);
00256 
00257   return tempd;
00258 }
00259 
00260 u8 filter_solution(gnss_solution* soln, dops_t* dops)
00261 {
00262   if (dops->pdop > 50.0)
00263     /* PDOP is too high to yeild a good solution. */
00264     return 1;
00265 
00266   if (soln->pos_llh[2] < -1e3 || soln->pos_llh[2] > 1e8)
00267     /* Altitude is unreasonable. */
00268     return 2;
00269 
00270   /* NOTE: The following condition is required to comply with US export
00271    * regulations. It must not be removed. Any modification to this condition
00272    * is strictly not approved by Swift Navigation Inc. */
00273 
00274   if (vector_norm(3, soln->vel_ecef) >= 0.514444444*1000)
00275     /* Velocity is greater than 1000kts. */
00276     return 3;
00277 
00278   return 0;
00279 }
00280 
00281 s8 calc_PVT(const u8 n_used,
00282             const navigation_measurement_t nav_meas[n_used],
00283             gnss_solution *soln,
00284             dops_t *dops)
00285 {
00286   /* Initial state is the center of the Earth with zero velocity and zero
00287    * clock error, if we have some a priori position estimate we could use
00288    * that here to speed convergence a little on the first iteration.
00289    *
00290    *  rx_state format:
00291    *    pos[3], clock error, vel[3], intermediate freq error
00292    */
00293   static double rx_state[8];
00294 
00295   double H[4][4];
00296 
00297   soln->valid = 0;
00298 
00299   soln->n_used = n_used; // Keep track of number of working channels
00300 
00301   /* reset state to zero !? */
00302   for(u8 i=4; i<8; i++) {
00303     rx_state[i] = 0;
00304   }
00305 
00306   double update;
00307   u8 iters;
00308   /* Newton-Raphson iteration. */
00309   for (iters=0; iters<PVT_MAX_ITERATIONS; iters++) {
00310     if ((update = pvt_solve(rx_state, n_used, nav_meas, H)) > 0) {
00311       break;
00312     }
00313   }
00314 
00315   /* Compute various dilution of precision metrics. */
00316   compute_dops((const double(*)[4])H, rx_state, dops);
00317   soln->err_cov[6] = dops->gdop;
00318 
00319   /* Populate error covariances according to layout in definition
00320    * of gnss_solution struct.
00321    */
00322   soln->err_cov[0] = H[0][0];
00323   soln->err_cov[1] = H[0][1];
00324   soln->err_cov[2] = H[0][2];
00325   soln->err_cov[3] = H[1][1];
00326   soln->err_cov[4] = H[1][2];
00327   soln->err_cov[5] = H[2][2];
00328 
00329   if (iters >= PVT_MAX_ITERATIONS) {
00330     /* Reset state if solution fails */
00331     rx_state[0] = 0;
00332     rx_state[1] = 0;
00333     rx_state[2] = 0;
00334     return -4;
00335   }
00336 
00337   /* Save as x, y, z. */
00338   for (u8 i=0; i<3; i++) {
00339     soln->pos_ecef[i] = rx_state[i];
00340     soln->vel_ecef[i] = rx_state[4+i];
00341   }
00342 
00343   wgsecef2ned(soln->vel_ecef, soln->pos_ecef, soln->vel_ned);
00344 
00345   /* Convert to lat, lon, hgt. */
00346   wgsecef2llh(rx_state, soln->pos_llh);
00347 
00348   soln->clock_offset = rx_state[3] / GPS_C;
00349   soln->clock_bias = rx_state[7] / GPS_C;
00350 
00351   /* Time at receiver is TOT plus time of flight. Time of flight is eqaul to
00352    * the pseudorange minus the clock bias. */
00353   soln->time = nav_meas[0].tot;
00354   soln->time.tow += nav_meas[0].pseudorange / GPS_C;
00355   /* Subtract clock offset. */
00356   soln->time.tow -= rx_state[3] / GPS_C;
00357   soln->time = normalize_gps_time(soln->time);
00358 
00359   u8 ret;
00360   if ((ret = filter_solution(soln, dops))) {
00361     memset(soln, 0, sizeof(*soln));
00362     /* Reset state if solution fails */
00363     rx_state[0] = 0;
00364     rx_state[1] = 0;
00365     rx_state[2] = 0;
00366     return -ret;
00367   }
00368 
00369   soln->valid = 1;
00370   return 0;
00371 }
00372 


swiftnav
Author(s):
autogenerated on Sat Jun 8 2019 18:55:56