From fa5a00d871682e9a4ce7cad292187c24fd01ed8e Mon Sep 17 00:00:00 2001 From: kritz Date: Wed, 8 Apr 2020 16:39:15 +0200 Subject: [PATCH] Refactor velocity reset (#788) * Refactor velocity reset * Add unit tests for velocity resets * Expand updates to vertical buffer to velocity resets outside of resetHeight * Improve matrix library usage * Improve naming of vertical output samples * Fix update of output_vert_new during reset * Improve naming of vertical output samples 2 --- EKF/common.h | 4 +- EKF/control.cpp | 3 + EKF/ekf.cpp | 24 ++--- EKF/ekf.h | 6 ++ EKF/ekf_helper.cpp | 102 ++++++++----------- EKF/estimator_interface.h | 2 +- test/CMakeLists.txt | 1 + test/sensor_simulator/range_finder.cpp | 8 +- test/sensor_simulator/range_finder.h | 4 +- test/sensor_simulator/sensor_simulator.cpp | 4 +- test/test_EKF_externalVision.cpp | 41 ++++++++ test/test_EKF_flow.cpp | 113 +++++++++++++++++++++ test/test_EKF_gps.cpp | 25 +++++ 13 files changed, 259 insertions(+), 78 deletions(-) create mode 100644 test/test_EKF_flow.cpp diff --git a/EKF/common.h b/EKF/common.h index c870d0e00d..b7cd23090b 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -81,8 +81,8 @@ struct outputSample { }; struct outputVert { - float vel_d; ///< D velocity calculated using alternative algorithm (m/sec) - float vel_d_integ; ///< Integral of vel_d (m) + float vert_vel; ///< Vertical velocity calculated using alternative algorithm (m/sec) + float vert_vel_integ; ///< Integral of vertical velocity (m) float dt; ///< delta time (sec) uint64_t time_us; ///< timestamp of the measurement (uSec) }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 4d3c0961ac..8e1517ffde 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -496,6 +496,9 @@ 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(); resetPosition(); diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 529af9db39..9b0cb90443 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -369,13 +369,13 @@ void Ekf::calculateOutputStates() // increment the INS velocity states by the measurement plus corrections // do the same for vertical state used by alternative correction algorithm _output_new.vel += delta_vel_earth; - _output_vert_new.vel_d += delta_vel_earth(2); + _output_vert_new.vert_vel += delta_vel_earth(2); // use trapezoidal integration to calculate the INS position states // do the same for vertical state used by alternative correction algorithm const Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (imu.delta_vel_dt * 0.5f); _output_new.pos += delta_pos_NED; - _output_vert_new.vel_d_integ += delta_pos_NED(2); + _output_vert_new.vert_vel_integ += delta_pos_NED(2); // accumulate the time for each update _output_vert_new.dt += imu.delta_vel_dt; @@ -447,21 +447,21 @@ void Ekf::calculateOutputStates() const float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f); { /* - * Calculate a correction to be applied to vel_d that casues vel_d_integ to track the EKF + * Calculate a correction to be applied to vert_vel that casues vert_vel_integ to track the EKF * down position state at the fusion time horizon using an alternative algorithm to what - * is used for the vel and pos state tracking. The algorithm applies a correction to the vel_d - * state history and propagates vel_d_integ forward in time using the corrected vel_d history. + * is used for the vel and pos state tracking. The algorithm applies a correction to the vert_vel + * state history and propagates vert_vel_integ forward in time using the corrected vert_vel history. * This provides an alternative vertical velocity output that is closer to the first derivative * of the position but does degrade tracking relative to the EKF state. */ // calculate down velocity and position tracking errors - const float vel_d_err = (_state.vel(2) - _output_vert_delayed.vel_d); - const float pos_d_err = (_state.pos(2) - _output_vert_delayed.vel_d_integ); + const float vert_vel_err = (_state.vel(2) - _output_vert_delayed.vert_vel); + const float vert_vel_integ_err = (_state.pos(2) - _output_vert_delayed.vert_vel_integ); // calculate a velocity correction that will be applied to the output state history // using a PD feedback tuned to a 5% overshoot - const float vel_d_correction = pos_d_err * pos_gain + vel_d_err * pos_gain * 1.1f; + const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * pos_gain * 1.1f; /* * Calculate corrections to be applied to vel and pos output state history. @@ -470,7 +470,7 @@ void Ekf::calculateOutputStates() */ // loop through the vertical output filter state history starting at the oldest and apply the corrections to the - // vel_d states and propagate vel_d_integ forward using the corrected vel_d + // vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel uint8_t index = _output_vert_buffer.get_oldest_index(); const uint8_t size = _output_vert_buffer.get_length(); @@ -482,13 +482,13 @@ void Ekf::calculateOutputStates() // correct the velocity if (counter == 0) { - current_state.vel_d += vel_d_correction; + current_state.vert_vel += vert_vel_correction; } - next_state.vel_d += vel_d_correction; + next_state.vert_vel += vert_vel_correction; // position is propagated forward using the corrected velocity and a trapezoidal integrator - next_state.vel_d_integ = current_state.vel_d_integ + (current_state.vel_d + next_state.vel_d) * 0.5f * next_state.dt; + next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * next_state.dt; // advance the index index = (index + 1) % size; diff --git a/EKF/ekf.h b/EKF/ekf.h index e070204fcc..cc25170f6c 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -577,6 +577,12 @@ private: // reset velocity states of the ekf bool resetVelocity(); + void resetVelocityTo(const Vector3f &vel); + + inline void resetHorizontalVelocityTo(const Vector2f &new_horz_vel); + + inline void resetVerticalVelocityTo(float new_vert_vel); + // fuse optical flow line of sight rate measurements void fuseOptFlow(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index e070286abc..7e5d5ca8e8 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -49,16 +49,11 @@ // gps measurement then use for velocity initialisation bool Ekf::resetVelocity() { - // used to calculate the velocity change due to the reset - const Vector3f vel_before_reset = _state.vel; - - // reset EKF states if (_control_status.flags.gps && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) { ECL_INFO_TIMESTAMPED("reset velocity to GPS"); // this reset is only called if we have new gps data at the fusion time horizon - _state.vel = _gps_sample_delayed.vel; + resetVelocityTo(_gps_sample_delayed.vel); - // use GPS accuracy to reset variances P.uncorrelateCovarianceSetVariance<3>(4, sq(_gps_sample_delayed.sacc)); } else if (_control_status.flags.opt_flow) { @@ -78,16 +73,11 @@ bool Ekf::resetVelocity() vel_optflow_body(2) = 0.0f; // rotate from body to earth frame - Vector3f vel_optflow_earth; - vel_optflow_earth = _R_to_earth * vel_optflow_body; - - // take x and Y components - _state.vel(0) = vel_optflow_earth(0); - _state.vel(1) = vel_optflow_earth(1); + const Vector3f vel_optflow_earth = _R_to_earth * vel_optflow_body; + resetHorizontalVelocityTo(Vector2f(vel_optflow_earth)); } else { - _state.vel(0) = 0.0f; - _state.vel(1) = 0.0f; + resetHorizontalVelocityTo(Vector2f{0.f, 0.f}); } // reset the horizontal velocity variance using the optical flow noise variance @@ -99,35 +89,53 @@ bool Ekf::resetVelocity() if(_params.fusion_mode & MASK_ROTATE_EV){ _ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel; } - _state.vel = _ev_vel; + resetVelocityTo(_ev_vel); P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velVar); + } else { ECL_INFO_TIMESTAMPED("reset velocity to zero"); // Used when falling back to non-aiding mode of operation - _state.vel(0) = 0.0f; - _state.vel(1) = 0.0f; + resetHorizontalVelocityTo(Vector2f{0.f, 0.f}); P.uncorrelateCovarianceSetVariance<2>(4, 25.0f); } - // calculate the change in velocity and apply to the output predictor state history - const Vector3f velocity_change = _state.vel - vel_before_reset; + return true; +} + +void Ekf::resetVelocityTo(const Vector3f &new_vel) { + resetHorizontalVelocityTo(Vector2f(new_vel)); + resetVerticalVelocityTo(new_vel(2)); +} + +void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel) { + const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel); + _state.vel.xy() = new_horz_vel; for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { - _output_buffer[index].vel += velocity_change; + _output_buffer[index].vel(0) += delta_horz_vel(0); + _output_buffer[index].vel(1) += delta_horz_vel(1); } + _output_new.vel(0) += delta_horz_vel(0); + _output_new.vel(1) += delta_horz_vel(1); - // apply the change in velocity to our newest velocity estimate - // which was already taken out from the output buffer - _output_new.vel += velocity_change; - - // capture the reset event - _state_reset_status.velNE_change(0) = velocity_change(0); - _state_reset_status.velNE_change(1) = velocity_change(1); - _state_reset_status.velD_change = velocity_change(2); + _state_reset_status.velNE_change = delta_horz_vel; _state_reset_status.velNE_counter++; - _state_reset_status.velD_counter++; +} - return true; +void Ekf::resetVerticalVelocityTo(float new_vert_vel) { + const float delta_vert_vel = new_vert_vel - _state.vel(2); + _state.vel(2) = new_vert_vel; + + for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { + _output_buffer[index].vel(2) += delta_vert_vel; + _output_vert_buffer[index].vert_vel += delta_vert_vel; + } + _output_new.vel(2) += delta_vert_vel; + _output_vert_delayed.vert_vel = new_vert_vel; + _output_vert_new.vert_vel += delta_vert_vel; + + _state_reset_status.velD_change = delta_vert_vel; + _state_reset_status.velD_counter++; } // Reset position states. If we have a recent and valid @@ -222,8 +230,6 @@ void Ekf::resetHeight() // store the current vertical position and velocity for reference so we can calculate and publish the reset amount float old_vert_pos = _state.pos(2); bool vert_pos_reset = false; - float old_vert_vel = _state.vel(2); - bool vert_vel_reset = false; // reset the vertical position if (_control_status.flags.rng_hgt) { @@ -297,66 +303,44 @@ void Ekf::resetHeight() // reset the vertical velocity state if (_control_status.flags.gps && !_gps_hgt_intermittent) { // If we are using GPS, then use it to reset the vertical velocity - _state.vel(2) = gps_newest.vel(2); + resetVerticalVelocityTo(gps_newest.vel(2)); // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_newest.sacc)); } else { // we don't know what the vertical velocity is, so set it to zero - _state.vel(2) = 0.0f; + resetVerticalVelocityTo(0.0f); // Set the variance to a value large enough to allow the state to converge quickly // that does not destabilise the filter P.uncorrelateCovarianceSetVariance<1>(6, 10.0f); - } - vert_vel_reset = true; - // store the reset amount and time to be published if (vert_pos_reset) { _state_reset_status.posD_change = _state.pos(2) - old_vert_pos; _state_reset_status.posD_counter++; } - if (vert_vel_reset) { - _state_reset_status.velD_change = _state.vel(2) - old_vert_vel; - _state_reset_status.velD_counter++; - } - // apply the change in height / height rate to our newest height / height rate estimate // which have already been taken out from the output buffer if (vert_pos_reset) { _output_new.pos(2) += _state_reset_status.posD_change; } - if (vert_vel_reset) { - _output_new.vel(2) += _state_reset_status.velD_change; - } - // add the reset amount to the output observer buffered data for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { if (vert_pos_reset) { _output_buffer[i].pos(2) += _state_reset_status.posD_change; - _output_vert_buffer[i].vel_d_integ += _state_reset_status.posD_change; - } - - if (vert_vel_reset) { - _output_buffer[i].vel(2) += _state_reset_status.velD_change; - _output_vert_buffer[i].vel_d += _state_reset_status.velD_change; + _output_vert_buffer[i].vert_vel_integ += _state_reset_status.posD_change; } } // add the reset amount to the output observer vertical position state if (vert_pos_reset) { - _output_vert_delayed.vel_d_integ = _state.pos(2); - _output_vert_new.vel_d_integ = _state.pos(2); - } - - if (vert_vel_reset) { - _output_vert_delayed.vel_d = _state.vel(2); - _output_vert_new.vel_d = _state.vel(2); + _output_vert_delayed.vert_vel_integ = _state.pos(2); + _output_vert_new.vert_vel_integ = _state.pos(2); } } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 09c666e1ff..d379781485 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -302,7 +302,7 @@ public: // get the derivative of the vertical position of the body frame origin in local NED earth frame float getVerticalPositionDerivative() const { - return _output_vert_new.vel_d - _vel_imu_rel_body_ned(2); + return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); } // get the position of the body frame origin in local earth frame diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 9d5b4bc2ca..54b3c01e3d 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -48,6 +48,7 @@ set(SRCS test_EKF_externalVision.cpp test_EKF_airspeed.cpp test_EKF_withReplayData.cpp + test_EKF_flow.cpp test_SensorRangeFinder.cpp ) add_executable(ECL_GTESTS ${SRCS}) diff --git a/test/sensor_simulator/range_finder.cpp b/test/sensor_simulator/range_finder.cpp index 79f596546b..489bdd3304 100644 --- a/test/sensor_simulator/range_finder.cpp +++ b/test/sensor_simulator/range_finder.cpp @@ -17,7 +17,7 @@ void RangeFinder::send(uint64_t time) { _range_sample.time_us = time; _ekf->setRangeData(_range_sample); - _ekf->set_rangefinder_limits(0.2f, 20.f); // This usually comes from the driver + _ekf->set_rangefinder_limits(_min_distance, _max_distance); } void RangeFinder::setData(float range_data_meters, int8_t range_quality) @@ -26,5 +26,11 @@ void RangeFinder::setData(float range_data_meters, int8_t range_quality) _range_sample.quality = range_quality; } +void RangeFinder::setLimits(float min_distance_m, float max_distance_m) +{ + _min_distance = min_distance_m; + _max_distance = max_distance_m; +} + } // namespace sensor } // namespace sensor_simulator diff --git a/test/sensor_simulator/range_finder.h b/test/sensor_simulator/range_finder.h index 3b034d4820..ab126311c8 100644 --- a/test/sensor_simulator/range_finder.h +++ b/test/sensor_simulator/range_finder.h @@ -51,12 +51,14 @@ public: ~RangeFinder(); void setData(float range_data, int8_t range_quality); + void setLimits(float min_distance_m, float max_distance_m); private: rangeSample _range_sample {}; + float _min_distance {0.2f}; + float _max_distance {20.0f}; void send(uint64_t time) override; - }; } // namespace sensor diff --git a/test/sensor_simulator/sensor_simulator.cpp b/test/sensor_simulator/sensor_simulator.cpp index 5399588c73..ed7b8172a1 100644 --- a/test/sensor_simulator/sensor_simulator.cpp +++ b/test/sensor_simulator/sensor_simulator.cpp @@ -104,7 +104,7 @@ void SensorSimulator::loadSensorDataFromFile(std::string file_name) _has_replay_data = true; } -void SensorSimulator::setSensorDataToDefault() +void SensorSimulator::setSensorRateToDefault() { _imu.setRateHz(250); _mag.setRateHz(80); @@ -115,7 +115,7 @@ void SensorSimulator::setSensorDataToDefault() _vio.setRateHz(30); _airspeed.setRateHz(100); } -void SensorSimulator::setSensorRateToDefault() +void SensorSimulator::setSensorDataToDefault() { _imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G}, Vector3f{0.0f,0.0f,0.0f}); diff --git a/test/test_EKF_externalVision.cpp b/test/test_EKF_externalVision.cpp index e520b2de22..c6b82b763d 100644 --- a/test/test_EKF_externalVision.cpp +++ b/test/test_EKF_externalVision.cpp @@ -101,6 +101,47 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic) EXPECT_FALSE(_ekf->global_position_is_valid()); } +TEST_F(EkfExternalVisionTest, visionVelocityReset) +{ + const Vector3f simulated_velocity(0.3f, -1.0f, 0.4f); + + _sensor_simulator._vio.setVelocity(simulated_velocity); + _ekf_wrapper.enableExternalVisionVelocityFusion(); + _sensor_simulator.startExternalVision(); + _sensor_simulator.runMicroseconds(6e5); + + // THEN: a reset to Vision velocity should be done + const Vector3f estimated_velocity = _ekf->getVelocity(); + EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity, 1e-5f)); +} + +TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment) +{ + // GIVEN: Drone is pointing north, and we use mag (ROTATE_EV) + // Heading of drone in EKF frame is 0° + + // WHEN: Vision frame is rotate +90°. The reported heading is -90° + Quatf vision_to_ekf(Eulerf(0.0f,0.0f,math::radians(-90.0f))); + _sensor_simulator._vio.setOrientation(vision_to_ekf.inversed()); + _ekf_wrapper.enableExternalVisionAlignment(); + + const Vector3f simulated_velocity_in_vision_frame(0.3f, -1.0f, 0.4f); + const Vector3f simulated_velocity_in_ekf_frame = + Dcmf(vision_to_ekf) * simulated_velocity_in_vision_frame; + _sensor_simulator._vio.setVelocity(simulated_velocity_in_vision_frame); + _ekf_wrapper.enableExternalVisionVelocityFusion(); + _sensor_simulator.startExternalVision(); + _sensor_simulator.runMicroseconds(6e5); + + // THEN: a reset to Vision velocity should be done + const Vector3f estimated_velocity_in_ekf_frame = _ekf->getVelocity(); + EXPECT_TRUE(isEqual(estimated_velocity_in_ekf_frame, simulated_velocity_in_ekf_frame, 1e-5f)); + // And: the frame offset should be estimated correctly + Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion(); + EXPECT_TRUE(matrix::isEqual(vision_to_ekf.canonical(), + estimatedExternalVisionFrameOffset.canonical())); +} + TEST_F(EkfExternalVisionTest, visionVarianceCheck) { const Vector3f velVar_init = _ekf->getVelocityVariance(); diff --git a/test/test_EKF_flow.cpp b/test/test_EKF_flow.cpp new file mode 100644 index 0000000000..0e9b856dba --- /dev/null +++ b/test/test_EKF_flow.cpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2019 ECL Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Test the gps fusion + * @author Kamil Ritz + */ + +#include +#include "EKF/ekf.h" +#include "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" + + +class EkfFlowTest : public ::testing::Test { + public: + + EkfFlowTest(): ::testing::Test(), + _ekf{std::make_shared()}, + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf) {}; + + std::shared_ptr _ekf; + SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + + // Setup the Ekf with synthetic measurements + void SetUp() override + { + _ekf->init(0); + _sensor_simulator.runSeconds(2); + } + + // Use this method to clean up any memory, network etc. after each test + void TearDown() override + { + } +}; + +TEST_F(EkfFlowTest, resetToFlowVelocityInAir) +{ + // 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(1.5f); + + const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); + EXPECT_FLOAT_EQ(estimated_distance_to_ground, simulated_distance_to_ground); + + // WHEN: start fusing flow data + const 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(); + _sensor_simulator.runSeconds(0.2); + + // THEN: estimated velocity should match simulated velocity + const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity()); + EXPECT_FALSE(isEqual(estimated_horz_velocity, simulated_horz_velocity)); // TODO: This needs to change +} + +TEST_F(EkfFlowTest, resetToFlowVelocityOnGround) +{ + // WHEN: being on ground + const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); + EXPECT_LT(estimated_distance_to_ground, 0.3f); + + // WHEN: start fusing flow data + _ekf_wrapper.enableFlowFusion(); + _sensor_simulator.startFlow(); + _sensor_simulator.runSeconds(0.6); + + // 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))); +} diff --git a/test/test_EKF_gps.cpp b/test/test_EKF_gps.cpp index 3675e3d258..49b1c56f3e 100644 --- a/test/test_EKF_gps.cpp +++ b/test/test_EKF_gps.cpp @@ -83,3 +83,28 @@ TEST_F(EkfGpsTest, gpsTimeout) // TODO: this is not happening as expected EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); } + +TEST_F(EkfGpsTest, resetToGpsVelocity) +{ + // GIVEN:EKF that fuses GPS + // and has gps checks already passed + + // WHEN: stopping GPS fusion + _sensor_simulator.stopGps(); + _sensor_simulator.runSeconds(11); + + // AND: simulate constant velocity gps samples for short time + _sensor_simulator.startGps(); + const Vector3f simulated_velocity(0.5f, 1.0f, -0.3f); + _sensor_simulator._gps.setVelocity(simulated_velocity); + const uint64_t dt_us = 1e5; + _sensor_simulator._gps.stepHorizontalPositionByMeters( + Vector2f(simulated_velocity) * dt_us * 1e-6); + _sensor_simulator._gps.stepHeightByMeters( + simulated_velocity(2) * dt_us * 1e-6); + _sensor_simulator.runMicroseconds(dt_us); + + // THEN: a reset to GPS velocity should be done + const Vector3f estimated_velocity = _ekf->getVelocity(); + EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity, 1e-2f)); +}