From f20a5d814d55d88681c7b2fefae104c2fb85f4ab Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 13 Aug 2020 17:43:36 +1000 Subject: [PATCH] EKF: Remove redundant tilt check Also fix comment indent --- EKF/optflow_fusion.cpp | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index a4dd1f9f3f..4d2e0691b7 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -48,14 +48,6 @@ void Ekf::fuseOptFlow() { - // get rotation matrix from earth to body - const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal); - - if (earth_to_body(2,2) < _params.range_cos_max_tilt) { - // vehicle is excessively tilted so optical flow will not work - return; - } - float gndclearance = fmaxf(_params.rng_gnd_clearance, 0.1f); // get latest estimated orientation @@ -75,6 +67,9 @@ void Ekf::fuseOptFlow() SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians Vector24f Kfusion; // Optical flow Kalman gains + // get rotation matrix from earth to body + const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal); + // calculate the sensor position relative to the IMU const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; @@ -117,9 +112,9 @@ void Ekf::fuseOptFlow() return; } - // The derivation allows for an arbitrary body to flow sensor frame rotation which is - // currently not supported by the EKF, so assume sensor frame is aligned with the - // body frame + // The derivation allows for an arbitrary body to flow sensor frame rotation which is + // currently not supported by the EKF, so assume sensor frame is aligned with the + // body frame Dcmf Tbs; Tbs.identity();