Exp Log IMU Lie test passing

include/IMU/math/IMU_tools_Lie.h

@@ -382,7 +382,7 @@ inline Matrix<typename D1::Scalar, 11, 1> betweenStates(const MatrixBase<D1>& x1
 template<typename D1, typename D2, typename D3>
 inline void QandPmat(const MatrixBase<D1>& th, MatrixBase<D2>& Q, MatrixBase<D3>& P){
     // See equations 29-31 humanoids fourmy 19
-    // th = omega * dt
+    // th = omega * dt is elt of the IMU Lie group tangent space
     MatrixSizeCheck<3, 1>::check(th);
     MatrixSizeCheck<3, 3>::check(Q);
     MatrixSizeCheck<3, 3>::check(P);
@@ -399,7 +399,7 @@ inline void QandPmat(const MatrixBase<D1>& th, MatrixBase<D2>& Q, MatrixBase<D3>
         Matrix<typename D1::Scalar, 3, 1> u = th / thn;
         Matrix<typename D1::Scalar, 3, 3> ux = skew(u);
 
-        Q = Id + 
+        Q = Id 
         + ((1  - cos(thn))/thn)*ux 
         + ((thn - sin(thn))/(thn*thn))*(ux*ux);
         
@@ -429,13 +429,9 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in)
     Matrix<typename Derived::Scalar, 3, 3> Q;
     Matrix<typename Derived::Scalar, 3, 3> P;
     QandPmat(do_in, Q, P);
-    std::cout << "Q" << std::endl;
-    std::cout << Q << std::endl;
-    std::cout << "P" << std::endl;
-    std::cout << P << std::endl;
 
-    dp = Q*dp_in + P*dv_in*dt_in;    
-    dv = Q*dp_in;
+    dp = Q*dp_in + P*dv_in*dt_in;
+    dv = Q*dv_in;
     dq = exp_q(do_in);
     dt = dt_in;
 
@@ -451,7 +447,7 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta
     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) );
-    const typename Derived::Scalar& dt_in = delta_in(9);
+    const typename Derived::Scalar& dt_in = delta_in(10);
     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) );
@@ -468,6 +464,7 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta
     dv_ret = Qinv*dv_in;
     dt_ret = dt_in;
 
+    std::cout << "Log ret" << ret.transpose() << std::endl;
     return ret;
 }