From cf1df5861a1af374dec8d27c3bf0848286ca0aa6 Mon Sep 17 00:00:00 2001 From: devbharat Date: Wed, 20 Apr 2016 11:43:11 +0200 Subject: [PATCH] Undid hack in ecl. Handled yaw missalignment on the vision side. --- EKF/ekf.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index f0e01dc3bc..d6072e1e5b 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -313,9 +313,9 @@ bool Ekf::update() // correct position and height for offset relative to IMU Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - // warnx("pos_offset_earth %lf %lf %lf", (double)pos_offset_earth(0)*100, (double)pos_offset_earth(1)*100, (double)pos_offset_earth(2)*100); - _ev_sample_delayed.posNED(0) += pos_offset_earth(1); // XXX FIX IT Hack After "_R_to_earth * pos_offset_body" x and y offset seem to be switched! - _ev_sample_delayed.posNED(1) -= pos_offset_earth(0); // XXX FIX IT Hack After "_R_to_earth * pos_offset_body" x and y offset seem to be switched! + warnx("pos_offset_earth %lf %lf %lf", (double)pos_offset_earth(0)*100, (double)pos_offset_earth(1)*100, (double)pos_offset_earth(2)*100); + _ev_sample_delayed.posNED(0) -= pos_offset_earth(0); + _ev_sample_delayed.posNED(1) -= pos_offset_earth(1); _ev_sample_delayed.posNED(2) -= pos_offset_earth(2); } // use external vision yaw observation