diff --git a/EKF/control.cpp b/EKF/control.cpp index 3ff7e0cae6..68712f1915 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -144,12 +144,13 @@ void Ekf::controlFusionModes() // This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow, // in this case we need to empty the buffer if (!_flow_data_ready || !_control_status.flags.opt_flow) { - _flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) - && (_R_to_earth(2, 2) > _params.range_cos_max_tilt); + _flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed); } // check if we should fuse flow data for terrain estimation if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) { + // TODO: WARNING, _flow_data_ready can be modified in controlOpticalFlowFusion + // due to some checks failing // only fuse flow for terrain if range data hasn't been fused for 5 seconds _flow_for_terrain_data_ready = isTimedOut(_time_last_hagl_fuse, (uint64_t)5E6); // only fuse flow for terrain if the main filter is not fusing flow and we are using gps @@ -343,33 +344,56 @@ void Ekf::controlExternalVisionFusion() void Ekf::controlOpticalFlowFusion() { - // TODO: These motion checks run all the time. Pull them out of this function // Check if on ground motion is un-suitable for use of optical flow if (!_control_status.flags.in_air) { - // When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation - const float accel_norm = _accel_vec_filt.norm(); - - const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit - || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit - || (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit - || (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively - - if (motion_is_excessive) { - _time_bad_motion_us = _imu_sample_delayed.time_us; - - } else { - _time_good_motion_us = _imu_sample_delayed.time_us; - } + updateOnGroundMotionForOpticalFlowChecks(); } else { - _time_bad_motion_us = 0; - _time_good_motion_us = _imu_sample_delayed.time_us; + resetOnGroundMotionForOpticalFlowChecks(); } // Accumulate autopilot gyro data across the same time interval as the flow sensor _imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.delta_ang_bias; _delta_time_of += _imu_sample_delayed.delta_ang_dt; + if (_flow_data_ready) { + const bool is_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min); + const bool is_magnitude_good = !_flow_sample_delayed.flow_xy_rad.longerThan(_flow_sample_delayed.dt * _flow_max_rate); + const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt); + + const float delta_time_min = fmaxf(0.8f * _delta_time_of, 0.001f); + const float delta_time_max = fminf(1.2f * _delta_time_of, 0.2f); + const bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min + && _flow_sample_delayed.dt <= delta_time_max; + const bool is_body_rate_comp_available = calcOptFlowBodyRateComp(); + + if (is_quality_good + && is_magnitude_good + && is_tilt_good + && is_body_rate_comp_available + && is_delta_time_good) { + // compensate for body motion to give a LOS rate + _flow_compensated_XY_rad = _flow_sample_delayed.flow_xy_rad - _flow_sample_delayed.gyro_xyz.xy(); + + } else if (!_control_status.flags.in_air && is_body_rate_comp_available) { + + if (!is_delta_time_good) { + // handle special case of SITL and PX4Flow where dt is forced to + // zero when the quaity is 0 + _flow_sample_delayed.dt = delta_time_min; + } + + // when on the ground with poor flow quality, + // assume zero ground relative velocity and LOS rate + _flow_compensated_XY_rad.setZero(); + + } else { + // don't use this flow data and wait for the next data to arrive + _flow_data_ready = false; + _flow_for_terrain_data_ready = false; // TODO: find a better place + } + } + // New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon if (_flow_data_ready) { // Inhibit flow use if motion is un-suitable or we have good quality GPS @@ -377,47 +401,38 @@ void Ekf::controlOpticalFlowFusion() const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f; // Check if we are in-air and require optical flow to control position drift - bool flow_required = _control_status.flags.in_air && - (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently - || (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) - || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad + const bool is_flow_required = _control_status.flags.in_air + && (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently + || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow) + || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad - if (!_inhibit_flow_use && _control_status.flags.opt_flow) { - // inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation - bool preflight_motion_not_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5); - bool flight_motion_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid(); - if ((preflight_motion_not_ok || flight_motion_not_ok) && !flow_required) { - _inhibit_flow_use = true; - } - } else if (_inhibit_flow_use && !_control_status.flags.opt_flow){ - // allow use of optical flow if motion is suitable or we are reliant on it for flight navigation - bool preflight_motion_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6); - bool flight_motion_ok = _control_status.flags.in_air && isRangeAidSuitable(); - if (preflight_motion_ok || flight_motion_ok || flow_required) { - _inhibit_flow_use = false; - } - } - // Handle cases where we are using optical flow but are no longer able to because data is old - // or its use has been inhibited. + // inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation + const bool preflight_motion_not_ok = !_control_status.flags.in_air + && ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5)) + || (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6))); + const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid(); + + _inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required) + || !_control_status.flags.tilt_align; + + // Handle cases where we are using optical flow but we should not use it anymore if (_control_status.flags.opt_flow) { - if (_inhibit_flow_use) { - stopFlowFusion(); - _time_last_of_fuse = 0; + if (!(_params.fusion_mode & MASK_USE_OF) + || _inhibit_flow_use) { - } else if (isTimedOut(_time_last_of_fuse, (uint64_t)_params.reset_timeout_max)) { stopFlowFusion(); + return; } } // optical flow fusion mode selection logic if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user && !_control_status.flags.opt_flow // we are not yet using flow data - && _control_status.flags.tilt_align // we know our tilt attitude - && !_inhibit_flow_use - && isTerrainEstimateValid()) + && !_inhibit_flow_use) { // If the heading is not aligned, reset the yaw and magnetic field states + // TODO: ekf2 should always try to align itself if not already aligned if (!_control_status.flags.yaw_align) { _control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState()); } @@ -430,61 +445,65 @@ void Ekf::controlOpticalFlowFusion() // if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set const bool flow_aid_only = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow); + if (flow_aid_only) { - // TODO: Issue: First time we want to reset the optical flow velocity - // we will use _flow_compensated_XY_rad in the resetVelocity() function, - // but _flow_compensated_XY_rad is this zero as it gets updated for the first time below here. resetVelocity(); resetHorizontalPosition(); } } - - } else if (!(_params.fusion_mode & MASK_USE_OF)) { - _control_status.flags.opt_flow = false; } - // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period - if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) { + if (_control_status.flags.opt_flow) { + // Wait until the midpoint of the flow sample has fallen behind the fusion time horizon + if (_imu_sample_delayed.time_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) { + // Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently + // but use a relaxed time criteria to enable it to coast through bad range finder data + if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) { + fuseOptFlow(); + _last_known_posNE = _state.pos.xy(); + } - bool do_reset = isTimedOut(_time_last_of_fuse, _params.reset_timeout_max); + _flow_data_ready = false; + } + + // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period + if (isTimedOut(_time_last_of_fuse, _params.reset_timeout_max) + && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) { - if (do_reset) { resetVelocity(); resetHorizontalPosition(); } } - // Only fuse optical flow if valid body rate compensation data is available - if (calcOptFlowBodyRateComp()) { - - bool flow_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min); - - if (!flow_quality_good && !_control_status.flags.in_air) { - // when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate - _flow_compensated_XY_rad.setZero(); - } else { - // compensate for body motion to give a LOS rate - _flow_compensated_XY_rad = _flow_sample_delayed.flow_xy_rad - _flow_sample_delayed.gyro_xyz.xy(); - } - } else { - // don't use this flow data and wait for the next data to arrive - _flow_data_ready = false; - } + } else if (_control_status.flags.opt_flow && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)10e6)) { + stopFlowFusion(); } +} - // Wait until the midpoint of the flow sample has fallen behind the fusion time horizon - if (_flow_data_ready && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) { - // Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently - // but use a relaxed time criteria to enable it to coast through bad range finder data - if (_control_status.flags.opt_flow && isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) { - fuseOptFlow(); - _last_known_posNE = _state.pos.xy(); - } +void Ekf::updateOnGroundMotionForOpticalFlowChecks() +{ + // When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation + const float accel_norm = _accel_vec_filt.norm(); - _flow_data_ready = false; + const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit + || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit + || (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit + || (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively + + if (motion_is_excessive) { + _time_bad_motion_us = _imu_sample_delayed.time_us; + + } else { + _time_good_motion_us = _imu_sample_delayed.time_us; } } +void Ekf::resetOnGroundMotionForOpticalFlowChecks() +{ + _time_bad_motion_us = 0; + _time_good_motion_us = _imu_sample_delayed.time_us; +} + void Ekf::controlGpsFusion() { // Check for new GPS data that has fallen behind the fusion time horizon diff --git a/EKF/ekf.h b/EKF/ekf.h index e075b367ae..39a1a33381 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -753,6 +753,8 @@ private: // control fusion of optical flow observations void controlOpticalFlowFusion(); + void updateOnGroundMotionForOpticalFlowChecks(); + void resetOnGroundMotionForOpticalFlowChecks(); // control fusion of GPS observations void controlGpsFusion(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 0939e228c7..f6fa48d2eb 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -79,7 +79,7 @@ void Ekf::resetHorizontalVelocityToOpticalFlow() // calculate absolute distance from focal point to centre of frame assuming a flat earth const float range = heightAboveGndEst / _range_sensor.getCosTilt(); - if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { + if ((range - _params.rng_gnd_clearance) > 0.3f) { // we should have reliable OF measurements so // calculate X and Y body relative velocities from OF measurements Vector3f vel_optflow_body; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 10d3ff8314..b25eb591e8 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -354,47 +354,14 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow) // limit data rate to prevent data being lost if ((flow.time_us - _time_last_optflow) > _min_obs_interval_us) { - // check if enough integration time and fail if integration time is less than 50% - // of min arrival interval because too much data is being lost - float delta_time = flow.dt; // in seconds - const float delta_time_min = 0.5e-6f * (float)_min_obs_interval_us; - const bool delta_time_good = delta_time >= delta_time_min; + _time_last_optflow = flow.time_us; - bool flow_magnitude_good = true; + flowSample optflow_sample_new = flow; - if (delta_time_good) { - // check magnitude is within sensor limits - // use this to prevent use of a saturated flow sensor - // when there are other aiding sources available - const float flow_rate_magnitude = flow.flow_xy_rad.norm() / delta_time; - flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate); + optflow_sample_new.time_us -= _params.flow_delay_ms * 1000; + optflow_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - } else { - // protect against overflow caused by division with very small delta_time - delta_time = delta_time_min; - } - - const bool relying_on_flow = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow); - - const bool flow_quality_good = (flow.quality >= _params.flow_qual_min); - - // Check data validity and write to buffers - // Invalid flow data is allowed when on ground and is handled as a special case in controlOpticalFlowFusion() - bool use_flow_data_to_navigate = delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow); - - if (use_flow_data_to_navigate || (!_control_status.flags.in_air && relying_on_flow)) { - - _time_last_optflow = flow.time_us; - - flowSample optflow_sample_new = flow; - - optflow_sample_new.time_us -= _params.flow_delay_ms * 1000; - optflow_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - - optflow_sample_new.dt = delta_time; - - _flow_buffer.push(optflow_sample_new); - } + _flow_buffer.push(optflow_sample_new); } } diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index b09f0950c2..a5b5415c28 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -108,13 +108,8 @@ void Ekf::fuseOptFlow() _flow_vel_body(1) = opt_flow_rate(0) * range; _flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f)); - if (opt_flow_rate.norm() < _flow_max_rate) { - _flow_innov(0) = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis - _flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis - - } else { - return; - } + _flow_innov(0) = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis + _flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis // 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 @@ -350,7 +345,9 @@ bool Ekf::calcOptFlowBodyRateComp() // if accumulation time differences are not excessive and accumulation time is adequate // compare the optical flow and and navigation rate data and calculate a bias error - if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) { + if ((_delta_time_of > FLT_EPSILON) + && (_flow_sample_delayed.dt > FLT_EPSILON) + && (fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f)) { const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of)); diff --git a/test/sensor_simulator/flow.cpp b/test/sensor_simulator/flow.cpp index 5026b6db81..2ef66d019d 100644 --- a/test/sensor_simulator/flow.cpp +++ b/test/sensor_simulator/flow.cpp @@ -15,7 +15,6 @@ Flow::~Flow() void Flow::send(uint64_t time) { - _flow_data.dt = static_cast(time - _time_last_data_sent) * 1e-6f; _flow_data.time_us = time; _ekf->setOpticalFlowData(_flow_data); } @@ -28,12 +27,12 @@ void Flow::setData(const flowSample& flow) flowSample Flow::dataAtRest() { - flowSample _flow_at_rest; - _flow_at_rest.dt = 0.02f; - _flow_at_rest.flow_xy_rad = Vector2f{0.0f, 0.0f}; - _flow_at_rest.gyro_xyz = Vector3f{0.0f, 0.0f, 0.0f}; - _flow_at_rest.quality = 255; - return _flow_at_rest; + flowSample flow_at_rest; + flow_at_rest.dt = static_cast(_update_period) * 1e-6f; + flow_at_rest.flow_xy_rad = Vector2f{0.0f, 0.0f}; + flow_at_rest.gyro_xyz = Vector3f{0.0f, 0.0f, 0.0f}; + flow_at_rest.quality = 255; + return flow_at_rest; } } // namespace sensor diff --git a/test/sensor_simulator/sensor_simulator.cpp b/test/sensor_simulator/sensor_simulator.cpp index 00bf310727..8b836607ca 100644 --- a/test/sensor_simulator/sensor_simulator.cpp +++ b/test/sensor_simulator/sensor_simulator.cpp @@ -12,8 +12,8 @@ _rng(ekf), _vio(ekf), _airspeed(ekf) { - setSensorDataToDefault(); setSensorRateToDefault(); + setSensorDataToDefault(); startBasicSensor(); } diff --git a/test/test_EKF_flow.cpp b/test/test_EKF_flow.cpp index f3684e641a..0268de4714 100644 --- a/test/test_EKF_flow.cpp +++ b/test/test_EKF_flow.cpp @@ -58,6 +58,11 @@ class EkfFlowTest : public ::testing::Test { // Setup the Ekf with synthetic measurements void SetUp() override { + const float max_flow_rate = 5.f; + const float min_ground_distance = 0.f; + const float max_ground_distance = 50.f; + _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); + _ekf->init(0); _sensor_simulator.runSeconds(7); } @@ -93,18 +98,17 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir) -simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground); _sensor_simulator._flow.setData(flow_sample); _ekf_wrapper.enableFlowFusion(); - const float max_flow_rate = 5.f; - const float min_ground_distance = 0.f; - const float max_ground_distance = 50.f; - _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); _sensor_simulator.startFlow(); - _sensor_simulator.runSeconds(0.12); // Let it reset but not fuse more measurements + // Let it reset but not fuse more measurements. We actually need to send 2 + // samples to get a reset because the first one cannot be used as the gyro + // compensation needs to be accumulated between two samples. + _sensor_simulator.runSeconds(0.14); // THEN: estimated velocity should match simulated velocity const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity()); - EXPECT_FALSE(isEqual(estimated_horz_velocity, simulated_horz_velocity)) + EXPECT_TRUE(isEqual(estimated_horz_velocity, simulated_horz_velocity)) << "estimated vel = " << estimated_horz_velocity(0) << ", " - << estimated_horz_velocity(1); // TODO: This needs to change (reset is always 0) + << estimated_horz_velocity(1); // AND: the reset in velocity should be saved correctly reset_logging_checker.capturePostResetState(); @@ -124,13 +128,18 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround) reset_logging_checker.capturePreResetState(); // WHEN: start fusing flow data + flowSample flow_sample = _sensor_simulator._flow.dataAtRest(); + flow_sample.dt = 0.f; // some sensors force dt to zero when quality is low + flow_sample.quality = 0; + _sensor_simulator._flow.setData(flow_sample); _ekf_wrapper.enableFlowFusion(); _sensor_simulator.startFlow(); _sensor_simulator.runSeconds(1.0); // THEN: estimated velocity should match simulated velocity const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity()); - EXPECT_TRUE(isEqual(estimated_horz_velocity, Vector2f(0.f, 0.f))); + EXPECT_TRUE(isEqual(estimated_horz_velocity, Vector2f(0.f, 0.f))) + << estimated_horz_velocity(0) << ", " << estimated_horz_velocity(1); // AND: the reset in velocity should be saved correctly reset_logging_checker.capturePostResetState(); @@ -138,3 +147,52 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround) EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); EXPECT_TRUE(reset_logging_checker.isVelocityDeltaLoggedCorrectly(1e-9f)); } + +TEST_F(EkfFlowTest, inAirConvergence) +{ + // WHEN: simulate being 5m above ground + const float simulated_distance_to_ground = 5.f; + _sensor_simulator._rng.setData(simulated_distance_to_ground, 100); + _sensor_simulator._rng.setLimits(0.1f, 9.f); + _sensor_simulator.startRangeFinder(); + _ekf->set_in_air_status(true); + _sensor_simulator.runSeconds(5.f); + + const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); + + // WHEN: start fusing flow data + Vector2f simulated_horz_velocity(0.5f, -0.2f); + flowSample flow_sample = _sensor_simulator._flow.dataAtRest(); + flow_sample.flow_xy_rad = + Vector2f( simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground, + -simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground); + _sensor_simulator._flow.setData(flow_sample); + _ekf_wrapper.enableFlowFusion(); + _sensor_simulator.startFlow(); + // Let it reset but not fuse more measurements. We actually need to send 2 + // samples to get a reset because the first one cannot be used as the gyro + // compensation needs to be accumulated between two samples. + _sensor_simulator.runSeconds(0.14); + + // THEN: estimated velocity should match simulated velocity + Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity()); + EXPECT_TRUE(isEqual(estimated_horz_velocity, simulated_horz_velocity)) + << "estimated vel = " << estimated_horz_velocity(0) << ", " + << estimated_horz_velocity(1); + + // AND: when the velocity changes + simulated_horz_velocity = Vector2f(0.8f, -0.5f); + flow_sample.flow_xy_rad = + Vector2f( simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground, + -simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground); + _sensor_simulator._flow.setData(flow_sample); + _sensor_simulator.runSeconds(5.0); + + // THEN: estimated velocity should converge to the simulated velocity + // This takes a bit of time because the data is inconsistent with IMU measurements + estimated_horz_velocity = Vector2f(_ekf->getVelocity()); + EXPECT_NEAR(estimated_horz_velocity(0), simulated_horz_velocity(0), 0.05f) + << "estimated vel = " << estimated_horz_velocity(0); + EXPECT_NEAR(estimated_horz_velocity(1), simulated_horz_velocity(1), 0.05f) + << estimated_horz_velocity(1); +} diff --git a/test/test_EKF_fusionLogic.cpp b/test/test_EKF_fusionLogic.cpp index 367b70e813..ca7ef3b37e 100644 --- a/test/test_EKF_fusionLogic.cpp +++ b/test/test_EKF_fusionLogic.cpp @@ -164,6 +164,10 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // GIVEN: a tilt and heading aligned filter // WHEN: sending flow data without having the flow fusion enabled // flow measurement fusion should not be intended. + const float max_flow_rate = 5.f; + const float min_ground_distance = 0.f; + const float max_ground_distance = 50.f; + _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); _sensor_simulator.startFlow(); _sensor_simulator.runSeconds(4); @@ -198,10 +202,10 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // WHEN: Stop sending flow data _sensor_simulator.stopFlow(); - _sensor_simulator.runSeconds(10); + _sensor_simulator.runSeconds(11); // THEN: EKF should not intend to fuse flow measurements - EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion()); // TODO: change to false + EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion()); // THEN: Local and global position should not be valid EXPECT_FALSE(_ekf->local_position_is_valid()); EXPECT_FALSE(_ekf->global_position_is_valid());