From 77acba5c52451b88260e8cc21aa5eae23093611a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Wed, 9 Oct 2019 19:21:24 +0200
Subject: [PATCH] Exp Log IMU Lie test passing
---
include/IMU/math/IMU_tools_Lie.h | 15 ++++++---------
1 file changed, 6 insertions(+), 9 deletions(-)
diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index a6abf5657..f0297c337 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;
}
--
GitLab