From 27f784649533a4a70e8ed8dfd0cca02f9de760d6 Mon Sep 17 00:00:00 2001 From: Roman Date: Sun, 2 Dec 2018 16:34:28 +0100 Subject: [PATCH] optical flow: fixed sign in calculation of optical flow sensor velocity - the velocity of the optical flow sensor relative to the IMU expressed in body frame is the cross product of the angular velocity with the vector from IMU to the sensor. If we use the angular velocity stored in the flow sample struct we need to use a negative sign as that angular velocity follows the opposite sign convention. Signed-off-by: Roman --- EKF/optflow_fusion.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 370d640adf..c680f3a846 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -75,7 +75,8 @@ void Ekf::fuseOptFlow() Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; // calculate the velocity of the sensor reelative to the imu in body frame - Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt, pos_offset_body); + // Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign + Vector3f vel_rel_imu_body = cross_product(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt, pos_offset_body); // calculate the velocity of the sensor in the earth frame Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;