diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 682b02f654cf396452c4b646163a69bb284c219d..5f4d77d4c097efb9330116aebccdb98f820d03f3 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -66,11 +66,13 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, calib_preint_(_calib_size), jacobian_delta_preint_(delta_cov_size_, delta_cov_size_), jacobian_delta_(delta_cov_size_, delta_cov_size_), - jacobian_calib_(delta_cov_size_, calib_size_) + jacobian_calib_(delta_cov_size_, calib_size_), + jacobian_delta_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_); + jacobian_delta_calib_ .setZero(delta_cov_size_,calib_size_); unmeasured_perturbation_cov_ = params_motion_->unmeasured_perturbation_std * params_motion_->unmeasured_perturbation_std