diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h index a6abf56578104e405775db1612c148aaa0d04c6b..f0297c3375350c3eb950a9fdc659febe107a96b0 100644 --- a/include/IMU/math/IMU_tools_Lie.h +++ b/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; }