Make the bias test a true gtest by adding asserts

test/gtest_processor_motion_intrinsics_update.cpp

 #include <fstream>
 
+#include "imu/internal/config.h"
 #include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
 
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
@@ -227,6 +229,28 @@ TEST_F(ProcessorImuTest, getState)
 #if WRITE_CSV_FILE
     file_est.close();
 #endif
+
+    /** assert final results
+     *
+     * t            0   .9  1   1.9 2   3
+     * BIAS         .4  .4  0   0   0   0
+     * BIAS PREINT  .4  .4  .4  .4  0   0
+     * X            0   !0  0   0   0   0
+     * V            0   !0  0   0   0   0
+     *
+     * though we only test at KFs so t = { 0, 1, 2, 3 }
+     */
+
+    for (auto F : *problem_->getTrajectory() )
+    {
+        auto ts = F->getTimeStamp();
+
+        double bias = (t == 0 ? 0.42 : 0.0);
+        ASSERT_NEAR(F->getCaptureOf(sensor_)->getSensorIntrinsic()->getState()(0), bias, 1e-6);
+        ASSERT_NEAR(F->getP()->getState()(0),0.0,1e-6);
+        ASSERT_NEAR(F->getV()->getState()(0),0.0,1e-6);
+    }
+
 }