diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h index d9b0b58454132c8abaa11d50a5847d18b829337f..ac93e3fe2228f39fab603cec3b889e598309086f 100644 --- a/include/imu/math/imu_tools.h +++ b/include/imu/math/imu_tools.h @@ -566,7 +566,7 @@ inline void plus(const MatrixBase<D1>& dp1, const MatrixBase<D2>& dq1, const Mat template<typename D1, typename D2, typename D3> inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, - MatrixBase<D3>& d_pert) + MatrixBase<D3>& d) { Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) ); Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) ); @@ -574,11 +574,11 @@ inline void plus(const MatrixBase<D1>& d1, Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) ); Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) ); Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(6) ); - Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_pert(0) ); - Map<Quaternion<typename D3::Scalar> > dq_p ( & d_pert(3) ); - Map<Matrix<typename D3::Scalar, 3, 1> > dv_p ( & d_pert(7) ); + Map<Matrix<typename D3::Scalar, 3, 1> > dp ( & d (0) ); + Map<Quaternion<typename D3::Scalar> > dq ( & d (3) ); + Map<Matrix<typename D3::Scalar, 3, 1> > dv ( & d (7) ); - plus(dp1, dq1, dv1, dp2, do2, dv2, dp_p, dq_p, dv_p); + plus(dp1, dq1, dv1, dp2, do2, dv2, dp, dq, dv); } template<typename D1, typename D2>