diff --git a/include/base/processor/processor_diff_drive.h b/include/base/processor/processor_diff_drive.h
index 4ea5181b81321b4649f1e2bdf835db2e0e820959..9d05bab54b8e0ac41bf3567eb62f59c125610d25 100644
--- a/include/base/processor/processor_diff_drive.h
+++ b/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,
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index 46af97f524059b9c5590a40d569d9f8f5c1c2acc..38a4faa0d8dc1b971c789617af036c69ec177b01 100644
--- a/include/base/processor/processor_motion.h
+++ b/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
};
}
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
index ae73d0e93662231dc0a77f4e5c26f26fddda7160..5def5e3ad512036fc227e773c48211284892ec73 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/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:
diff --git a/src/examples/processor_imu.yaml b/src/examples/processor_imu.yaml
index 7e684c8833a6e9e3123863c71366a989b30e4004..9d3d4fb807f1cecd9b3fe99ffe733a69622af4be 100644
--- a/src/examples/processor_imu.yaml
+++ b/src/examples/processor_imu.yaml
@@ -1,5 +1,6 @@
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
diff --git a/src/examples/processor_imu_no_vote.yaml b/src/examples/processor_imu_no_vote.yaml
index 4f6ad39556cd9a09a215f043d4beb0066d4a37bb..4a1dd140e695cefc4590feb9213df76639cc7282 100644
--- a/src/examples/processor_imu_no_vote.yaml
+++ b/src/examples/processor_imu_no_vote.yaml
@@ -1,5 +1,6 @@
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
diff --git a/src/examples/processor_imu_t1.yaml b/src/examples/processor_imu_t1.yaml
index e0c21758c11ed2a684b2f3f2bc2aeb4c557c84ef..e290b5ac495cffcd20ae7b8bd2ee4a809fc05c9e 100644
--- a/src/examples/processor_imu_t1.yaml
+++ b/src/examples/processor_imu_t1.yaml
@@ -1,5 +1,6 @@
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
diff --git a/src/examples/processor_imu_t6.yaml b/src/examples/processor_imu_t6.yaml
index e3a4b17df72c957fec49d935ddcd3a9a8c824a96..b864e9859a4df06048e61ac83ec48ce18fbcf96c 100644
--- a/src/examples/processor_imu_t6.yaml
+++ b/src/examples/processor_imu_t6.yaml
@@ -1,5 +1,6 @@
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
diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp
index 5b3f12ac6100f079d3653acea7f941d51afdca2d..4146484942a63db640b6c4405444195fe27859a5 100644
--- a/src/yaml/processor_IMU_yaml.cpp
+++ b/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>();
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 2aa224b3143b723ec7d98be9183c26392522206a..604ef489478f5779ac4b79a14095c37436c81be4 100644
--- a/test/gtest_odom_2D.cpp
+++ b/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);