diff --git a/src/imu_tools.h b/src/imu_tools.h
index 087164618436b78ef251b2adddcda3c1fbc1e514..9a5a83b736ae5b07a84f455c3a923500ad18e41c 100644
--- a/src/imu_tools.h
+++ b/src/imu_tools.h
@@ -17,15 +17,50 @@ namespace wolf
 namespace imu {
 using namespace Eigen;
 
+template<typename D1, typename D2, typename D3>
+inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q, MatrixBase<D3>& v)
+{
+    p = MatrixBase<D1>::Zero(3,1);
+    q = QuaternionBase<D2>::Identity();
+    v = MatrixBase<D3>::Zero(3,1);
+}
+
+template<typename D1, typename D2, typename D3>
+inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q, MatrixBase<D3>& v)
+{
+    typedef typename D1::Scalar T1;
+    typedef typename D2::Scalar T2;
+    typedef typename D3::Scalar T3;
+    p << T1(0), T1(0), T1(0);
+    q << T2(0), T2(0), T2(0), T2(1);
+    v << T3(0), T3(0), T3(0);
+}
+
 template<typename T = wolf::Scalar>
 inline Matrix<T, 10, 1> identity()
 {
     Matrix<T, 10, 1> ret;
-    ret.setZero();
-    ret(6) = 1.0;
+    ret<< T(0), T(0), T(0),
+          T(0), T(0), T(0), T(1),
+          T(0), T(0), T(0);
     return ret;
 }
 
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, class T>
+inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq, const MatrixBase<D3>& dv,
+                    const T dt,
+                    MatrixBase<D4>& idp, QuaternionBase<D5>& idq, MatrixBase<D6>& idv )
+{
+    MatrixSizeCheck<3, 1>::check(dp);
+    MatrixSizeCheck<3, 1>::check(dv);
+    MatrixSizeCheck<3, 1>::check(idp);
+    MatrixSizeCheck<3, 1>::check(idv);
+
+    idp = - ( dq.conjugate() * (dp - dv * typename D3::Scalar(dt) ) );
+    idv = - ( dq.conjugate() * dv );
+    idq =     dq.conjugate();
+}
+
 template<typename D1, typename D2, class T>
 inline void inverse(const MatrixBase<D1>& d,
                     T dt,
@@ -34,16 +69,14 @@ inline void inverse(const MatrixBase<D1>& d,
     MatrixSizeCheck<10, 1>::check(d);
     MatrixSizeCheck<10, 1>::check(id);
 
-    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp    ( &d( 0 ) );
-    Map<const Quaternion<typename D1::Scalar> >     dq    ( &d( 3 ) );
-    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv    ( &d( 7 ) );
-    Map<Matrix<typename D2::Scalar, 3, 1> >         idp  ( &id( 0 ));
-    Map<Quaternion<typename D2::Scalar> >           idq  ( &id( 3 ));
-    Map<Matrix<typename D2::Scalar, 3, 1> >         idv  ( &id( 7 ));
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp   ( & d( 0 ) );
+    Map<const Quaternion<typename D1::Scalar> >     dq   ( & d( 3 ) );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv   ( & d( 7 ) );
+    Map<Matrix<typename D2::Scalar, 3, 1> >         idp  ( & id( 0 ) );
+    Map<Quaternion<typename D2::Scalar> >           idq  ( & id( 3 ) );
+    Map<Matrix<typename D2::Scalar, 3, 1> >         idv  ( & id( 7 ) );
 
-    idp = - ( dq.conjugate() * (dp - dv * dt) );
-    idv = - ( dq.conjugate() * dv );
-    idq =     dq.conjugate();
+    inverse(dp, dq, dv, dt, idp, idq, idv);
 }
 
 template<typename D, class T>
@@ -51,12 +84,27 @@ inline Matrix<typename D::Scalar, 10, 1> inverse(const MatrixBase<D>& d,
                                                  T dt)
 {
     Matrix<typename D::Scalar, 10, 1> id;
-
     inverse(d, dt, id);
-
     return id;
 }
 
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
+inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
+                    const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
+                    const T dt,
+                    MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q, MatrixBase<D9>& sum_v )
+{
+        MatrixSizeCheck<3, 1>::check(dp1);
+        MatrixSizeCheck<3, 1>::check(dv1);
+        MatrixSizeCheck<3, 1>::check(dp2);
+        MatrixSizeCheck<3, 1>::check(dv2);
+        MatrixSizeCheck<3, 1>::check(sum_p);
+        MatrixSizeCheck<3, 1>::check(sum_v);
+
+        sum_p = dp1 + dv1*dt + dq1*dp2;
+        sum_v = dv1 +          dq1*dv2;
+        sum_q =                dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
+}
 
 template<typename D1, typename D2, typename D3, class T>
 inline void compose(const MatrixBase<D1>& d1,
@@ -68,19 +116,17 @@ inline void compose(const MatrixBase<D1>& d1,
     MatrixSizeCheck<10, 1>::check(d2);
     MatrixSizeCheck<10, 1>::check(sum);
 
-    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( &d1( 0 ) );
-    Map<const Quaternion<typename D1::Scalar> >     dq1    ( &d1( 3 ) );
-    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv1    ( &d1( 7 ) );
-    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( &d2( 0 ) );
-    Map<const Quaternion<typename D2::Scalar> >     dq2    ( &d2( 3 ) );
-    Map<const Matrix<typename D2::Scalar, 3, 1> >   dv2    ( &d2( 7 ) );
-    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_p  ( &sum( 0 ));
-    Map<Quaternion<typename D3::Scalar> >           sum_q  ( &sum( 3 ));
-    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_v  ( &sum( 7 ));
-
-    sum_p = dp1 + dv1*dt + dq1*dp2;
-    sum_v = dv1 +          dq1*dv2;
-    sum_q =                dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & d1( 0 ) );
+    Map<const Quaternion<typename D1::Scalar> >     dq1    ( & d1( 3 ) );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv1    ( & d1( 7 ) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( & d2( 0 ) );
+    Map<const Quaternion<typename D2::Scalar> >     dq2    ( & d2( 3 ) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dv2    ( & d2( 7 ) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_p  ( & sum( 0 ) );
+    Map<Quaternion<typename D3::Scalar> >           sum_q  ( & sum( 3 ) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_v  ( & sum( 7 ) );
+
+    compose(dp1, dq1, dv1, dp2, dq2, dv2, dt, sum_p, sum_q, sum_v);
 }
 
 template<typename D1, typename D2, class T>
@@ -108,15 +154,15 @@ inline void compose(const MatrixBase<D1>& d1,
     MatrixSizeCheck< 9, 1>::check(J_sum_d2);
 
     // Maps over provided data
-    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( &d1( 0 ) );
-    Map<const Quaternion<typename D1::Scalar> >     dq1    ( &d1( 3 ) );
-    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv1    ( &d1( 7 ) );
-    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( &d2( 0 ) );
-    Map<const Quaternion<typename D2::Scalar> >     dq2    ( &d2( 3 ) );
-    Map<const Matrix<typename D2::Scalar, 3, 1> >   dv2    ( &d2( 7 ) );
-    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_p  ( &sum( 0 ));
-    Map<Quaternion<typename D3::Scalar> >           sum_q  ( &sum( 3 ));
-    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_v  ( &sum( 7 ));
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & d1( 0 ) );
+    Map<const Quaternion<typename D1::Scalar> >     dq1    ( & d1( 3 ) );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv1    ( & d1( 7 ) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( & d2( 0 ) );
+    Map<const Quaternion<typename D2::Scalar> >     dq2    ( & d2( 3 ) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dv2    ( & d2( 7 ) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_p  ( & sum( 0 ) );
+    Map<Quaternion<typename D3::Scalar> >           sum_q  ( & sum( 3 ) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_v  ( & sum( 7 ) );
 
     // Some useful temporaries
     Matrix<typename D1::Scalar, 3, 3> dR1 = dq1.matrix(); // First  Delta, DR
@@ -139,6 +185,24 @@ inline void compose(const MatrixBase<D1>& d1,
     compose(d1, d2, dt, sum);
 }
 
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
+inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
+                    const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
+                    const T dt,
+                    MatrixBase<D7>& diff_p, QuaternionBase<D8>& diff_q, MatrixBase<D9>& diff_v )
+{
+        MatrixSizeCheck<3, 1>::check(dp1);
+        MatrixSizeCheck<3, 1>::check(dv1);
+        MatrixSizeCheck<3, 1>::check(dp2);
+        MatrixSizeCheck<3, 1>::check(dv2);
+        MatrixSizeCheck<3, 1>::check(diff_p);
+        MatrixSizeCheck<3, 1>::check(diff_v);
+
+        diff_p = dq1.conjugate() * ( dp2 - dp1 - dv1*dt );
+        diff_q = dq1.conjugate() *   dq2;
+        diff_v = dq1.conjugate() * ( dv2 - dv1 );
+}
+
 template<typename D1, typename D2, typename D3, class T>
 inline void between(const MatrixBase<D1>& d1,
                     const MatrixBase<D2>& d2,
@@ -149,19 +213,17 @@ inline void between(const MatrixBase<D1>& d1,
     MatrixSizeCheck<10, 1>::check(d2);
     MatrixSizeCheck<10, 1>::check(d2_minus_d1);
 
-    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( &d1(0) );
-    Map<const Quaternion<typename D1::Scalar> >     dq1    ( &d1(3) );
-    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv1    ( &d1(7) );
-    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( &d2(0) );
-    Map<const Quaternion<typename D2::Scalar> >     dq2    ( &d2(3) );
-    Map<const Matrix<typename D2::Scalar, 3, 1> >   dv2    ( &d2(7) );
-    Map<Matrix<typename D3::Scalar, 3, 1> >         diff_p (&d2_minus_d1(0));
-    Map<Quaternion<typename D3::Scalar> >           diff_q (&d2_minus_d1(3));
-    Map<Matrix<typename D3::Scalar, 3, 1> >         diff_v (&d2_minus_d1(7));
-
-    diff_p = dq1.conjugate() * ( dp2 - dp1 - dv1*dt );
-    diff_q = dq1.conjugate() *   dq2;
-    diff_v = dq1.conjugate() * ( dv2 - dv1 );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & d1(0) );
+    Map<const Quaternion<typename D1::Scalar> >     dq1    ( & d1(3) );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv1    ( & d1(7) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( & d2(0) );
+    Map<const Quaternion<typename D2::Scalar> >     dq2    ( & d2(3) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dv2    ( & d2(7) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         diff_p ( & d2_minus_d1(0) );
+    Map<Quaternion<typename D3::Scalar> >           diff_q ( & d2_minus_d1(3) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         diff_v ( & d2_minus_d1(7) );
+
+    between(dp1, dq1, dv1, dp2, dq2, dv2, dt, diff_p, diff_q, diff_v);
 }
 
 template<typename D1, typename D2, class T>
@@ -169,9 +231,9 @@ inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1,
                                                   const MatrixBase<D2>& d2,
                                                   T dt)
 {
-    Matrix<typename D1::Scalar, 10, 1> d_bet;
-    between(d1, d2, dt, d_bet);
-    return d_bet;
+    Matrix<typename D1::Scalar, 10, 1> diff;
+    between(d1, d2, dt, diff);
+    return diff;
 }
 
 template<typename D1, typename D2, typename D3, class T>
@@ -184,15 +246,15 @@ inline void composeOverState(const MatrixBase<D1>& x,
     MatrixSizeCheck<10, 1>::check(d);
     MatrixSizeCheck<10, 1>::check(x_plus_d);
 
-    Map<const Matrix<typename D1::Scalar, 3, 1> >   p         ( &x( 0 ) );
-    Map<const Quaternion<typename D1::Scalar> >     q         ( &x( 3 ) );
-    Map<const Matrix<typename D1::Scalar, 3, 1> >   v         ( &x( 7 ) );
-    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp        ( &d( 0 ) );
-    Map<const Quaternion<typename D2::Scalar> >     dq        ( &d( 3 ) );
-    Map<const Matrix<typename D2::Scalar, 3, 1> >   dv        ( &d( 7 ) );
-    Map<Matrix<typename D3::Scalar, 3, 1> >         p_plus_d  ( &x_plus_d( 0 ));
-    Map<Quaternion<typename D3::Scalar> >           q_plus_d  ( &x_plus_d( 3 ));
-    Map<Matrix<typename D3::Scalar, 3, 1> >         v_plus_d  ( &x_plus_d( 7 ));
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   p         ( & x( 0 ) );
+    Map<const Quaternion<typename D1::Scalar> >     q         ( & x( 3 ) );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   v         ( & x( 7 ) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp        ( & d( 0 ) );
+    Map<const Quaternion<typename D2::Scalar> >     dq        ( & d( 3 ) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dv        ( & d( 7 ) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         p_plus_d  ( & x_plus_d( 0 ) );
+    Map<Quaternion<typename D3::Scalar> >           q_plus_d  ( & x_plus_d( 3 ) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         v_plus_d  ( & x_plus_d( 7 ) );
 
     p_plus_d = p + v*dt + 0.5*gravity()*dt*dt + q*dp;
     v_plus_d = v +            gravity()*dt    + q*dv;
@@ -209,6 +271,24 @@ inline Matrix<typename D1::Scalar, 10, 1> composeOverState(const MatrixBase<D1>&
     return ret;
 }
 
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
+inline void betweenStates(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, const MatrixBase<D3>& v1,
+                          const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2, const MatrixBase<D6>& v2,
+                          const T dt,
+                          MatrixBase<D7>& dp, QuaternionBase<D8>& dq, MatrixBase<D9>& dv )
+{
+        MatrixSizeCheck<3, 1>::check(p1);
+        MatrixSizeCheck<3, 1>::check(v1);
+        MatrixSizeCheck<3, 1>::check(p2);
+        MatrixSizeCheck<3, 1>::check(v2);
+        MatrixSizeCheck<3, 1>::check(dp);
+        MatrixSizeCheck<3, 1>::check(dv);
+
+        dp = q1.conjugate() * ( p2 - p1 - v1*dt - 0.5*gravity()*dt*dt );
+        dq = q1.conjugate() *   q2;
+        dv = q1.conjugate() * ( v2 - v1         -     gravity()*dt );
+}
+
 template<typename D1, typename D2, typename D3, class T>
 inline void betweenStates(const MatrixBase<D1>& x1,
                           const MatrixBase<D2>& x2,
@@ -219,19 +299,17 @@ inline void betweenStates(const MatrixBase<D1>& x1,
     MatrixSizeCheck<10, 1>::check(x2);
     MatrixSizeCheck<10, 1>::check(x2_minus_x1);
 
-    Map<const Matrix<typename D1::Scalar, 3, 1> >   p1  ( &x1(0) );
-    Map<const Quaternion<typename D1::Scalar> >     q1  ( &x1(3) );
-    Map<const Matrix<typename D1::Scalar, 3, 1> >   v1  ( &x1(7) );
-    Map<const Matrix<typename D2::Scalar, 3, 1> >   p2  ( &x2(0) );
-    Map<const Quaternion<typename D2::Scalar> >     q2  ( &x2(3) );
-    Map<const Matrix<typename D2::Scalar, 3, 1> >   v2  ( &x2(7) );
-    Map<Matrix<typename D3::Scalar, 3, 1> >         dp  (&x2_minus_x1(0));
-    Map<Quaternion<typename D3::Scalar> >           dq  (&x2_minus_x1(3));
-    Map<Matrix<typename D3::Scalar, 3, 1> >         dv  (&x2_minus_x1(7));
-
-    dp = q1.conjugate() * ( p2 - p1 - v1*dt - 0.5*gravity()*dt*dt );
-    dq = q1.conjugate() *   q2;
-    dv = q1.conjugate() * ( v2 - v1         -     gravity()*dt );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   p1  ( & x1(0) );
+    Map<const Quaternion<typename D1::Scalar> >     q1  ( & x1(3) );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   v1  ( & x1(7) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   p2  ( & x2(0) );
+    Map<const Quaternion<typename D2::Scalar> >     q2  ( & x2(3) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   v2  ( & x2(7) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         dp  ( & x2_minus_x1(0) );
+    Map<Quaternion<typename D3::Scalar> >           dq  ( & x2_minus_x1(3) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         dv  ( & x2_minus_x1(7) );
+
+    betweenStates(p1, q1, v1, p2, q2, v2, dt, dp, dq, dv);
 }
 
 template<typename D1, typename D2, class T>
@@ -251,12 +329,12 @@ Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
 
     Matrix<typename Derived::Scalar, 9, 1> ret;
 
-    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dp_in  ( &delta_in(0) );
-    Map<const Quaternion<typename Derived::Scalar> >     dq_in  ( &delta_in(3) );
-    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dv_in  ( &delta_in(7) );
-    Map<Matrix<typename Derived::Scalar, 3, 1> >         dp_ret     ( & ret(0) );
-    Map<Matrix<typename Derived::Scalar, 3, 1> >         do_ret     ( & ret(3) );
-    Map<Matrix<typename Derived::Scalar, 3, 1> >         dv_ret     ( & ret(6) );
+    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dp_in  ( & delta_in(0) );
+    Map<const Quaternion<typename Derived::Scalar> >     dq_in  ( & delta_in(3) );
+    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dv_in  ( & delta_in(7) );
+    Map<Matrix<typename Derived::Scalar, 3, 1> >         dp_ret ( & ret(0) );
+    Map<Matrix<typename Derived::Scalar, 3, 1> >         do_ret ( & ret(3) );
+    Map<Matrix<typename Derived::Scalar, 3, 1> >         dv_ret ( & ret(6) );
 
     dp_ret = dp_in;
     do_ret = log_q(dq_in);
@@ -272,12 +350,12 @@ Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_in)
 
     Matrix<typename Derived::Scalar, 10, 1> ret;
 
-    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dp_in  ( &d_in(0) );
-    Map<const Matrix<typename Derived::Scalar, 3, 1> >   do_in  ( &d_in(3) );
-    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dv_in  ( &d_in(6) );
-    Map<Matrix<typename Derived::Scalar, 3, 1> >         dp     ( & ret(0) );
-    Map<Quaternion<typename Derived::Scalar> >           dq     ( & ret(3) );
-    Map<Matrix<typename Derived::Scalar, 3, 1> >         dv     ( & ret(7) );
+    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dp_in  ( & d_in(0) );
+    Map<const Matrix<typename Derived::Scalar, 3, 1> >   do_in  ( & d_in(3) );
+    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dv_in  ( & d_in(6) );
+    Map<Matrix<typename Derived::Scalar, 3, 1> >         dp     ( &  ret(0) );
+    Map<Quaternion<typename Derived::Scalar> >           dq     ( &  ret(3) );
+    Map<Matrix<typename Derived::Scalar, 3, 1> >         dv     ( &  ret(7) );
 
     dp = dp_in;
     dq = exp_q(do_in);
diff --git a/src/test/gtest_imu_tools.cpp b/src/test/gtest_imu_tools.cpp
index 5588df879e2d12e05f1fa469a43099995f17d848..37be31e3bde637e1b55ea65e627d1605d263ae24 100644
--- a/src/test/gtest_imu_tools.cpp
+++ b/src/test/gtest_imu_tools.cpp
@@ -23,6 +23,22 @@ TEST(IMU_tools, identity)
     ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
 }
 
+TEST(IMU_tools, identity_split)
+{
+    VectorXs p(3), qv(4), v(3);
+    Quaternions q;
+
+    imu::identity(p,q,v);
+    ASSERT_MATRIX_APPROX(p, Vector3s::Zero(), 1e-10);
+    ASSERT_QUATERNION_APPROX(q, Quaternions::Identity(), 1e-10);
+    ASSERT_MATRIX_APPROX(v, Vector3s::Zero(), 1e-10);
+
+    imu::identity(p,qv,v);
+    ASSERT_MATRIX_APPROX(p , Vector3s::Zero(), 1e-10);
+    ASSERT_MATRIX_APPROX(qv, (Vector4s()<<0,0,0,1).finished(), 1e-10);
+    ASSERT_MATRIX_APPROX(v , Vector3s::Zero(), 1e-10);
+}
+
 TEST(IMU_tools, inverse)
 {
     VectorXs d(10), id(10), iid(10), iiid(10), I(10);