void dgnss_init_known_baseline(u8 num_sats, sdiff_t *sdiffs,
double receiver_ecef[3], double b[3])
{
double ref_ecef[3];
vector_add_sc(3, receiver_ecef, b, 0.5, ref_ecef);
sdiff_t corrected_sdiffs[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 corrected_sdiffs accordingly) */
dgnss_rebase_ref(num_sats, sdiffs, ref_ecef, old_sids, corrected_sdiffs);
double dds[2*(num_sats-1)];
make_measurements(num_sats-1, corrected_sdiffs, dds);
double DE[(num_sats-1)*3];
assign_de_mtx(num_sats, corrected_sdiffs, ref_ecef, DE);
dgnss_reset_iar();
memcpy(&ambiguity_test.sats, &sats_management, sizeof(sats_management));
hypothesis_t *hyp = (hypothesis_t *)memory_pool_add(ambiguity_test.pool);
hyp->ll = 0;
amb_from_baseline(num_sats-1, DE, dds, b, hyp->N);
double obs_cov[(num_sats-1) * (num_sats-1) * 4];
memset(obs_cov, 0, (num_sats-1) * (num_sats-1) * 4 * sizeof(double));
u8 num_dds = num_sats-1;
for (u8 i=0; i<num_dds; i++) {
for (u8 j=0; j<num_dds; j++) {
u8 i_ = i+num_dds;
u8 j_ = j+num_dds;
if (i==j) {
obs_cov[i*2*num_dds + j] = dgnss_settings.phase_var_test * 2;
obs_cov[i_*2*num_dds + j_] = dgnss_settings.code_var_test * 2;
}
else {
obs_cov[i*2*num_dds + j] = dgnss_settings.phase_var_test;
obs_cov[i_*2*num_dds + j_] = dgnss_settings.code_var_test;
}
}
}
init_residual_matrices(&ambiguity_test.res_mtxs, num_sats-1, DE, obs_cov);
}
END_TEST
START_TEST(test_vector_add_sc) {
u32 i, t;
seed_rng();
for (t = 0; t < LINALG_NUM; t++) {
u32 n = sizerand(MSIZE_MAX);
double A[n], B[n];
for (i = 0; i < n; i++)
A[i] = mrand;
vector_add_sc(n, A, A, -1, B);
for (i = 0; i < n; i++)
fail_unless(fabs(B[i]) < LINALG_TOL,
"Vector element differs from 0: %lf",
B[i]);
}
}
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();
}
