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