예제 #1
0
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);
}
예제 #2
0
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;
}
예제 #3
0
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);
}
예제 #4
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));
}
예제 #5
0
  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)
{