void dgnss_incorporate_observation(sdiff_t *sdiffs, double * dd_measurements,
double *reciever_ecef, double dt)
{
(void) dt;
double b2[3];
least_squares_solve_b(&nkf, sdiffs, dd_measurements, reciever_ecef, b2);
double ref_ecef[3];
ref_ecef[0] = reciever_ecef[0] + 0.5 * b2[0];
ref_ecef[1] = reciever_ecef[1] + 0.5 * b2[0];
ref_ecef[2] = reciever_ecef[2] + 0.5 * b2[0];
/* TODO: make a common DE and use it instead. */
assign_decor_obs_mtx(sats_management.num_sats, sdiffs, ref_ecef,
kf.decor_mtx, kf.decor_obs_mtx);
set_nkf_matrices(&nkf,
dgnss_settings.phase_var_kf, dgnss_settings.code_var_kf,
sats_management.num_sats, sdiffs, ref_ecef);
kalman_filter_update(&kf, dd_measurements);
nkf_update(&nkf, dd_measurements);
}
/** Constructs a low latency float baseline measurement.
* The sdiffs have no particular reason (other than a general tendency
* brought by hysteresis) to match up with the float filter's sats, so we have
* to check if we can solve. For now, unless the sdiffs are a superset of the
* float sats, we don't solve.
*
* Requires num_sdiffs >= 4 and (global) sats_management.num_sats >= 4.
*
* \TODO solve whenever the information is there.
*
* \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 function into the KF, once we pull the sats_management struct
* into the KF too. When we do, do the same for the IAR low lat solution.
*
* \param num_sdiffs The number of sdiffs input. Must be >= 4.
* \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_float_baseline(u8 num_sdiffs, sdiff_t *sdiffs,
double ref_ecef[3], u8 *num_used, double b[3])
{
DEBUG_ENTRY();
if (num_sdiffs < 4 || sats_management.num_sats < 4) {
/* For a position solution, we need at least 4 sats. That means we must
* have at least 4 sats in common between what the KF is tracking and
* the sdiffs we give this function. If either is less than 4,
* this criterion cannot be satisfied. */
log_debug("Low latency solution can't be computed. Too few observations"
" or too few sats in the current filter.\n");
DEBUG_EXIT();
return -1;
}
double float_dd_measurements[2 * (sats_management.num_sats - 1)];
sdiff_t float_sdiffs[sats_management.num_sats];
s8 can_make_obs = make_dd_measurements_and_sdiffs(sats_management.prns[0],
&sats_management.prns[1], sats_management.num_sats - 1,
num_sdiffs, sdiffs,
float_dd_measurements, float_sdiffs);
if (can_make_obs == -1) {
log_debug("make_float_dd_measurements_and_sdiffs has error code -1\n");
DEBUG_EXIT();
return -1;
}
least_squares_solve_b(&nkf, float_sdiffs, float_dd_measurements,
ref_ecef, b);
*num_used = sats_management.num_sats;
DEBUG_EXIT();
return 0;
}
void dgnss_new_float_baseline(u8 num_sats, sdiff_t *sdiffs, double receiver_ecef[3], u8 *num_used, double b[3])
{
sdiff_t corrected_sdiffs[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 corrected_sdiffs accordingly)
dgnss_rebase_ref(num_sats, sdiffs, receiver_ecef, old_prns, corrected_sdiffs);
double dd_measurements[2*(num_sats-1)];
make_measurements(num_sats-1, corrected_sdiffs, dd_measurements);
least_squares_solve_b(&nkf, corrected_sdiffs, dd_measurements, receiver_ecef, b);
*num_used = sats_management.num_sats;
}
void dgnss_update(u8 num_sats, sdiff_t *sdiffs, double reciever_ecef[3], double dt)
{
sdiff_t corrected_sdiffs[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 corrected_sdiffs accordingly)
dgnss_rebase_ref(num_sats, sdiffs, reciever_ecef, old_prns, corrected_sdiffs);
double dd_measurements[2*(num_sats-1)];
make_measurements(num_sats-1, corrected_sdiffs, dd_measurements);
//all the added/dropped sat stuff
dgnss_update_sats(num_sats, reciever_ecef, corrected_sdiffs, dd_measurements, dt);
/*printf("done updating sats\n");*/
/*MAT_PRINTF(kf.decor_obs_mtx, kf.obs_dim, kf.state_dim);*/
if (num_sats >= 5) {
// update for observation
dgnss_incorporate_observation(corrected_sdiffs, dd_measurements, reciever_ecef, dt);
}
double b2[3];
least_squares_solve_b(&nkf, corrected_sdiffs, dd_measurements, reciever_ecef, b2);
double ref_ecef[3];
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];
/*
update_ambiguity_test(ref_ecef,
dgnss_settings.phase_var_test,
dgnss_settings.code_var_test,
&ambiguity_test,
kf.state_dim, &sats_management, sdiffs,
kf.state_mean, kf.state_cov_U, kf.state_cov_D);
*/
update_ambiguity_test(ref_ecef,
dgnss_settings.phase_var_test,
dgnss_settings.code_var_test,
&ambiguity_test, nkf.state_dim, &sats_management,
sdiffs, nkf.state_mean, nkf.state_cov_U,
nkf.state_cov_D);
}
static void dgnss_incorporate_observation(sdiff_t *sdiffs, double * dd_measurements,
double *reciever_ecef)
{
DEBUG_ENTRY();
double b2[3];
least_squares_solve_b(&nkf, sdiffs, dd_measurements, reciever_ecef, b2);
double ref_ecef[3];
ref_ecef[0] = reciever_ecef[0] + 0.5 * b2[0];
ref_ecef[1] = reciever_ecef[1] + 0.5 * b2[0];
ref_ecef[2] = reciever_ecef[2] + 0.5 * b2[0];
/* TODO: make a common DE and use it instead. */
set_nkf_matrices(&nkf,
dgnss_settings.phase_var_kf, dgnss_settings.code_var_kf,
sats_management.num_sats, sdiffs, ref_ecef);
nkf_update(&nkf, dd_measurements);
DEBUG_EXIT();
}
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();
}
