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