diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 7beb21e5e4459f24ce647d31ccf3cbb22d811602..805f9bb4cac6847906a3aa9ed2e5f577dbdc63c4 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -67,7 +67,13 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, jacobian_delta_(delta_cov_size_, delta_cov_size_), jacobian_calib_(delta_cov_size_, calib_size_) { - // + jacobian_delta_preint_ .setIdentity(delta_cov_size_,delta_cov_size_); // dDp'/dDp, dDv'/dDv, all zeros + jacobian_delta_ .setIdentity(delta_cov_size_,delta_cov_size_); // + jacobian_calib_ .setZero(delta_cov_size_,calib_size_); + unmeasured_perturbation_cov_ = + params_motion_->unmeasured_perturbation_std + * params_motion_->unmeasured_perturbation_std + * MatrixXd::Identity(delta_cov_size_, delta_cov_size_); } ProcessorMotion::~ProcessorMotion() diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp index d090a507116134cda4d861fea4686502325f1c5c..98f55c0f0d6c5ff9af08d627c1ef4d67b351783e 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -32,7 +32,7 @@ ProcessorOdom2d::ProcessorOdom2d(ParamsProcessorOdom2dPtr _params) : ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params), params_odom_2d_(_params) { - unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3d::Identity(); + // } ProcessorOdom2d::~ProcessorOdom2d() diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp index f3eae8987b95ae4e9d8770199644ea6933e5eb53..bf24a1d4e2d04c9111605792120df484e369f888 100644 --- a/src/processor/processor_odom_3d.cpp +++ b/src/processor/processor_odom_3d.cpp @@ -29,11 +29,7 @@ ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) : ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params), params_odom_3d_ (_params) { - // Set constant parts of Jacobians - jacobian_delta_preint_.setIdentity(6,6); - jacobian_delta_.setIdentity(6,6); - jacobian_calib_.setZero(6,0); - unmeasured_perturbation_cov_ = pow(params_odom_3d_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix6d::Identity(); + // } ProcessorOdom3d::~ProcessorOdom3d()