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