diff --git a/src/processor_imu.h b/src/processor_imu.h index 98619262cb12e661da65fb5faca22721d17cfe93..d2531d495edeb623b144a6619603103ccd7bb32c 100644 --- a/src/processor_imu.h +++ b/src/processor_imu.h @@ -424,12 +424,12 @@ inline void ProcessorIMU::getJacobians(Eigen::Matrix<wolf::Scalar,9,6>& _dD_db) [ dDv_dab dDv_dwb ] */ - _dD_db.block(3,3,0,0) = dDp_dab_; - _dD_db.block(3,3,3,0) = Eigen::Matrix3s::Zero(); - _dD_db.block(3,3,6,0) = dDv_dab_; - _dD_db.block(3,3,0,3) = dDp_dwb_; + _dD_db.block(0,0,3,3) = dDp_dab_; + _dD_db.block(3,0,3,3) = Eigen::Matrix3s::Zero(); + _dD_db.block(6,0,3,3) = dDv_dab_; + _dD_db.block(0,3,3,3) = dDp_dwb_; _dD_db.block(3,3,3,3) = dDq_dwb_; - _dD_db.block(3,3,6,3) = dDv_dwb_; + _dD_db.block(6,3,3,3) = dDv_dwb_; } } // namespace wolf