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>