diff --git a/EKF/common.h b/EKF/common.h index b7cd23090b..f085be1085 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -55,6 +55,8 @@ using matrix::Vector2f; using matrix::Vector3f; using matrix::wrap_pi; +enum velocity_frame_t {LOCAL_FRAME_FRD, BODY_FRAME_FRD}; + struct gps_message { uint64_t time_usec; int32_t lat; ///< Latitude in 1E-7 degrees @@ -139,11 +141,12 @@ struct flowSample { struct extVisionSample { Vector3f pos; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis - Vector3f vel; ///< XYZ velocity in external vision's local reference frame (m/sec) - Z must be aligned with down axis + Vector3f vel; ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis Quatf quat; ///< quaternion defining rotation from body to earth frame Vector3f posVar; ///< XYZ position variances (m**2) - Vector3f velVar; ///< XYZ velocity variances ((m/sec)**2) + Matrix3f velCov; ///< XYZ velocity covariances ((m/sec)**2) float angVar; ///< angular heading variance (rad**2) + velocity_frame_t vel_frame = BODY_FRAME_FRD; uint64_t time_us; ///< timestamp of the measurement (uSec) }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 4fd98508bd..641b906275 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -202,14 +202,14 @@ void Ekf::controlExternalVisionFusion() if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) { _control_status.flags.ev_pos = true; resetPosition(); - ECL_INFO_TIMESTAMPED("commencing external vision position fusion"); + ECL_INFO_TIMESTAMPED("starting vision pos fusion"); } // turn on use of external vision measurements for velocity if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) { _control_status.flags.ev_vel = true; resetVelocity(); - ECL_INFO_TIMESTAMPED("commencing external vision velocity fusion"); + ECL_INFO_TIMESTAMPED("starting vision vel fusion"); } } } @@ -235,7 +235,7 @@ void Ekf::controlExternalVisionFusion() stopMagHdgFusion(); stopMag3DFusion(); - ECL_INFO_TIMESTAMPED("commencing external vision yaw fusion"); + ECL_INFO_TIMESTAMPED("starting vision yaw fusion"); } } @@ -324,22 +324,7 @@ void Ekf::controlExternalVisionFusion() Vector3f ev_vel_obs_var; Vector2f ev_vel_innov_gates; - Vector3f vel_aligned{_ev_sample_delayed.vel}; - Matrix3f ev_vel_var = matrix::diag(_ev_sample_delayed.velVar); - - // rotate measurement into correct earth frame if required - if (_params.fusion_mode & MASK_ROTATE_EV) { - vel_aligned = _R_ev_to_ekf * _ev_sample_delayed.vel; - ev_vel_var = _R_ev_to_ekf * ev_vel_var * _R_ev_to_ekf.transpose(); - } - - // correct velocity for offset relative to IMU - const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; - const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; - const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - vel_aligned -= vel_offset_earth; - - _ev_vel_innov = _state.vel - vel_aligned; + _ev_vel_innov = _state.vel - getVisionVelocityInEkfFrame(); // check if we have been deadreckoning too long if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) { @@ -350,7 +335,7 @@ void Ekf::controlExternalVisionFusion() } } - ev_vel_obs_var = matrix::max(ev_vel_var.diag(), sq(0.01f)); + ev_vel_obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f)); ev_vel_innov_gates.setAll(fmaxf(_params.ev_vel_innov_gate, 1.0f)); @@ -368,7 +353,7 @@ void Ekf::controlExternalVisionFusion() // Turn off EV fusion mode if no data has been received stopEvFusion(); - ECL_INFO_TIMESTAMPED("External Vision Data Stopped"); + ECL_INFO_TIMESTAMPED("vision data stopped"); } } diff --git a/EKF/ekf.h b/EKF/ekf.h index ed86aa1768..f98853ec88 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -645,6 +645,10 @@ private: // update the rotation matrix which transforms EV navigation frame measurements into NED void calcExtVisRotMat(); + Vector3f getVisionVelocityInEkfFrame(); + + Vector3f getVisionVelocityVarianceInEkfFrame(); + // limit the diagonal of the covariance matrix // force symmetry when the argument is true void fixCovarianceErrors(bool force_symmetry); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 212fc8bcd1..942549c0b8 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -106,8 +106,8 @@ void Ekf::resetVelocityToVision() { if(_params.fusion_mode & MASK_ROTATE_EV){ _ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel; } - resetVelocityTo(_ev_vel); - P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velVar); + resetVelocityTo(getVisionVelocityInEkfFrame()); + P.uncorrelateCovarianceSetVariance<3>(4, getVisionVelocityVarianceInEkfFrame()); } void Ekf::resetHorizontalVelocityToZero() { @@ -310,13 +310,15 @@ void Ekf::resetHeight() P.uncorrelateCovarianceSetVariance<1>(6, 10.0f); } + // store the reset amount and time to be published if (vert_pos_reset) { - // store the reset amount and time to be published _state_reset_status.posD_change = _state.pos(2) - old_vert_pos; _state_reset_status.posD_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 + // 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; } @@ -392,11 +394,11 @@ bool Ekf::realignYawGPS() // correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned if (badMagYaw || !_control_status.flags.yaw_align) { - ECL_WARN_TIMESTAMPED("bad yaw corrected using GPS course"); + ECL_WARN_TIMESTAMPED("bad yaw, using GPS course"); // declare the magnetometer as failed if a bad yaw has occurred more than once if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) { - ECL_WARN_TIMESTAMPED("stopping magnetometer use"); + ECL_WARN_TIMESTAMPED("stopping mag use"); _control_status.flags.mag_fault = true; } @@ -1504,6 +1506,51 @@ void Ekf::updateBaroHgtOffset() } } +Vector3f Ekf::getVisionVelocityInEkfFrame() +{ + Vector3f vel; + // correct velocity for offset relative to IMU + const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; + const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; + + // rotate measurement into correct earth frame if required + switch(_ev_sample_delayed.vel_frame) { + case BODY_FRAME_FRD: + vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body); + break; + case LOCAL_FRAME_FRD: + const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + if (_params.fusion_mode & MASK_ROTATE_EV) + { + vel = _R_ev_to_ekf *_ev_sample_delayed.vel - vel_offset_earth; + } else { + vel = _ev_sample_delayed.vel - vel_offset_earth; + } + break; + } + return vel; +} + +Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() +{ + Matrix3f ev_vel_cov = _ev_sample_delayed.velCov; + + // rotate measurement into correct earth frame if required + switch(_ev_sample_delayed.vel_frame) { + case BODY_FRAME_FRD: + ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose(); + break; + + case LOCAL_FRAME_FRD: + if(_params.fusion_mode & MASK_ROTATE_EV) + { + ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose(); + } + break; + } + return ev_vel_cov.diag(); +} + // update the rotation matrix which rotates EV measurements into the EKF's navigation frame void Ekf::calcExtVisRotMat() { @@ -1755,7 +1802,7 @@ bool Ekf::resetYawToEKFGSF() // stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around // and cause another navigation failure _control_status.flags.mag_fault = true; - ECL_INFO_TIMESTAMPED("Emergency yaw reset - magnetometer use stopped"); + ECL_INFO_TIMESTAMPED("Emergency yaw reset - mag use stopped"); } return true; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 0c8b0ba4d0..3739c3c104 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -99,11 +99,9 @@ bool Ekf::collect_gps(const gps_message &gps) // if the user has selected GPS as the primary height source, switch across to using it if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) { - ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set, using GPS height)"); startGpsHgtFusion(); - } else { - ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set)"); } + ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set)"); } // start collecting GPS if there is a 3D fix and the NED origin has been set diff --git a/test/sensor_simulator/vio.cpp b/test/sensor_simulator/vio.cpp index c32bca540c..587ce51c2c 100644 --- a/test/sensor_simulator/vio.cpp +++ b/test/sensor_simulator/vio.cpp @@ -26,7 +26,12 @@ void Vio::setData(const extVisionSample& vio_data) void Vio::setVelocityVariance(const Vector3f& velVar) { - _vio_data.velVar = velVar; + setVelocityCovariance(matrix::diag(velVar)); +} + +void Vio::setVelocityCovariance(const Matrix3f& velCov) +{ + _vio_data.velCov = velCov; } void Vio::setPositionVariance(const Vector3f& posVar) @@ -54,6 +59,17 @@ void Vio::setOrientation(const Quatf& quat) _vio_data.quat = quat; } +void Vio::setVelocityFrameToBody() +{ + _vio_data.vel_frame = BODY_FRAME_FRD; +} + +void Vio::setVelocityFrameToLocal() +{ + _vio_data.vel_frame = LOCAL_FRAME_FRD; +} + + extVisionSample Vio::dataAtRest() { extVisionSample vio_data; @@ -61,8 +77,9 @@ extVisionSample Vio::dataAtRest() vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};; vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f}; vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f}; - vio_data.velVar = Vector3f{0.1f, 0.1f, 0.1f}; + vio_data.velCov = matrix::eye() * 0.1f; vio_data.angVar = 0.05f; + vio_data.vel_frame = LOCAL_FRAME_FRD; return vio_data; } diff --git a/test/sensor_simulator/vio.h b/test/sensor_simulator/vio.h index 5b873b508a..bcae46d4eb 100644 --- a/test/sensor_simulator/vio.h +++ b/test/sensor_simulator/vio.h @@ -52,11 +52,14 @@ public: void setData(const extVisionSample& vio_data); void setVelocityVariance(const Vector3f& velVar); + void setVelocityCovariance(const Matrix3f& velCov); void setPositionVariance(const Vector3f& posVar); void setAngularVariance(float angVar); void setVelocity(const Vector3f& vel); void setPosition(const Vector3f& pos); void setOrientation(const Quatf& quat); + void setVelocityFrameToLocal(); + void setVelocityFrameToBody(); extVisionSample dataAtRest(); diff --git a/test/test_EKF_externalVision.cpp b/test/test_EKF_externalVision.cpp index 69b11a72b0..f358a7c839 100644 --- a/test/test_EKF_externalVision.cpp +++ b/test/test_EKF_externalVision.cpp @@ -59,7 +59,6 @@ class EkfExternalVisionTest : public ::testing::Test { void SetUp() override { _ekf->init(0); - _sensor_simulator.runSeconds(3); } // Use this method to clean up any memory, network etc. after each test @@ -70,6 +69,7 @@ class EkfExternalVisionTest : public ::testing::Test { TEST_F(EkfExternalVisionTest, checkVisionFusionLogic) { + _sensor_simulator.runSeconds(3); _ekf_wrapper.enableExternalVisionPositionFusion(); _sensor_simulator.startExternalVision(); _sensor_simulator.runSeconds(2); @@ -104,6 +104,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic) TEST_F(EkfExternalVisionTest, visionVelocityReset) { + _sensor_simulator.runSeconds(3); ResetLoggingChecker reset_logging_checker(_ekf); reset_logging_checker.capturePreResetState(); @@ -129,6 +130,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityReset) TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment) { + _sensor_simulator.runSeconds(3); ResetLoggingChecker reset_logging_checker(_ekf); reset_logging_checker.capturePreResetState(); // GIVEN: Drone is pointing north, and we use mag (ROTATE_EV) @@ -164,6 +166,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment) TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset) { + _sensor_simulator.runSeconds(3); const Vector3f simulated_position(8.3f, -1.0f, 0.0f); _sensor_simulator._vio.setPosition(simulated_position); @@ -178,6 +181,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset) TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment) { + _sensor_simulator.runSeconds(3); // GIVEN: Drone is pointing north, and we use mag (ROTATE_EV) // Heading of drone in EKF frame is 0° @@ -202,6 +206,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment) TEST_F(EkfExternalVisionTest, visionVarianceCheck) { + _sensor_simulator.runSeconds(3); const Vector3f velVar_init = _ekf->getVelocityVariance(); EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001); @@ -216,6 +221,7 @@ TEST_F(EkfExternalVisionTest, visionVarianceCheck) TEST_F(EkfExternalVisionTest, visionAlignment) { + _sensor_simulator.runSeconds(3); // GIVEN: Drone is pointing north, and we use mag (ROTATE_EV) // Heading of drone in EKF frame is 0° @@ -244,3 +250,73 @@ TEST_F(EkfExternalVisionTest, visionAlignment) EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(), estimatedExternalVisionFrameOffset.canonical())); } + +TEST_F(EkfExternalVisionTest, velocityFrameBody) +{ + // GIVEN: Drone is turned 90 degrees + const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f))); + _sensor_simulator.simulateOrientation(quat_sim); + _sensor_simulator.runSeconds(3); + + // Without any measurement x and y velocity variance are close + const Vector3f velVar_init = _ekf->getVelocityVariance(); + EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001); + + // WHEN: measurement is given in BODY-FRAME and + // x variance is bigger than y variance + _sensor_simulator._vio.setVelocityFrameToBody(); + float vel_cov_data [9] = {2.0f, 0.0f, 0.0f, + 0.0f, 0.01f, 0.0f, + 0.0f, 0.0f, 0.01f}; + const Matrix3f vel_cov_body(vel_cov_data); + const Vector3f vel_body(1.0f, 0.0f, 0.0f); + _sensor_simulator._vio.setVelocityCovariance(vel_cov_body); + _sensor_simulator._vio.setVelocity(vel_body); + _ekf_wrapper.enableExternalVisionVelocityFusion(); + _sensor_simulator.startExternalVision(); + _sensor_simulator.runSeconds(4); + + // THEN: As the drone is turned 90 degrees, velocity variance + // along local y axis is expected to be bigger + const Vector3f velVar_new = _ekf->getVelocityVariance(); + EXPECT_NEAR(velVar_new(1) / velVar_new(0), 80.f, 10.f); + + const Vector3f vel_earth_est = _ekf->getVelocity(); + EXPECT_NEAR(vel_earth_est(0), 0.0f, 0.1f); + EXPECT_NEAR(vel_earth_est(1), 1.0f, 0.1f); +} + +TEST_F(EkfExternalVisionTest, velocityFrameLocal) +{ + // GIVEN: Drone is turned 90 degrees + const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f))); + _sensor_simulator.simulateOrientation(quat_sim); + _sensor_simulator.runSeconds(3); + + // Without any measurement x and y velocity variance are close + const Vector3f velVar_init = _ekf->getVelocityVariance(); + EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001); + + // WHEN: measurement is given in LOCAL-FRAME and + // x variance is bigger than y variance + _sensor_simulator._vio.setVelocityFrameToLocal(); + float vel_cov_data [9] = {2.0f, 0.0f, 0.0f, + 0.0f, 0.01f, 0.0f, + 0.0f, 0.0f, 0.01f}; + const Matrix3f vel_cov_earth(vel_cov_data); + const Vector3f vel_earth(1.0f, 0.0f, 0.0f); + _sensor_simulator._vio.setVelocityCovariance(vel_cov_earth); + _sensor_simulator._vio.setVelocity(vel_earth); + _ekf_wrapper.enableExternalVisionVelocityFusion(); + _sensor_simulator.startExternalVision(); + _sensor_simulator.runSeconds(4); + + // THEN: Independently on drones heading, velocity variance + // along local x axis is expected to be bigger + const Vector3f velVar_new = _ekf->getVelocityVariance(); + EXPECT_NEAR(velVar_new(0) / velVar_new(1), 80.f, 10.f); + + const Vector3f vel_earth_est = _ekf->getVelocity(); + EXPECT_NEAR(vel_earth_est(0), 1.0f, 0.1f); + EXPECT_NEAR(vel_earth_est(1), 0.0f, 0.1f); +}