END_TEST
START_TEST(test_outlier)
{
/* Test with H, U, D, R = I */
nkf_t kf;
kf.state_dim = 2;
kf.obs_dim = 2;
matrix_eye(2, kf.decor_obs_mtx);
kf.decor_obs_cov[0] = 1;
kf.decor_obs_cov[1] = 1;
matrix_eye(2, kf.state_cov_U);
kf.state_cov_D[0] = 2;
kf.state_cov_D[1] = 3;
kf.state_mean[0] = 1;
kf.state_mean[1] = 2;
double obs[2] = {1,2};
double k_scalar;
/* Test with the predicted and actual observations the same. */
bool is_outlier = outlier_check(&kf, obs, &k_scalar);
/* A perfect match should never be an outlier, so k_scalar must be 1. */
fail_unless(!is_outlier);
fail_unless(within_epsilon(k_scalar, 1));
kf.state_mean[0] = 1e20;
kf.state_mean[1] = 1e20;
/* Test with the predicted and actual observations wildly different. */
is_outlier = outlier_check(&kf, obs, &k_scalar);
/* This horrible of a match should always be an outlier,
* so k_scalar must be < 1. */
fail_unless(is_outlier);
fail_unless(k_scalar < 1);
}
u8 arr_within_epsilon(u32 n, const double *a, const double *b) {
for (u32 i = 0; i < n; i++) {
if (!within_epsilon(a[i], b[i])) {
return false;
}
}
return true;
}
END_TEST
START_TEST(test_sos_innov)
{
/* Test with H, U, D, R = I */
nkf_t kf;
kf.state_dim = 2;
kf.obs_dim = 2;
matrix_eye(2, kf.decor_obs_mtx);
kf.decor_obs_cov[0] = 1;
kf.decor_obs_cov[1] = 1;
matrix_eye(2, kf.state_cov_U);
kf.state_cov_D[0] = 2;
kf.state_cov_D[1] = 3;
kf.state_mean[0] = 1;
kf.state_mean[1] = 2;
double obs[2] = {1,2};
/* Test with the predicted and actual observations the same. */
fail_unless(within_epsilon(get_sos_innov(&kf, obs), 0));
fail_unless(get_sos_innov(&kf, obs) >= 0);
kf.state_mean[0] = 0;
kf.state_mean[1] = 0;
/* Test with the predicted and actual observations slightly different.
* S = R + H * U * D * U^T * H^T = diag(3,4).
* y = z - H*x = z = (1, 2).
* sos = sum_i (y_i / S_ii) = 1/3 + 2^2 / 4. */
fail_unless(within_epsilon(get_sos_innov(&kf, obs), 1.0f/3 + 4.0f/4));
/* Test it with a singular matrix.
kf.state_cov = {{1,1},{1,1}} */
matrix_eye(2, kf.state_cov_U);
kf.state_cov_U[1] = 1;
kf.state_cov_D[0] = 0;
kf.state_cov_D[1] = 1;
memset(kf.decor_obs_cov, 0, 2*sizeof(double));
fail_unless(isfinite(get_sos_innov(&kf, obs)));
fail_unless(get_sos_innov(&kf, obs) >= 0);
}
END_TEST
START_TEST(test_sos_innov_dims)
{
/* Make sure that the SOS innovation calculation works for a few different
* combinations of state_dim and obs_dim, including zeros.*/
nkf_t kf;
kf.decor_obs_cov[0] = 1;
kf.decor_obs_cov[1] = 1;
matrix_eye(2, kf.state_cov_U);
kf.state_cov_D[0] = 2;
kf.state_cov_D[1] = 3;
kf.state_mean[0] = -1;
kf.state_mean[1] = -2;
double obs[2] = {1,2};
kf.state_dim = 1;
kf.obs_dim = 1;
matrix_eye(2, kf.decor_obs_mtx);
fail_unless(within_epsilon(get_sos_innov(&kf, obs), 4.0f/3));
kf.state_dim = 0;
kf.obs_dim = 1;
fail_unless(get_sos_innov(&kf, obs) == 0);
kf.state_dim = 1;
kf.obs_dim = 0;
fail_unless(get_sos_innov(&kf, obs) == 0);
kf.state_dim = 0;
kf.obs_dim = 0;
fail_unless(get_sos_innov(&kf, obs) == 0);
kf.state_dim = 1;
kf.obs_dim = 2;
kf.decor_obs_mtx[0] = 1;
kf.decor_obs_mtx[1] = 1;
fail_unless(within_epsilon(get_sos_innov(&kf, obs), 4.0f/3 + 9.0f/3));
kf.state_dim = 2;
kf.obs_dim = 1;
fail_unless(within_epsilon(get_sos_innov(&kf, obs), 16.0f/6));
}
fail_unless(k_scalar < 1);
}
END_TEST
START_TEST(test_outlier_dims)
{
/* Test that the outlier detection says that a null measurement
* (either dim = 0) is a good measurement. Tests different combos
* of which dim is zero. */
nkf_t kf = {.state_dim = 1,
.obs_dim = 0};
double obs;
double k_scalar;
bool bad = outlier_check(&kf, &obs, &k_scalar);
fail_unless(bad == false);
fail_unless(within_epsilon(k_scalar, 1));
kf.state_dim = 0;
kf.obs_dim = 1;
bad = outlier_check(&kf, &obs, &k_scalar);
fail_unless(bad == false);
fail_unless(within_epsilon(k_scalar, 1));
kf.state_dim = 0;
kf.obs_dim = 0;
bad = outlier_check(&kf, &obs, &k_scalar);
fail_unless(bad == false);
fail_unless(within_epsilon(k_scalar, 1));
}
END_TEST
START_TEST(test_kf_update_noop)
{
