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