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