Skip to content
Snippets Groups Projects
Commit 40f87968 authored by Joan Solà Ortega's avatar Joan Solà Ortega Committed by Mederic Fourmy
Browse files

Promote unmeasured cov to ProcessorMotion

parent 2f9d3ffa
No related branches found
No related tags found
No related merge requests found
......@@ -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,
......
......@@ -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
};
}
......
......@@ -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:
......
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
time tolerance: 0.001
unmeasured perturbation std: 0.0001
unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 999999.0 # seconds
max buffer length: 999999 # motion deltas
......
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
time tolerance: 0.001
unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 0.9999 # seconds
max buffer length: 10000 # motion deltas
......
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
time tolerance: 0.001
unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 5.9999 # seconds
max buffer length: 10000 # motion deltas
......
......@@ -26,11 +26,12 @@ static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _file
if (config["processor type"].as<std::string>() == "IMU")
{
YAML::Node kf_vote = config["keyframe vote"];
ProcessorParamsIMUPtr params = std::make_shared<ProcessorParamsIMU>();
params->time_tolerance = config["time tolerance"] .as<Scalar>();
params->unmeasured_perturbation_std_ = config["unmeasured perturbation std"].as<Scalar>();
YAML::Node kf_vote = config["keyframe vote"];
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>();
......
......@@ -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.0000001;
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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment