Promote unmeasured cov to ProcessorMotion

include/base/processor/processor_diff_drive.h

@@ -29,7 +29,6 @@ struct ProcessorParamsDiffDrive : public ProcessorParamsMotion
 //  {
 //      time_tolerance = _time_tolerance;
 //  }
-  Scalar unmeasured_perturbation_std = 0.0001;
 };
 
 /**
@@ -62,7 +61,6 @@ protected:
 
   /// @brief Intrinsic params
   ProcessorParamsDiffDrivePtr params_motion_diff_drive_;
-  MatrixXs unmeasured_perturbation_cov_;
 
   virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
                                    const Eigen::MatrixXs& _data_cov,

include/base/processor/processor_motion.h

@@ -23,10 +23,11 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsMotion);
 
 struct ProcessorParamsMotion : public ProcessorParamsBase
 {
-        Scalar max_time_span    = 0.5;
-        unsigned int max_buff_length  = 10;
-        Scalar dist_traveled    = 5;
-        Scalar angle_turned     = 0.5;
+        Scalar unmeasured_perturbation_std  = 1e-4;
+        Scalar max_time_span                = 0.5;
+        unsigned int max_buff_length        = 10;
+        Scalar dist_traveled                = 5;
+        Scalar angle_turned                 = 0.5;
 };
 
 /** \brief class for Motion processors
@@ -472,6 +473,7 @@ class ProcessorMotion : public ProcessorBase
         Eigen::MatrixXs jacobian_delta_;        ///< jacobian of delta composition w.r.t current delta
         Eigen::MatrixXs jacobian_calib_;        ///< jacobian of delta preintegration wrt calibration params
         Eigen::MatrixXs jacobian_delta_calib_;  ///< jacobian of delta wrt calib params
+        Eigen::MatrixXs unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
 };
 
 }

include/base/processor/processor_odom_2D.h

@@ -21,7 +21,6 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D);
 struct ProcessorParamsOdom2D : public ProcessorParamsMotion
 {
     Scalar cov_det                  = 1.0;      // 1 rad^2
-    Scalar unmeasured_perturbation_std = 0.001;    // no particular dimension: the same for displacement and angle
 };
 
 class ProcessorOdom2D : public ProcessorMotion
@@ -75,7 +74,6 @@ class ProcessorOdom2D : public ProcessorMotion
 
     protected:
         ProcessorParamsOdom2DPtr params_odom_2D_;
-        MatrixXs unmeasured_perturbation_cov_;
 
         // Factory method
     public:

src/examples/processor_imu.yaml

 processor type: "IMU"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
 processor name: "Main imu"        # This is ignored. The name provided to the SensorFactory prevails
+unmeasured perturbation std: 0.00001
 keyframe vote:
     max time span:      2.0   # seconds
     max buffer length:  20000    # motion deltas

src/examples/processor_imu_no_vote.yaml

 processor type: "IMU"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
 processor name: "Main imu"        # This is ignored. The name provided to the SensorFactory prevails
+unmeasured perturbation std: 0.00001
 keyframe vote:
     max time span:      999999.0   # seconds
     max buffer length:  999999     # motion deltas

src/examples/processor_imu_t1.yaml

 processor type: "IMU"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
 processor name: "Main imu"        # This is ignored. The name provided to the SensorFactory prevails
+unmeasured perturbation std: 0.00001
 keyframe vote:
     max time span:      0.9999   # seconds
     max buffer length:  10000    # motion deltas

src/examples/processor_imu_t6.yaml

 processor type: "IMU"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
 processor name: "Main imu"        # This is ignored. The name provided to the SensorFactory prevails
+unmeasured perturbation std: 0.00001
 keyframe vote:
     max time span:      5.9999   # seconds
     max buffer length:  10000    # motion deltas

src/yaml/processor_IMU_yaml.cpp

@@ -30,6 +30,7 @@ static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _file
 
         ProcessorParamsIMUPtr params = std::make_shared<ProcessorParamsIMU>();
 
+        params->unmeasured_perturbation_std = config["unmeasured perturbation std"].as<Scalar>();
         params->max_time_span       = kf_vote["max time span"]      .as<Scalar>();
         params->max_buff_length     = kf_vote["max buffer length"]  .as<SizeEigen  >();
         params->dist_traveled       = kf_vote["dist traveled"]      .as<Scalar>();

test/gtest_odom_2D.cpp

@@ -329,7 +329,7 @@ TEST(Odom2D, KF_callback)
     params->angle_turned    = 6.28;
     params->max_time_span   = 100;
     params->cov_det         = 100;
-    params->unmeasured_perturbation_std = 0.001;
+    params->unmeasured_perturbation_std = 0.000001;
     Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity();
     ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
     ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);