From 935de1dbdce50167baa0d1089e254337cfda2958 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 24 Oct 2019 14:04:00 +0200
Subject: [PATCH] Fix formula of matrix Q
---
include/IMU/math/IMU_tools_Lie.h | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index 6ea135922..a2140e099 100644
--- a/include/IMU/math/IMU_tools_Lie.h
+++ b/include/IMU/math/IMU_tools_Lie.h
@@ -431,7 +431,7 @@ inline void QandPmat(const MatrixBase<D1>& th, MatrixBase<D2>& Q, MatrixBase<D3>
Q = Id
+ ((1 - cos_thn)/thn)*ux
- + ((thn - sin_thn)/thn2)*(ux2);
+ + ((thn - sin_thn)/thn)*(ux2);
P = 0.5*Id
+ ((thn - sin_thn)/thn2)*ux
--
GitLab