static void dgnss_update_sats(u8 num_sdiffs, double receiver_ecef[3], sdiff_t *sdiffs_with_ref_first, double *dd_measurements) { DEBUG_ENTRY(); (void)dd_measurements; gnss_signal_t new_sids[num_sdiffs]; sdiffs_to_sids(num_sdiffs, sdiffs_with_ref_first, new_sids); gnss_signal_t old_sids[MAX_CHANNELS]; memcpy(old_sids, sats_management.sids, sats_management.num_sats * sizeof(gnss_signal_t)); if (!sids_match(&old_sids[1], num_sdiffs-1, &sdiffs_with_ref_first[1])) { u8 ndx_of_intersection_in_old[sats_management.num_sats]; u8 ndx_of_intersection_in_new[sats_management.num_sats]; ndx_of_intersection_in_old[0] = 0; ndx_of_intersection_in_new[0] = 0; u8 num_intersection_sats = dgnss_intersect_sats( sats_management.num_sats-1, &old_sids[1], num_sdiffs-1, &sdiffs_with_ref_first[1], &ndx_of_intersection_in_old[1], &ndx_of_intersection_in_new[1]) + 1; set_nkf_matrices( &nkf, dgnss_settings.phase_var_kf, dgnss_settings.code_var_kf, num_sdiffs, sdiffs_with_ref_first, receiver_ecef ); if (num_intersection_sats < sats_management.num_sats) { /* we lost sats */ nkf_state_projection(&nkf, sats_management.num_sats-1, num_intersection_sats-1, &ndx_of_intersection_in_old[1]); } if (num_intersection_sats < num_sdiffs) { /* we gained sats */ double simple_estimates[num_sdiffs-1]; dgnss_simple_amb_meas(num_sdiffs, sdiffs_with_ref_first, simple_estimates); nkf_state_inclusion(&nkf, num_intersection_sats-1, num_sdiffs-1, &ndx_of_intersection_in_new[1], simple_estimates, dgnss_settings.new_int_var); } update_sats_sats_management(&sats_management, num_sdiffs-1, &sdiffs_with_ref_first[1]); } else { set_nkf_matrices( &nkf, dgnss_settings.phase_var_kf, dgnss_settings.code_var_kf, num_sdiffs, sdiffs_with_ref_first, receiver_ecef ); } DEBUG_EXIT(); }
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); }
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 receiver_ecef[3], bool disable_raim, double raim_threshold) { DEBUG_ENTRY(); if (DEBUG) { printf("sdiff[*].prn = {"); for (u8 i=0; i < num_sats; i++) { printf("%u, ", sdiffs[i].sid.sat); } printf("}\n"); } if (num_sats <= 1) { sats_management.num_sats = num_sats; if (num_sats == 1) { sats_management.sids[0] = sdiffs[0].sid; } create_ambiguity_test(&ambiguity_test); DEBUG_EXIT(); return; } if (sats_management.num_sats <= 1) { dgnss_init(num_sats, sdiffs, receiver_ecef); } sdiff_t sdiffs_with_ref_first[num_sats]; gnss_signal_t old_sids[MAX_CHANNELS]; memcpy(old_sids, sats_management.sids, sats_management.num_sats * sizeof(gnss_signal_t)); /* rebase globals to a new reference sat * (permutes sdiffs_with_ref_first accordingly) */ dgnss_rebase_ref(num_sats, sdiffs, receiver_ecef, old_sids, 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, receiver_ecef, sdiffs_with_ref_first, dd_measurements); /* Unless the KF says otherwise, DONT TRUST THE MEASUREMENTS */ u8 is_bad_measurement = true; double ref_ecef[3]; if (num_sats >= 5) { double b2[3]; s8 code = least_squares_solve_b_external_ambs(nkf.state_dim, nkf.state_mean, sdiffs_with_ref_first, dd_measurements, receiver_ecef, b2, disable_raim, raim_threshold); if (code < 0) { log_warn("dgnss_update. baseline estimate error: %d", code); /* Use b = 0, continue */ memset(b2, 0, sizeof(b2)); } double ref_ecef[3]; vector_add_sc(3, receiver_ecef, b2, 0.5, ref_ecef); /* 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_with_ref_first, ref_ecef); is_bad_measurement = nkf_update(&nkf, dd_measurements); } u8 changed_sats = ambiguity_update_sats(&ambiguity_test, num_sats, sdiffs, &sats_management, nkf.state_mean, nkf.state_cov_U, nkf.state_cov_D, is_bad_measurement); /* TODO: Refactor - looks like ref_ecef can be passed in uninitialized */ if (!is_bad_measurement) { 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); DEBUG_EXIT(); }
void dgnss_update_sats(u8 num_sdiffs, double reciever_ecef[3], sdiff_t *corrected_sdiffs, double *dd_measurements, double dt) { (void)dd_measurements; u8 new_prns[num_sdiffs]; sdiffs_to_prns(num_sdiffs, corrected_sdiffs, new_prns); u8 old_prns[MAX_CHANNELS]; memcpy(old_prns, sats_management.prns, sats_management.num_sats * sizeof(u8)); if (!prns_match(&old_prns[1], num_sdiffs-1, &corrected_sdiffs[1])) { u8 ndx_of_intersection_in_old[sats_management.num_sats]; u8 ndx_of_intersection_in_new[sats_management.num_sats]; ndx_of_intersection_in_old[0] = 0; ndx_of_intersection_in_new[0] = 0; u8 num_intersection_sats = dgnss_intersect_sats( sats_management.num_sats-1, &old_prns[1], num_sdiffs-1, &corrected_sdiffs[1], &ndx_of_intersection_in_old[1], &ndx_of_intersection_in_new[1]) + 1; reset_kf_except_state( &kf, dgnss_settings.phase_var_kf, dgnss_settings.code_var_kf, dgnss_settings.pos_trans_var, dgnss_settings.vel_trans_var, dgnss_settings.int_trans_var, num_sdiffs, corrected_sdiffs, reciever_ecef, dt ); set_nkf_matrices( &nkf, dgnss_settings.phase_var_kf, dgnss_settings.code_var_kf, num_sdiffs, corrected_sdiffs, reciever_ecef ); if (num_intersection_sats < sats_management.num_sats) { //lost sats kalman_filter_state_projection(&kf, sats_management.num_sats-1, num_intersection_sats-1, &ndx_of_intersection_in_old[1]); nkf_state_projection(&nkf, sats_management.num_sats-1, num_intersection_sats-1, &ndx_of_intersection_in_old[1]); } if (num_intersection_sats < num_sdiffs) { //gained sats kalman_filter_state_inclusion(&kf, num_intersection_sats-1, num_sdiffs-1, &ndx_of_intersection_in_new[1], dgnss_settings.new_int_var); nkf_state_inclusion(&nkf, num_intersection_sats-1, num_sdiffs-1, &ndx_of_intersection_in_new[1], dgnss_settings.new_int_var); } /*print_sats_management(&sats_management);*/ update_sats_sats_management(&sats_management, num_sdiffs-1, &corrected_sdiffs[1]); /*print_sats_management(&sats_management);*/ } else { reset_kf_except_state( &kf, dgnss_settings.phase_var_kf, dgnss_settings.code_var_kf, dgnss_settings.pos_trans_var, dgnss_settings.vel_trans_var, dgnss_settings.int_trans_var, num_sdiffs, corrected_sdiffs, reciever_ecef, dt ); set_nkf_matrices( &nkf, dgnss_settings.phase_var_kf, dgnss_settings.code_var_kf, num_sdiffs, corrected_sdiffs, reciever_ecef ); } }