/* Update ambiguity states from filter states. * Updates the set of fixed and float ambiguities using the current filter * state. * * \param s Pointer to ambiguity state structure */ void dgnss_update_ambiguity_state(ambiguity_state_t *s) { /* Float filter */ /* NOTE: if sats_management.num_sats <= 1 the filter is not updated and * nkf.state_dim may not match. */ if (sats_management.num_sats > 1) { assert(sats_management.num_sats == nkf.state_dim+1); s->float_ambs.n = nkf.state_dim; memcpy(s->float_ambs.sids, sats_management.sids, (nkf.state_dim+1) * sizeof(gnss_signal_t)); memcpy(s->float_ambs.ambs, nkf.state_mean, nkf.state_dim * sizeof(double)); } else { s->float_ambs.n = 0; } /* Fixed filter */ if (ambiguity_iar_can_solve(&ambiguity_test)) { s->fixed_ambs.n = ambiguity_test.amb_check.num_matching_ndxs; s->fixed_ambs.sids[0] = ambiguity_test.sats.sids[0]; for (u8 i=0; i < s->fixed_ambs.n; i++) { s->fixed_ambs.sids[i + 1] = ambiguity_test.sats.sids[1 + ambiguity_test.amb_check.matching_ndxs[i]]; s->fixed_ambs.ambs[i] = ambiguity_test.amb_check.ambs[i]; } } else { s->fixed_ambs.n = 0; } }
/** Constructs a low latency IAR resolved baseline measurement. * The sdiffs have no particular reason (other than a general tendency * brought by hysteresis) to match up with the IAR sats, so we have * to check if we can solve. For now, unless the sdiffs are a superset of the * IAR sats, we don't solve. * * Requires num_sdiffs >= 4. * * \TODO, solve whenever we can * * \TODO since we're now using make_dd_measurements_and_sdiffs outside of the * amb_test context, pull it into another file. * * \TODO pull this into the IAR file when we do the same for the float low lat * solution. * * \todo This function is identical to dgnss_fixed_baseline()? * * \param num_sdiffs The number of sdiffs input. * \param sdiffs The sdiffs used to measure. (These should be a superset * of the float sats). * \param ref_ecef The reference position used for solving, and making * observation matrices. * \param num_used The number of sats actually used to compute the baseline. * \param b The baseline computed. * \return -1 if it can't solve. * 0 If it can solve. */ s8 _dgnss_low_latency_IAR_baseline(u8 num_sdiffs, sdiff_t *sdiffs, double ref_ecef[3], u8 *num_used, double b[3]) { DEBUG_ENTRY(); assert(num_sdiffs >= 4); if (!ambiguity_iar_can_solve(&ambiguity_test)) { DEBUG_EXIT(); return -1; } sdiff_t ambiguity_sdiffs[ambiguity_test.amb_check.num_matching_ndxs+1]; double dd_meas[2 * ambiguity_test.amb_check.num_matching_ndxs]; s8 valid_sdiffs = make_ambiguity_resolved_dd_measurements_and_sdiffs( &ambiguity_test, num_sdiffs, sdiffs, dd_meas, ambiguity_sdiffs); if (valid_sdiffs != 0) { if (valid_sdiffs != -1) { log_error("_dgnss_low_latency_IAR_baseline: Invalid sdiffs."); } DEBUG_EXIT(); return -1; } /* TODO: check internals of this if's content and abstract it from the KF */ double DE[ambiguity_test.amb_check.num_matching_ndxs * 3]; assign_de_mtx(ambiguity_test.amb_check.num_matching_ndxs + 1, ambiguity_sdiffs, ref_ecef, DE); *num_used = ambiguity_test.amb_check.num_matching_ndxs + 1; s8 ret = lesq_solution_int(ambiguity_test.amb_check.num_matching_ndxs, dd_meas, ambiguity_test.amb_check.ambs, DE, b, 0); if (ret) { log_error("_dgnss_low_latency_IAR_baseline: " "lesq_solution returned error %d\n", ret); DEBUG_EXIT(); return -1; } DEBUG_EXIT(); return 0; }
/* Returns the fixed baseline iff there are at least 3 dd ambs unanimously * agreed upon in the ambiguity_test. * \return 1 If fixed baseline calculation succeeds * 0 If iar cannot solve or an error occurs. Signals that float * baseline is needed instead. */ s8 dgnss_fixed_baseline(u8 num_sdiffs, sdiff_t *sdiffs, double ref_ecef[3], u8 *num_used, double b[3]) { if (!ambiguity_iar_can_solve(&ambiguity_test)) { return 0; } sdiff_t ambiguity_sdiffs[ambiguity_test.amb_check.num_matching_ndxs+1]; double dd_meas[2 * ambiguity_test.amb_check.num_matching_ndxs]; s8 valid_sdiffs = make_ambiguity_resolved_dd_measurements_and_sdiffs( &ambiguity_test, num_sdiffs, sdiffs, dd_meas, ambiguity_sdiffs); /* At this point, sdiffs should be valid due to dgnss_update * Return code not equal to 0 signals an error. */ if (valid_sdiffs != 0) { if (valid_sdiffs != -1) { log_error("dgnss_fixed_baseline: Invalid sdiffs."); } return 0; } double DE[ambiguity_test.amb_check.num_matching_ndxs * 3]; assign_de_mtx(ambiguity_test.amb_check.num_matching_ndxs + 1, ambiguity_sdiffs, ref_ecef, DE); *num_used = ambiguity_test.amb_check.num_matching_ndxs + 1; s8 ret = lesq_solution_int(ambiguity_test.amb_check.num_matching_ndxs, dd_meas, ambiguity_test.amb_check.ambs, DE, b, 0); if (ret) { log_error("dgnss_fixed_baseline: " "lesq_solution returned error %d\n", ret); DEBUG_EXIT(); return 0; } return 1; }
s8 dgnss_iar_resolved() { return ambiguity_iar_can_solve(&ambiguity_test); }
void dgnss_update(u8 num_sats, sdiff_t *sdiffs, double reciever_ecef[3]) { DEBUG_ENTRY(); if (DEBUG) { printf("sdiff[*].prn = {"); for (u8 i=0; i < num_sats; i++) { printf("%u, ",sdiffs[i].prn); } printf("}\n"); } if (num_sats <= 1) { sats_management.num_sats = num_sats; if (num_sats == 1) { sats_management.prns[0] = sdiffs[0].prn; } create_ambiguity_test(&ambiguity_test); DEBUG_EXIT(); return; } if (sats_management.num_sats <= 1) { dgnss_init(num_sats, sdiffs, reciever_ecef); } sdiff_t sdiffs_with_ref_first[num_sats]; u8 old_prns[MAX_CHANNELS]; memcpy(old_prns, sats_management.prns, sats_management.num_sats * sizeof(u8)); /* rebase globals to a new reference sat * (permutes sdiffs_with_ref_first accordingly) */ dgnss_rebase_ref(num_sats, sdiffs, reciever_ecef, old_prns, sdiffs_with_ref_first); double dd_measurements[2*(num_sats-1)]; make_measurements(num_sats-1, sdiffs_with_ref_first, dd_measurements); /* all the added/dropped sat stuff */ dgnss_update_sats(num_sats, reciever_ecef, sdiffs_with_ref_first, dd_measurements); double ref_ecef[3]; if (num_sats >= 5) { dgnss_incorporate_observation(sdiffs_with_ref_first, dd_measurements, reciever_ecef); double b2[3]; least_squares_solve_b(&nkf, sdiffs_with_ref_first, dd_measurements, reciever_ecef, b2); ref_ecef[0] = reciever_ecef[0] + 0.5 * b2[0]; ref_ecef[1] = reciever_ecef[1] + 0.5 * b2[1]; ref_ecef[2] = reciever_ecef[2] + 0.5 * b2[2]; } u8 changed_sats = ambiguity_update_sats(&ambiguity_test, num_sats, sdiffs, &sats_management, nkf.state_mean, nkf.state_cov_U, nkf.state_cov_D); update_ambiguity_test(ref_ecef, dgnss_settings.phase_var_test, dgnss_settings.code_var_test, &ambiguity_test, nkf.state_dim, sdiffs, changed_sats); update_unanimous_ambiguities(&ambiguity_test); if (DEBUG) { if (num_sats >=4) { double b3[3]; least_squares_solve_b(&nkf, sdiffs_with_ref_first, dd_measurements, reciever_ecef, b3); ref_ecef[0] = reciever_ecef[0] + 0.5 * b3[0]; ref_ecef[1] = reciever_ecef[1] + 0.5 * b3[1]; ref_ecef[2] = reciever_ecef[2] + 0.5 * b3[2]; double bb[3]; u8 num_used; dgnss_fixed_baseline(num_sats, sdiffs, ref_ecef, &num_used, bb); log_debug("\ndgnss_fixed_baseline:\nb = %f, \t%f, \t%f\nnum_used/num_sats = %u/%u\nusing_iar = %u\n\n", bb[0], bb[1], bb[2], num_used, num_sats, ambiguity_iar_can_solve(&ambiguity_test)); } } DEBUG_EXIT(); }