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;
}
