/* 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();
}
