diff --git a/demos/demo_2_lasers_offline.cpp b/demos/demo_2_lasers_offline.cpp
index 5fc9fdf55c04cbd72c9685a0af594328ecf078b2..94cedee65f5dd29bb9d952094628465fd7fc7c9a 100644
--- a/demos/demo_2_lasers_offline.cpp
+++ b/demos/demo_2_lasers_offline.cpp
@@ -189,7 +189,7 @@ int main(int argc, char** argv)
     odom_trajectory.head(3) = ground_truth_pose;
 
     // Origin Key Frame with covariance
-    problem.setPrior(ground_truth_pose, Eigen::Matrix3d::Identity() * 0.1, ts, 0.1);
+    problem.setPrior(ground_truth_pose, Eigen::Matrix3d::Identity() * 0.1, ts);
 
     // Ceres wrapper
     SolverCeres ceres_manager(&problem);
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 50872148fcc3afea0b727813fc3ee219f9e50d37..ad55f0aa8d949adbec78fcb0c76ba39aa581e00c 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -237,7 +237,7 @@ void ProcessorOdomIcp::resetDerived()
     }
 
     // Using processing_step_ to ensure that origin, last and incoming are different
-    if (processing_step_ != FIRST_TIME_WITH_PACK && processing_step_ != FIRST_TIME_WITHOUT_PACK) {
+    if (processing_step_ != FIRST_TIME_WITH_KEYFRAME && processing_step_ != FIRST_TIME_WITHOUT_KEYFRAME) {
         // Notation explanation:
         // x1_R_x2: Rotation from frame x1 to frame x2
         // x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
index 9b479fcbdd4b3407862db31c1224525be874dbc6..db934f54a34c0d3516a8d64819e2211c4f85298c 100644
--- a/test/gtest_processor_odom_icp.cpp
+++ b/test/gtest_processor_odom_icp.cpp
@@ -85,7 +85,7 @@ class ProcessorOdomIcp_Test : public testing::Test
             t0 = 0.0;
             x0 = VectorComposite("PO", {Vector2d(0,0), Vector1d(0)});
             s0 = VectorComposite("PO", {Vector2d(1,1), Vector1d(1)});
-            F0 = problem->setPriorFactor(x0, s0, t0, 0.1);
+            F0 = problem->setPriorFactor(x0, s0, t0);
         }
         void TearDown() override{}
 };
diff --git a/test/gtest_processor_tracker_feature_polyline_2d.cpp b/test/gtest_processor_tracker_feature_polyline_2d.cpp
index d2527c0d0f4d615de7bbf3e8334884e7838788c3..0553e9e6814e0444f22338c8aadf6de94665b359 100644
--- a/test/gtest_processor_tracker_feature_polyline_2d.cpp
+++ b/test/gtest_processor_tracker_feature_polyline_2d.cpp
@@ -283,7 +283,7 @@ TEST(ProcessorIMU, voteForKeyFrame)
     TimeStamp t(0);
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
     //data variable and covariance matrix
     // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
     Vector6d data;
@@ -346,7 +346,7 @@ TEST_F(ProcessorIMUt, interpolate)
     t.set(0);
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     data << 2, 0, 0, 0, 0, 0; // only acc_x
     cap_imu_ptr->setData(data);
@@ -392,7 +392,7 @@ TEST_F(ProcessorIMUt, acc_x)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
 
@@ -418,7 +418,7 @@ TEST_F(ProcessorIMUt, acc_y)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity!
 
@@ -455,7 +455,7 @@ TEST_F(ProcessorIMUt, acc_z)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity!
 
@@ -492,7 +492,7 @@ TEST_F(ProcessorIMUt, check_Covariance)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
 
@@ -513,7 +513,7 @@ TEST_F(ProcessorIMUt, gyro_x)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     double rate_of_turn = 5 * M_PI/180.0;
     data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
@@ -565,7 +565,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
     MatrixXd P0(9,9); P0.setIdentity();
 
     // init things
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
     problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
@@ -618,7 +618,7 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
     double abx(0.002), aby(0.01);
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     Vector6d bias; bias << abx,aby,0,  0,0,0;
     Vector3d acc_bias = bias.head(3);
@@ -670,7 +670,7 @@ TEST_F(ProcessorIMUt, gyro_z)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     double rate_of_turn = 5 * M_PI/180.0;
     data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
@@ -708,7 +708,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     Vector3d tmp_vec; //will be used to store rate of turn data
     Quaterniond quat_comp(Quaterniond::Identity());
@@ -795,7 +795,7 @@ TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  2,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     double rate_of_turn = 5 * M_PI/180.0;
     data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
@@ -834,7 +834,7 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  2,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     double rate_of_turn = 5 * M_PI/180.0;
     data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
@@ -878,7 +878,7 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  2,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     double rate_of_turn = 5 * M_PI/180.0;
     data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
@@ -922,7 +922,7 @@ TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  2,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     double rate_of_turn = 5 * M_PI/180.0;
     data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
@@ -965,7 +965,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  2,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     Vector3d tmp_vec; //will be used to store rate of turn data
     Quaterniond quat_comp(Quaterniond::Identity());
@@ -1054,7 +1054,7 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     double rate_of_turn = 5 * M_PI/180.0;
     data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity
@@ -1103,7 +1103,7 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     double rate_of_turn = 5 * M_PI/180.0;
     data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity
@@ -1152,7 +1152,7 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z)
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
     MatrixXd P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
+    problem->setPrior(x0, P0, t);
 
     double rate_of_turn = 5 * M_PI/180.0;
     data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity