From 1f5775615ffb04f22456795ea39cb1402ae51d5f Mon Sep 17 00:00:00 2001 From: Andrea Nistico Date: Fri, 24 Apr 2015 16:45:17 +0200 Subject: Rvis transposed, in this way we have consisency --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index b2a3572a7..68780d5dd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -465,8 +465,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) math::Vector<3> v(1.0f, 0.0f, 0.4f); - math::Vector<3> vn = Rvis * v; - + math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency z_k[6] = vn(0); z_k[7] = vn(1); z_k[8] = vn(2); -- cgit v1.2.3