Ejemplo n.º 1
0
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();
}
Ejemplo n.º 2
0
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
    );
  }

}