void clock_est_update(clock_est_state_t *s, gps_time_t meas_gpst, double meas_clock_period, double localt, double q, double r_gpst, double r_clock_period) { double temp[2][2]; double phi_t_0[2][2] = {{1, localt}, {0, 1}}; double phi_t_0_tr[2][2]; matrix_transpose(2, 2, (const double *)phi_t_0, (double *)phi_t_0_tr); double P_[2][2]; memcpy(P_, s->P, sizeof(P_)); P_[0][0] += q; double y[2]; gps_time_t pred_gpst = s->t0_gps; pred_gpst.tow += localt * s->clock_period; pred_gpst = normalize_gps_time(pred_gpst); y[0] = gpsdifftime(meas_gpst, pred_gpst); y[1] = meas_clock_period - s->clock_period; double S[2][2]; matrix_multiply(2, 2, 2, (const double *)phi_t_0, (const double *)P_, (double *)temp); matrix_multiply(2, 2, 2, (const double *)temp, (const double *)phi_t_0_tr, (double *)S); S[0][0] += r_gpst; S[1][1] += r_clock_period; double Sinv[2][2]; matrix_inverse(2, (const double *)S, (double *)Sinv); double K[2][2]; matrix_multiply(2, 2, 2, (const double *)P_, (const double *)phi_t_0_tr, (double *)temp); matrix_multiply(2, 2, 2, (const double *)temp, (const double *)Sinv, (double *)K); double dx[2]; matrix_multiply(2, 2, 1, (const double *)K, (const double *)y, (double *)dx); s->t0_gps.tow += dx[0]; s->t0_gps = normalize_gps_time(s->t0_gps); s->clock_period += dx[1]; matrix_multiply(2, 2, 2, (const double *)K, (const double *)phi_t_0, (double *)temp); temp[0][0] = 1 - temp[0][0]; temp[0][1] = -temp[0][1]; temp[1][1] = 1 - temp[1][1]; temp[1][0] = -temp[1][0]; matrix_multiply(2, 2, 2, (const double *)temp, (const double *)P_, (double *)s->P); }
/** Convert receiver time to GPS time. * * \note The GPS time may only be a guess or completely unknown. time_quality * should be checked first to determine the quality of the GPS time * estimate. * * \param tc Timing count in units of RX_DT_NOMINAL. * \return GPS time corresponding to Timing count. */ gps_time_t rx2gpstime(double tc) { gps_time_t t = clock_state.t0_gps; t.tow += tc * clock_state.clock_period; t = normalize_gps_time(t); return t; }
/** Update GPS time estimate precisely referenced to the local receiver time. * * \param tc SwiftNAP timing count. * \param t GPS time estimate associated with timing count. */ void set_time_fine(u64 tc, gps_time_t t) { clock_state.t0_gps = t; clock_state.t0_gps.tow -= tc * RX_DT_NOMINAL; clock_state.t0_gps = normalize_gps_time(clock_state.t0_gps); time_quality = TIME_FINE; }
/** Update GPS time estimate. * * This function may be called to update the GPS time estimate. If the time is * already known more precisely than the new estimate, the new estimate will be * ignored. * * This function should not be used to give an estimate with TIME_FINE quality * as this must be referenced to a particular value of the SwiftNAP timing * count. * * \param quality Quality of the time estimate. * \param t GPS time estimate. */ void set_time(time_quality_t quality, gps_time_t t) { if (quality > time_quality) { clock_state.t0_gps = t; clock_state.t0_gps.tow -= nap_timing_count() * RX_DT_NOMINAL; clock_state.t0_gps = normalize_gps_time(clock_state.t0_gps); time_quality = quality; time_t unix_t = gps2time(t); log_info("Time set to: %s (quality=%d)", ctime(&unix_t), quality); } }
void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[], navigation_measurement_t* nav_meas[], double nav_time, ephemeris_t* ephemerides[]) { double TOTs[n_channels]; double min_TOT = DBL_MAX; for (u8 i=0; i<n_channels; i++) { TOTs[i] = 1e-3 * meas[i]->time_of_week_ms; TOTs[i] += meas[i]->code_phase_chips / 1.023e6; TOTs[i] += (nav_time - meas[i]->receiver_time) * meas[i]->code_phase_rate / 1.023e6; /** \todo Handle GPS time properly here, e.g. week rollover */ nav_meas[i]->tot.wn = ephemerides[i]->toe.wn; nav_meas[i]->tot.tow = TOTs[i]; if (gpsdifftime(nav_meas[i]->tot, ephemerides[i]->toe) > 3*24*3600) nav_meas[i]->tot.wn -= 1; if (TOTs[i] < min_TOT) min_TOT = TOTs[i]; nav_meas[i]->raw_doppler = meas[i]->carrier_freq; nav_meas[i]->snr = meas[i]->snr; nav_meas[i]->prn = meas[i]->prn; nav_meas[i]->carrier_phase = meas[i]->carrier_phase; nav_meas[i]->carrier_phase += (nav_time - meas[i]->receiver_time) * meas[i]->carrier_freq; nav_meas[i]->lock_counter = meas[i]->lock_counter; } double clock_err, clock_rate_err; for (u8 i=0; i<n_channels; i++) { nav_meas[i]->raw_pseudorange = (min_TOT - TOTs[i])*GPS_C + GPS_NOMINAL_RANGE; calc_sat_pos(nav_meas[i]->sat_pos, nav_meas[i]->sat_vel, &clock_err, &clock_rate_err, ephemerides[i], nav_meas[i]->tot); nav_meas[i]->pseudorange = nav_meas[i]->raw_pseudorange \ + clock_err*GPS_C; nav_meas[i]->doppler = nav_meas[i]->raw_doppler + clock_rate_err*GPS_L1_HZ; nav_meas[i]->tot.tow -= clock_err; nav_meas[i]->tot = normalize_gps_time(nav_meas[i]->tot); } }
s8 calc_PVT(const u8 n_used, const navigation_measurement_t const nav_meas[n_used], gnss_solution *soln, dops_t *dops) { /* Initial state is the center of the Earth with zero velocity and zero * clock error, if we have some a priori position estimate we could use * that here to speed convergence a little on the first iteration. * * rx_state format: * pos[3], clock error, vel[3], intermediate freq error */ static double rx_state[8]; double H[4][4]; soln->valid = 0; soln->n_used = n_used; // Keep track of number of working channels /* reset state to zero !? */ for(u8 i=4; i<8; i++) { rx_state[i] = 0; } double update; u8 iters; /* Newton-Raphson iteration. */ for (iters=0; iters<PVT_MAX_ITERATIONS; iters++) { if ((update = pvt_solve(rx_state, n_used, nav_meas, H)) > 0) { break; } } /* Compute various dilution of precision metrics. */ compute_dops((const double(*)[4])H, rx_state, dops); soln->err_cov[6] = dops->gdop; /* Populate error covariances according to layout in definition * of gnss_solution struct. */ soln->err_cov[0] = H[0][0]; soln->err_cov[1] = H[0][1]; soln->err_cov[2] = H[0][2]; soln->err_cov[3] = H[1][1]; soln->err_cov[4] = H[1][2]; soln->err_cov[5] = H[2][2]; if (iters >= PVT_MAX_ITERATIONS) { /* Reset state if solution fails */ rx_state[0] = 0; rx_state[1] = 0; rx_state[2] = 0; return -4; } /* Save as x, y, z. */ for (u8 i=0; i<3; i++) { soln->pos_ecef[i] = rx_state[i]; soln->vel_ecef[i] = rx_state[4+i]; } wgsecef2ned(soln->vel_ecef, soln->pos_ecef, soln->vel_ned); /* Convert to lat, lon, hgt. */ wgsecef2llh(rx_state, soln->pos_llh); soln->clock_offset = rx_state[3] / GPS_C; soln->clock_bias = rx_state[7] / GPS_C; /* Time at receiver is TOT plus time of flight. Time of flight is eqaul to * the pseudorange minus the clock bias. */ soln->time = nav_meas[0].tot; soln->time.tow += nav_meas[0].pseudorange / GPS_C; /* Subtract clock offset. */ soln->time.tow -= rx_state[3] / GPS_C; soln->time = normalize_gps_time(soln->time); u8 ret; if ((ret = filter_solution(soln, dops))) { memset(soln, 0, sizeof(*soln)); /* Reset state if solution fails */ rx_state[0] = 0; rx_state[1] = 0; rx_state[2] = 0; return -ret; } soln->valid = 1; return 0; }
/** Try to calculate a single point gps solution * * \param n_used number of measurments * \param nav_meas array of measurements * \param disable_raim passing True will omit raim check/repair functionality * \param soln output solution struct * \param dops output doppler information * \return Non-negative values indicate a valid solution. * - `2`: Solution converged but RAIM unavailable or disabled * - `1`: Solution converged, failed RAIM but was successfully repaired * - `0`: Solution converged and verified by RAIM * - `-1`: PDOP is too high to yield a good solution. * - `-2`: Altitude is unreasonable. * - `-3`: Velocity is greater than or equal to 1000 kts. * - `-4`: RAIM check failed and repair was unsuccessful * - `-5`: RAIM check failed and repair was impossible (not enough measurements) * - `-6`: pvt_iter didn't converge * - `-7`: < 4 measurements */ s8 calc_PVT(const u8 n_used, const navigation_measurement_t nav_meas[n_used], bool disable_raim, gnss_solution *soln, dops_t *dops) { /* Initial state is the center of the Earth with zero velocity and zero * clock error, if we have some a priori position estimate we could use * that here to speed convergence a little on the first iteration. * * rx_state format: * pos[3], clock error, vel[3], intermediate freq error */ static double rx_state[8]; double H[4][4]; if (n_used < 4) { return -7; } soln->valid = 0; soln->n_used = n_used; // Keep track of number of working channels gnss_signal_t removed_sid; s8 raim_flag = pvt_solve_raim(rx_state, n_used, nav_meas, disable_raim, H, &removed_sid, 0); if (raim_flag < 0) { /* Didn't converge or least squares integrity check failed. */ return raim_flag - 3; } /* Initial solution failed, but repair was successful. */ if (raim_flag == 1) { soln->n_used--; } /* Compute various dilution of precision metrics. */ compute_dops((const double(*)[4])H, rx_state, dops); soln->err_cov[6] = dops->gdop; /* Populate error covariances according to layout in definition * of gnss_solution struct. */ soln->err_cov[0] = H[0][0]; soln->err_cov[1] = H[0][1]; soln->err_cov[2] = H[0][2]; soln->err_cov[3] = H[1][1]; soln->err_cov[4] = H[1][2]; soln->err_cov[5] = H[2][2]; /* Save as x, y, z. */ for (u8 i=0; i<3; i++) { soln->pos_ecef[i] = rx_state[i]; soln->vel_ecef[i] = rx_state[4+i]; } wgsecef2ned(soln->vel_ecef, soln->pos_ecef, soln->vel_ned); /* Convert to lat, lon, hgt. */ wgsecef2llh(rx_state, soln->pos_llh); soln->clock_offset = rx_state[3] / GPS_C; soln->clock_bias = rx_state[7] / GPS_C; /* Time at receiver is TOT plus time of flight. Time of flight is eqaul to * the pseudorange minus the clock bias. */ soln->time = nav_meas[0].tot; soln->time.tow += nav_meas[0].pseudorange / GPS_C; /* Subtract clock offset. */ soln->time.tow -= rx_state[3] / GPS_C; soln->time = normalize_gps_time(soln->time); u8 ret; if ((ret = filter_solution(soln, dops))) { memset(soln, 0, sizeof(*soln)); /* Reset position elements of state if solution fails. */ rx_state[0] = 0; rx_state[1] = 0; rx_state[2] = 0; return -ret; } soln->valid = 1; return raim_flag; }