diff --git a/.gitignore b/.gitignore
index 3d1d237de284515d210782928bd15eb347decf99..91c5366fd823c89dcb9024788ac8c419f5337c52 100644
--- a/.gitignore
+++ b/.gitignore
@@ -33,3 +33,4 @@ src/examples/map_apriltag_save.yaml
build_release/
IMU.found
+est.csv
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 35a9d415d3016f1b4849c398d8ad9aabeff12f85..a8cf0ab798de32fa1901d03d37e307295fa2c78b 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -32,3 +32,6 @@ target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
# Maybe call an archeologist to fix this thing?
# wolf_add_gtest(gtest_factor_imu gtest_factor_imu.cpp)
# target_link_libraries(gtest_factor_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
+
+wolf_add_gtest(gtest_processor_motion_intrinsics_update gtest_processor_motion_intrinsics_update.cpp)
+target_link_libraries(gtest_processor_motion_intrinsics_update ${PLUGIN_NAME} ${wolf_LIBRARY})
diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4dbac962133c7d8da9f76e152a09e0749c1f2e71
--- /dev/null
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -0,0 +1,264 @@
+#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"
+#include <core/ceres_wrapper/solver_ceres.h>
+#include "imu/sensor/sensor_imu.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/capture/capture_imu.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+
+
+/**
+Simplified test rolling along x axis only:
+Delta_x = 2*pi*R*(ticks/ticks_per_wheel_revolution)
+
+If we take ticks_per_wheel_revolution = 2*pi, even simpler formulat:
+Delta_x = R*ticks
+
+**/
+
+Vector10d integrateX(double ax, double dt){
+ Vector10d x; x << 0.5*ax*pow(dt,2),0,0, 0,0,0,1, ax*dt,0,0;
+ return x;
+}
+
+class ProcessorImuTest : public testing::Test
+{
+ public:
+ ParamsSensorImuPtr intr_;
+ SensorImuPtr sensor_;
+ ParamsProcessorImuPtr params_;
+ ProcessorImuPtr processor_;
+ ProblemPtr problem_;
+ SolverCeresPtr solver_;
+ FrameBasePtr KF0_;
+ double radius;
+
+ void SetUp() override
+ {
+ problem_ = Problem::create("POV", 3);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+
+ Vector4d q_init; q_init << 0,0,0,1;
+ VectorComposite x_origin("POV", {Vector3d::Zero(), q_init, Vector3d::Zero()});
+ VectorComposite std_origin("POV", {0.001*Vector3d::Ones(), 0.001*Vector3d::Ones(), 0.001*Vector3d::Ones()});
+
+ // ground truth of wheels radius
+ radius = 1;
+ // make a sensor first
+ intr_ = std::make_shared<ParamsSensorImu>();
+ intr_->a_noise = 0.001;
+ intr_->w_noise = 0.001;
+ intr_->ab_initial_stdev = 0.001;
+ intr_->wb_initial_stdev = 0.001;
+ intr_->ab_rate_stdev = 0.001;
+ intr_->wb_rate_stdev = 0.001;
+
+ Vector7d extr; extr << 0,0,0, 0,0,0,1;
+ sensor_ = std::static_pointer_cast<SensorImu>(problem_->installSensor("SensorImu", "sensor imu", extr, intr_));
+
+ // params and processor
+ params_ = std::make_shared<ParamsProcessorImu>();
+ params_->voting_active = true;
+ params_->max_time_span = 0.9999; // create one KF every second
+ params_->max_buff_length = 1000000000;
+ params_->dist_traveled = 100000000000;
+ params_->angle_turned = 10000000000;
+
+ params_->unmeasured_perturbation_std = 1e-4;
+ params_->time_tolerance = 0.0025;
+ processor_ = std::static_pointer_cast<ProcessorImu>(problem_->installProcessor("ProcessorImu", "processor imu", sensor_, params_));
+
+ KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0, 0.01);
+
+ Vector6d bias; bias << 0.42,0,0, 0,0,0;
+ // Vector6d bias; bias << 0.0,0,0, 0.42,0,0;
+ sensor_->getIntrinsic(0)->setState(bias);
+
+ processor_->setOrigin(KF0_);
+ }
+
+ void TearDown() override{}
+
+};
+
+TEST_F(ProcessorImuTest, test1)
+{
+ Matrix6d data_cov; data_cov. setIdentity();
+ // Advances at constant speed on both wheels
+ double AX = 0;
+
+ problem_->print(4,true,true,true);
+ CaptureMotionPtr CM;
+
+ CM = std::static_pointer_cast<CaptureMotion>(KF0_->getCaptureOf(sensor_));
+ std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+ solver_->solve();
+ std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+
+
+ Vector6d data; data << -wolf::gravity(), 0,0,0; data(0) = AX;
+ Vector10d state_pred; state_pred << 0,0,0, 0,0,0,1, 0,0,0;
+ auto C1 = std::make_shared<CaptureImu>(1, sensor_, data, data_cov, KF0_->getCaptureList().front()); C1->process();
+ FrameBasePtr KF1 = problem_->getTrajectory()->getLastFrame();
+ KF1->setState(integrateX(AX,1)); KF1->fix();
+ CM = std::static_pointer_cast<CaptureMotion>(KF1->getCaptureOf(sensor_));
+ std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+ solver_->solve();
+ std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+ // problem_->print(4,true,true,true);
+
+ auto C2 = std::make_shared<CaptureImu>(2, sensor_, data, data_cov, KF0_->getCaptureList().front()); C2->process();
+ FrameBasePtr KF2 = problem_->getTrajectory()->getLastFrame();
+ KF2->setState(integrateX(AX,2)); KF2->fix();
+ CM = std::static_pointer_cast<CaptureMotion>(KF2->getCaptureOf(sensor_));
+ std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+ solver_->solve();
+ std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+ // problem_->print(4,true,true,true);
+
+ auto C3 = std::make_shared<CaptureImu>(3, sensor_, data, data_cov, KF0_->getCaptureList().front()); C3->process();
+ FrameBasePtr KF3 = problem_->getTrajectory()->getLastFrame();
+ KF3->setState(integrateX(AX,3)); KF3->fix();
+ CM = std::static_pointer_cast<CaptureMotion>(KF3->getCaptureOf(sensor_));
+ std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+ solver_->solve();
+ std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+ // problem_->print(4,true,true,true);
+
+ auto C4 = std::make_shared<CaptureImu>(4, sensor_, data, data_cov, KF0_->getCaptureList().front()); C4->process();
+ FrameBasePtr KF4 = problem_->getTrajectory()->getLastFrame();
+ KF4->setState(integrateX(AX,4)); KF4->fix();
+ CM = std::static_pointer_cast<CaptureMotion>(KF4->getCaptureOf(sensor_));
+ std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+ solver_->solve();
+ std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+ // problem_->print(4,true,true,true);
+
+ for(auto kf: problem_->getTrajectory()->getFrameMap()){
+ CaptureMotionPtr cap = std::static_pointer_cast<CaptureMotion>(kf.second->getCaptureOf(sensor_));
+ }
+
+
+}
+
+
+/**
+ * SET TO TRUE TO PRODUCE CSV FILE
+ * SET TO FALSE TO STOP PRODUCING CSV FILE
+ */
+#define WRITE_CSV_FILE false
+
+TEST_F(ProcessorImuTest, getState)
+{
+ Matrix6d data_cov; data_cov. setIdentity();
+ // Advances at constant acceleration
+ double AX = 0;
+
+ problem_->print(4,true,true,true);
+ Vector6d data; data << -wolf::gravity(), 0,0,0; data(0) = AX;
+ CaptureImuPtr C;
+
+ TimeStamp t(0);
+ double dt(0.1);
+ int nb_kf = 1;
+
+#if WRITE_CSV_FILE
+ std::fstream file_est;
+ file_est.open("./est.csv", std::fstream::out);
+// std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+ std::string header_est = "t;px;vx;bax_est;bax_preint\n";
+ file_est << header_est;
+#endif
+
+ while (t < 4){
+ auto C = std::make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front());
+ C->process();
+
+
+ std::cout << "t " << t << "; cap id " << sensor_->findLastCaptureBefore(t) << "; cap ts " << sensor_->findLastCaptureBefore(t)->getTimeStamp() << std::endl;
+
+
+#if WRITE_CSV_FILE
+ // pre-solve print to CSV
+ VectorComposite state = problem_->getState(t);
+ VectorXd calib_estim = sensor_->getIntrinsic(t)->getState();
+ VectorXd calib_preint = processor_->getLast()->getCalibrationPreint();
+ file_est << std::fixed << t << ";"
+ << state['P'](0) << ";"
+ << state['V'](0) << ";"
+ << calib_estim(0) << ";"
+ << calib_preint(0) << "\n";
+#endif
+
+
+ if (problem_->getTrajectory()->getFrameMap().size() > nb_kf){
+ FrameBasePtr KF = problem_->getTrajectory()->getLastFrame();
+ KF->setState(integrateX(AX, KF->getTimeStamp().get()));
+ KF->fix();
+
+ solver_->solve();
+ problem_->print(4, true, true, true);
+
+ nb_kf = problem_->getTrajectory()->getFrameMap().size();
+
+#if WRITE_CSV_FILE
+ // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
+ state = problem_->getState(t);
+ calib_estim = sensor_->getIntrinsic(t)->getState();
+ calib_preint = processor_->getLast()->getCalibrationPreint();
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'](0) << ";"
+ << state['V'](0) << ";"
+ << calib_estim(0) << ";"
+ << calib_preint(0) << "\n";
+#endif
+ }
+
+ t += dt;
+ }
+
+#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);
+ }
+
+}
+
+
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}