END_TEST
START_TEST(test_lesq_solution5)
{
/* Over constrained with non-zero residuals */
double N[] = {0, 0, 0, 0};
u8 num_dds = sizeof(N)/sizeof(N[0]);
double DE[] = {1, 0, 0,
0, 1, 0,
0, 0, 1,
1, 0, 0};
double b_true[3] = {0, 0, 0};
double dd_obs[] = {0, 0, 0, 1};
double b[3];
double resid[num_dds];
s8 ret = lesq_solution_float(num_dds, dd_obs, N, DE, b, resid);
double b_expected[3] = {0.5*GPS_L1_LAMBDA_NO_VAC, 0, 0};
double resid_expected[] = {-0.5, 0, 0, 0.5};
fail_unless(ret == 0, "solution returned error %d", ret);
for (u8 i=0; i<3; i++) {
fail_unless(fabs(b[i] - b_expected[i]) < TOL,
"Baseline mismatch: %lf vs %lf",
b[i], b_true[i]);
}
for (u8 i=0; i<num_dds; i++) {
fail_unless(fabs(resid[i] - resid_expected[i]) < TOL,
"Residual mismatch: %lf vs %lf",
resid[i], resid_expected[i]);
}
}
END_TEST
START_TEST(test_lesq_solution2)
{
/* Exactly constrained. */
double N[] = {22, 23, 34};
u8 num_dds = sizeof(N)/sizeof(N[0]);
double DE[] = {1, 0, 0,
0, 1, 0,
1, 1, 1};
double b_true[3] = {1.234, 1.456, 1.789};
double dd_obs[num_dds];
predict_carrier_obs(num_dds, N, DE, b_true, dd_obs);
double b[3];
double resid[num_dds];
s8 ret = lesq_solution_float(num_dds, dd_obs, N, DE, b, resid);
fail_unless(ret == 0, "solution returned error %d", ret);
for (u8 i=0; i<3; i++) {
fail_unless(fabs(b[i] - b_true[i]) < TOL,
"Baseline mismatch: %lf vs %lf",
b[i], b_true[i]);
}
}
END_TEST
START_TEST(test_lesq_solution3)
{
/* Under constrained, should fail with correct return code. */
double N[2];
double DE[2*3];
double b[3];
double resid[2];
double dd_obs[2];
for (u8 num_dds = 0; num_dds < 3; num_dds++) {
s8 ret = lesq_solution_float(num_dds, dd_obs, N, DE, b, resid);
fail_unless(ret == -1, "solution under-constrained, "
"should have returned error -1, got %d, dds = %d",
ret, num_dds);
}
}
static s8 lesq_without_i(u8 dropped_dd, u8 num_dds, const double *dd_obs,
const double *N, const double *DE, double b[3],
double *resid)
{
assert(num_dds > 3);
assert(num_dds < MAX_CHANNELS);
u8 new_dds = num_dds - 1;
double new_obs[new_dds];
double new_N[new_dds];
double new_DE[new_dds * 3];
drop_i(dropped_dd, num_dds, 1, dd_obs, new_obs);
drop_i(dropped_dd, num_dds, 1, N, new_N);
drop_i(dropped_dd, num_dds, 3, DE, new_DE);
return lesq_solution_float(new_dds, new_obs, new_N, new_DE, b, resid);
}
/* TODO(dsk) update all call sites to use n_used as calculated here.
* TODO(dsk) add warn/info logging to call sites when repair occurs.
* TODO(dsk) make this static
*/
s8 lesq_solve_raim(u8 num_dds_u8, const double *dd_obs,
const double *N, const double *DE, double b[3],
bool disable_raim, double raim_threshold,
u8 *n_used, double *ret_residuals, u8 *removed_obs)
{
integer num_dds = num_dds_u8;
double residuals[num_dds];
double residual;
assert(num_dds < MAX_CHANNELS);
assert(num_dds_u8 < MAX_CHANNELS);
s8 okay = lesq_solution_float(num_dds_u8, dd_obs, N, DE, b, residuals);
if (okay != 0) {
/* Not enough sats or other error returned by initial lesq solution. */
return okay;
}
if (disable_raim || chi_test(raim_threshold, num_dds, residuals, &residual)) {
/* Solution using all sats ok. */
if (ret_residuals) {
memcpy(ret_residuals, residuals, num_dds * sizeof(double));
}
if (n_used) {
*n_used = num_dds;
}
if (disable_raim || num_dds == 3) {
return 2;
}
return 0;
}
if (num_dds < 5) {
/* We have just enough sats for a solution; can't search for solution
* after dropping one.
* 5 are needed because a 3 dimensional system is exactly constrained,
* so the bad measurement can't be detected.
*/
if (n_used) {
*n_used = 0;
}
return -4;
}
u8 num_passing = 0;
u8 bad_sat = -1;
u8 new_dds = num_dds - 1;
for (u8 i = 0; i < num_dds; i++) {
if (0 == lesq_without_i(i, num_dds, dd_obs, N, DE, b, residuals)) {
if (chi_test(raim_threshold, new_dds, residuals, &residual)) {
num_passing++;
bad_sat = i;
}
}
}
if (num_passing == 1) {
/* bad_sat holds index of bad dd
* Return solution without bad_sat. */
/* Recalculate this solution. */
(void)lesq_without_i(bad_sat, num_dds, dd_obs, N, DE, b, residuals);
if (removed_obs) {
*removed_obs = bad_sat;
}
if (ret_residuals) {
memcpy(ret_residuals, residuals, (num_dds-1) * sizeof(double));
}
if (n_used) {
*n_used = num_dds-1;
}
return 1;
} else if (num_passing == 0) {
/* Ref sat is bad? */
if (n_used) {
*n_used = 0;
}
return -3;
} else {
/* Had more than one acceptable solution.
* TODO(dsk) should we return the best one? */
if (n_used) {
*n_used = 0;
}
return -5;
}
}
