diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ecb3d8048c0ed1e6fb8c26d77032c093baefe6db
--- /dev/null
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -0,0 +1,148 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/common/wolf.h"
+#include "core/problem/problem.h"
+#include <core/ceres_wrapper/solver_ceres.h>
+#include "core/capture/capture_void.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/processor/processor_diff_drive.h"
+#include "core/sensor/sensor_diff_drive.h"
+#include "core/capture/capture_diff_drive.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
+
+**/
+
+class ProcessorDiffDriveTest : public testing::Test
+{
+ public:
+ ParamsSensorDiffDrivePtr intr_;
+ SensorDiffDrivePtr sensor_;
+ ParamsProcessorDiffDrivePtr params_;
+ ProcessorDiffDrivePtr processor_;
+ ProblemPtr problem_;
+ SolverCeresPtr solver_;
+ FrameBasePtr KF0_;
+ double radius;
+
+ void SetUp() override
+ {
+ problem_ = Problem::create("PO", 2);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+
+ VectorComposite x_origin("PO", {Vector2d::Zero(), Vector1d::Zero()});
+ VectorComposite std_origin("PO", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones()});
+
+ // ground truth of wheels radius
+ radius = 1;
+ // make a sensor first
+ intr_ = std::make_shared<ParamsSensorDiffDrive>();
+ intr_->radius_left = 1.042;
+ intr_->radius_right = 1.042;
+ intr_->wheel_separation = 1.0;
+ intr_->ticks_per_wheel_revolution = 2*M_PI;
+ Vector3d extr(0,0,0);
+ sensor_ = std::static_pointer_cast<SensorDiffDrive>(problem_->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr_));
+
+ // params and processor
+ params_ = std::make_shared<ParamsProcessorDiffDrive>();
+ params_->voting_active = true;
+ params_->angle_turned = 1000000;
+ params_->dist_traveled = 200000000;
+ params_->max_buff_length = 1000000000;
+ params_->max_time_span = 2.999; // controls KF creation
+ params_->cov_det = 10000000;
+
+ params_->unmeasured_perturbation_std = 1e-4;
+ processor_ = std::static_pointer_cast<ProcessorDiffDrive>(problem_->installProcessor("ProcessorDiffDrive", "processor diff drive", sensor_, params_));
+
+ KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0, 0.01);
+
+ processor_->setOrigin(KF0_);
+ }
+
+ void TearDown() override{}
+
+};
+
+TEST_F(ProcessorDiffDriveTest, test1)
+{
+ Matrix2d data_cov; data_cov . setIdentity();
+ // Advances at constant speed on both wheels
+ double ticks = 1;
+
+ Vector2d data; data << ticks, ticks;
+ auto C1 = std::make_shared<CaptureDiffDrive>(1, sensor_, data, data_cov, KF0_->getCaptureList().front()); C1->process();
+ auto C2 = std::make_shared<CaptureDiffDrive>(2, sensor_, data, data_cov, KF0_->getCaptureList().front()); C2->process();
+ auto C3 = std::make_shared<CaptureDiffDrive>(3, sensor_, data, data_cov, KF0_->getCaptureList().front()); C3->process();
+ FrameBasePtr KF1 = problem_->getTrajectory()->getLastFrame();
+ Vector2d state_pred1; state_pred1 << 3*ticks, 0;
+ auto CA1 = CaptureBase::emplace<CaptureVoid>(KF1, KF1->getTimeStamp(), nullptr);
+ auto FA1 = FeatureBase::emplace<FeatureBase>(CA1, "POSITION", state_pred1, pow(0.001, 2)*Matrix2d::Identity());
+ FactorBase::emplace<FactorBlockAbsolute>(FA1, FA1, FA1->getFrame()->getP(), nullptr, false);
+
+ problem_->print(4,true,true,true);
+ solver_->solve();
+ problem_->print(4,true,true,true);
+
+ // now, intrinsics are updated in KF0 and KF1 captures
+ auto C4 = std::make_shared<CaptureDiffDrive>(4, sensor_, data, data_cov, C3); C4->process();
+ auto C5 = std::make_shared<CaptureDiffDrive>(5, sensor_, data, data_cov, C3); C5->process();
+ auto C6 = std::make_shared<CaptureDiffDrive>(6, sensor_, data, data_cov, C3); C6->process();
+
+ FrameBasePtr KF2 = problem_->getTrajectory()->getLastFrame();
+ Vector2d state_pred2; state_pred2 << 6*ticks, 0;
+ auto CA2 = CaptureBase::emplace<CaptureVoid>(KF2, KF2->getTimeStamp(), nullptr);
+ auto FA2 = FeatureBase::emplace<FeatureBase>(CA2, "POSITION", state_pred2, pow(0.001, 2)*Matrix2d::Identity());
+ FactorBase::emplace<FactorBlockAbsolute>(FA2, FA2, FA2->getFrame()->getP(), nullptr, false);
+
+ problem_->print(4,true,true,true);
+ solver_->solve();
+ problem_->print(4,true,true,true);
+
+
+ // now, intrinsics are updated in KF0 and KF1 captures
+ auto C7 = std::make_shared<CaptureDiffDrive>(7, sensor_, data, data_cov, C6); C7->process();
+ auto C8 = std::make_shared<CaptureDiffDrive>(8, sensor_, data, data_cov, C6); C8->process();
+ auto C9 = std::make_shared<CaptureDiffDrive>(9, sensor_, data, data_cov, C6); C9->process();
+
+ FrameBasePtr KF3 = problem_->getTrajectory()->getLastFrame();
+ Vector2d state_pred3; state_pred3 << 9*ticks, 0;
+ auto CA3 = CaptureBase::emplace<CaptureVoid>(KF3, KF3->getTimeStamp(), nullptr);
+ auto FA3 = FeatureBase::emplace<FeatureBase>(CA3, "POSITION", state_pred3, pow(0.001, 2)*Matrix2d::Identity());
+ FactorBase::emplace<FactorBlockAbsolute>(FA3, FA3, FA3->getFrame()->getP(), nullptr, false);
+
+ problem_->print(4,true,true,true);
+ solver_->solve();
+ problem_->print(4,true,true,true);
+
+
+
+ // problem_->getTrajectory()->getLastFrame()
+ // problem_->print(4,true,true,true);
+ // problem_->perturb();
+ // problem_->print(4,true,true,true);
+
+
+
+}
+
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}