From 0efb34a493f4e83740e09b3ad8bcec1087e985e3 Mon Sep 17 00:00:00 2001 From: Dinesh Atchuthan <datchuth@laas.fr> Date: Thu, 15 Dec 2016 17:25:38 +0100 Subject: [PATCH] [WIP] check_covariance test_f --- src/test/gtest_processor_imu.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/test/gtest_processor_imu.cpp b/src/test/gtest_processor_imu.cpp index 1868d250c..1e5ebfc6a 100644 --- a/src/test/gtest_processor_imu.cpp +++ b/src/test/gtest_processor_imu.cpp @@ -161,6 +161,26 @@ TEST_F(ProcessorIMU, acc_z) ASSERT_TRUE((problem->getCurrentState() - x).isMuchSmallerThan(1, wolf::Constants::EPS_SMALL)); } +TEST_F(ProcessorIMU, check_Covariance) +{ + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; // Try some non-zero biases + + problem->getProcessorMotionPtr()->setOrigin(x0, t); + + data << 2, 0, 9.8, 0, 0, 0; // only acc_x, but measure gravity! + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.1); + sensor_ptr->process(cap_imu_ptr); + + Eigen::VectorXs delta_preint(problem->getProcessorMotionPtr()->getMotion().delta_integr_); + Eigen::Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotionPtr()->getCurrentDeltaPreintCov(); + + ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL)); + ASSERT_FALSE(delta_preint_cov.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL)); +} + TEST_F(ProcessorIMU, Covariances) { data_cov.topLeftCorner(3,3) *= 0.01; // acc variance -- GitLab