From 48a8992caf7a95e09a5e17235133894c072b96bd Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 30 Oct 2020 09:48:45 -0400 Subject: [PATCH] EKF: move small simple getters to header - return by const reference where possible --- EKF/airspeed_fusion.cpp | 5 -- EKF/covariance.cpp | 10 ---- EKF/ekf.cpp | 21 ++++--- EKF/ekf.h | 24 ++++---- EKF/ekf_helper.cpp | 120 ++++++++++++++++++++++++-------------- EKF/estimator_interface.h | 60 +++---------------- 6 files changed, 109 insertions(+), 131 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 84111988d5..0188c3397e 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -161,11 +161,6 @@ void Ekf::fuseAirspeed() } } -Vector2f Ekf::getWindVelocity() const -{ - return _state.wind_vel; -} - Vector2f Ekf::getWindVelocityVariance() const { return P.slice<2, 2>(22,22).diag(); diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 6645108a73..7f2491113b 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -103,16 +103,6 @@ void Ekf::initialiseCovariance() } -Vector3f Ekf::getPositionVariance() const -{ - return P.slice<3,3>(7,7).diag(); -} - -Vector3f Ekf::getVelocityVariance() const -{ - return P.slice<3,3>(4,4).diag(); -} - void Ekf::predictCovariance() { // assign intermediate state variables diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index a1c051eaa7..7a51552217 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -151,6 +151,7 @@ bool Ekf::initialiseFilter() _accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt); _gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt); _is_first_imu_sample = false; + } else { _accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt); _gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt); @@ -158,12 +159,13 @@ bool Ekf::initialiseFilter() // Sum the magnetometer measurements if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { - if(_mag_sample_delayed.time_us != 0) - { + if (_mag_sample_delayed.time_us != 0) { _mag_counter ++; + // wait for all bad initial data to be flushed if (_mag_counter <= uint8_t(_obs_buffer_length + 1)) { _mag_lpf.reset(_mag_sample_delayed.mag); + } else { _mag_lpf.update(_mag_sample_delayed.mag); } @@ -172,12 +174,13 @@ bool Ekf::initialiseFilter() // accumulate enough height measurements to be confident in the quality of the data if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { - if (_baro_sample_delayed.time_us != 0) - { + if (_baro_sample_delayed.time_us != 0) { _baro_counter ++; + // wait for all bad initial data to be flushed if (_baro_counter <= uint8_t(_obs_buffer_length + 1)) { _baro_hgt_offset = _baro_sample_delayed.hgt; + } else if (_baro_counter > (uint8_t)(_obs_buffer_length + 1)) { _baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt; } @@ -190,10 +193,11 @@ bool Ekf::initialiseFilter() if (not_enough_baro_samples_accumulated || not_enough_mag_samples_accumulated) { return false; } + // we use baro height initially and switch to GPS/range/EV finder later when it passes checks. setControlBaroHeight(); - if(!initialiseTilt()){ + if (!initialiseTilt()) { return false; } @@ -230,6 +234,7 @@ bool Ekf::initialiseTilt() { const float accel_norm = _accel_lpf.getState().norm(); const float gyro_norm = _gyro_lpf.getState().norm(); + if (accel_norm < 0.9f * CONSTANTS_ONE_G || accel_norm > 1.1f * CONSTANTS_ONE_G || gyro_norm > math::radians(15.0f)) { @@ -394,11 +399,11 @@ void Ekf::calculateOutputStates(const imuSample &imu) // this data will be at the EKF fusion time horizon // TODO: there is no guarantee that data is at delayed fusion horizon // Shouldnt we use pop_first_older_than? - const outputSample output_delayed = _output_buffer.get_oldest(); - const outputVert output_vert_delayed = _output_vert_buffer.get_oldest(); + const outputSample &output_delayed = _output_buffer.get_oldest(); + const outputVert &output_vert_delayed = _output_vert_buffer.get_oldest(); // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon - const Quatf q_error( (_state.quat_nominal.inversed() * output_delayed.quat_nominal).normalized() ); + const Quatf q_error((_state.quat_nominal.inversed() * output_delayed.quat_nominal).normalized()); // convert the quaternion delta to a delta angle const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f; diff --git a/EKF/ekf.h b/EKF/ekf.h index fc7bf3c1d7..c1272c0596 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -126,7 +126,7 @@ class Ekf final : public EstimatorInterface matrix::Vector getStateAtFusionHorizonAsVector() const override; // get the wind velocity in m/s - Vector2f getWindVelocity() const override; + const Vector2f &getWindVelocity() const { return _state.wind_vel; }; // get the wind velocity var Vector2f getWindVelocityVariance() const override; @@ -175,13 +175,13 @@ class Ekf final : public EstimatorInterface */ bool reset_imu_bias() override; - Vector3f getVelocityVariance() const override; + Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); }; - Vector3f getPositionVariance() const override; + Vector3f getPositionVariance() const { return P.slice<3, 3>(7, 7).diag(); } // return an array containing the output predictor angular, velocity and position tracking // error magnitudes (rad), (m/sec), (m) - Vector3f getOutputTrackingError() const override { return _output_tracking_error; } + const Vector3f &getOutputTrackingError() const { return _output_tracking_error; } /* Returns following IMU vibration metrics in the following array locations @@ -189,7 +189,7 @@ class Ekf final : public EstimatorInterface 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) */ - Vector3f getImuVibrationMetrics() const override { return _vibe_metrics; } + const Vector3f &getImuVibrationMetrics() const { return _vibe_metrics; } /* First argument returns GPS drift metrics in the following array locations @@ -220,10 +220,10 @@ class Ekf final : public EstimatorInterface float get_terrain_var() const { return _terrain_var; } // get the accelerometer bias in m/s**2 - Vector3f getAccelBias() const override { return _state.delta_vel_bias / _dt_ekf_avg; } + Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s - Vector3f getGyroBias() const override { return _state.delta_ang_bias / _dt_ekf_avg; } + Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get GPS check status void get_gps_check_status(uint16_t *val) override { *val = _gps_check_fail_status.value; } @@ -260,13 +260,14 @@ class Ekf final : public EstimatorInterface // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. - void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, float &hagl, float &beta) override; + void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, + float &hagl, float &beta) override; // return a bitmask integer that describes which state estimates can be used for flight control void get_ekf_soln_status(uint16_t *status) override; // return the quaternion defining the rotation from the External Vision to the EKF reference frame - matrix::Quatf getVisionAlignmentQuaternion() const override; + matrix::Quatf getVisionAlignmentQuaternion() const { return Quatf(_R_ev_to_ekf); }; // use the latest IMU data at the current time horizon. Quatf calculate_quaternion() const; @@ -471,7 +472,7 @@ class Ekf final : public EstimatorInterface gps_check_fail_status_u _gps_check_fail_status{}; // variables used to inhibit accel bias learning - bool _accel_bias_inhibit[3]{}; ///< true when the accel bias learning is being inhibited for the specified axis + bool _accel_bias_inhibit[3] {}; ///< true when the accel bias learning is being inhibited for the specified axis Vector3f _accel_vec_filt; ///< acceleration vector after application of a low pass filter (m/sec**2) float _accel_magnitude_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec) float _ang_rate_magnitude_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec) @@ -541,7 +542,8 @@ class Ekf final : public EstimatorInterface // variance : observaton variance // gate_sigma : innovation consistency check gate size (Sigma) // jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state - void updateQuaternion(const float innovation, const float variance, const float gate_sigma, const Vector4f &yaw_jacobian); + void updateQuaternion(const float innovation, const float variance, const float gate_sigma, + const Vector4f &yaw_jacobian); // fuse the yaw angle obtained from a dual antenna GPS unit void fuseGpsYaw(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 853dd1094f..697c821b71 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -63,13 +63,15 @@ void Ekf::resetVelocity() } } -void Ekf::resetVelocityToGps() { +void Ekf::resetVelocityToGps() +{ ECL_INFO_TIMESTAMPED("reset velocity to GPS"); resetVelocityTo(_gps_sample_delayed.vel); P.uncorrelateCovarianceSetVariance<3>(4, sq(_gps_sample_delayed.sacc)); } -void Ekf::resetHorizontalVelocityToOpticalFlow() { +void Ekf::resetHorizontalVelocityToOpticalFlow() +{ ECL_INFO_TIMESTAMPED("reset velocity to flow"); // constrain height above ground to be above minimum possible const float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); @@ -89,6 +91,7 @@ void Ekf::resetHorizontalVelocityToOpticalFlow() { const Vector3f vel_optflow_earth = _R_to_earth * vel_optflow_body; resetHorizontalVelocityTo(Vector2f(vel_optflow_earth)); + } else { resetHorizontalVelocityTo(Vector2f{0.f, 0.f}); } @@ -97,38 +100,44 @@ void Ekf::resetHorizontalVelocityToOpticalFlow() { P.uncorrelateCovarianceSetVariance<2>(4, sq(range) * calcOptFlowMeasVar()); } -void Ekf::resetVelocityToVision() { +void Ekf::resetVelocityToVision() +{ ECL_INFO_TIMESTAMPED("reset to vision velocity"); resetVelocityTo(getVisionVelocityInEkfFrame()); P.uncorrelateCovarianceSetVariance<3>(4, getVisionVelocityVarianceInEkfFrame()); } -void Ekf::resetHorizontalVelocityToZero() { +void Ekf::resetHorizontalVelocityToZero() +{ ECL_INFO_TIMESTAMPED("reset velocity to zero"); // Used when falling back to non-aiding mode of operation resetHorizontalVelocityTo(Vector2f{0.f, 0.f}); P.uncorrelateCovarianceSetVariance<2>(4, 25.0f); } -void Ekf::resetVelocityTo(const Vector3f &new_vel) { +void Ekf::resetVelocityTo(const Vector3f &new_vel) +{ resetHorizontalVelocityTo(Vector2f(new_vel)); resetVerticalVelocityTo(new_vel(2)); } -void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel) { +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.xy() += delta_horz_vel; } + _output_new.vel.xy() += delta_horz_vel; _state_reset_status.velNE_change = delta_horz_vel; _state_reset_status.velNE_counter++; } -void Ekf::resetVerticalVelocityTo(float new_vert_vel) { +void Ekf::resetVerticalVelocityTo(float new_vert_vel) +{ const float delta_vert_vel = new_vert_vel - _state.vel(2); _state.vel(2) = new_vert_vel; @@ -136,6 +145,7 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel) { _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_new.vert_vel += delta_vert_vel; @@ -163,6 +173,7 @@ void Ekf::resetHorizontalPosition() if (!_control_status.flags.in_air) { // we are likely starting OF for the first time so reset the horizontal position resetHorizontalPositionTo(Vector2f(0.f, 0.f)); + } else { resetHorizontalPositionTo(_last_known_posNE); } @@ -178,29 +189,35 @@ void Ekf::resetHorizontalPosition() } } -void Ekf::resetHorizontalPositionToGps() { +void Ekf::resetHorizontalPositionToGps() +{ ECL_INFO_TIMESTAMPED("reset position to GPS"); resetHorizontalPositionTo(_gps_sample_delayed.pos); P.uncorrelateCovarianceSetVariance<2>(7, sq(_gps_sample_delayed.hacc)); } -void Ekf::resetHorizontalPositionToVision() { +void Ekf::resetHorizontalPositionToVision() +{ ECL_INFO_TIMESTAMPED("reset position to ev position"); Vector3f _ev_pos = _ev_sample_delayed.pos; - if(_params.fusion_mode & MASK_ROTATE_EV){ - _ev_pos = _R_ev_to_ekf *_ev_sample_delayed.pos; + + if (_params.fusion_mode & MASK_ROTATE_EV) { + _ev_pos = _R_ev_to_ekf * _ev_sample_delayed.pos; } + resetHorizontalPositionTo(Vector2f(_ev_pos)); P.uncorrelateCovarianceSetVariance<2>(7, _ev_sample_delayed.posVar.slice<2, 1>(0, 0)); } -void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos) { +void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos) +{ const Vector2f delta_horz_pos = new_horz_pos - Vector2f(_state.pos); _state.pos.xy() = new_horz_pos; for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { _output_buffer[index].pos.xy() += delta_horz_pos; } + _output_new.pos.xy() += delta_horz_pos; _state_reset_status.posNE_change = delta_horz_pos; @@ -392,13 +409,15 @@ bool Ekf::realignYawGPS() 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) { + if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2) + && !_control_status.flags.mag_fault) { ECL_WARN_TIMESTAMPED("stopping mag use"); _control_status.flags.mag_fault = true; } // calculate new yaw estimate float yaw_new; + if (!_control_status.flags.mag_aligned_in_flight) { // This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a // forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error @@ -410,7 +429,7 @@ bool Ekf::realignYawGPS() // we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is // aligned with the wind relative GPS velocity vector yaw_new = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)), - (_gps_sample_delayed.vel(0) - _state.wind_vel(0))); + (_gps_sample_delayed.vel(0) - _state.wind_vel(0))); } else { // we don't have wind estimates, so align yaw to the GPS velocity vector @@ -419,7 +438,7 @@ bool Ekf::realignYawGPS() } // use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment - const float SpdErrorVariance = sq(_gps_sample_delayed.sacc) + P(4,4) + P(5,5); + const float SpdErrorVariance = sq(_gps_sample_delayed.sacc) + P(4, 4) + P(5, 5); const float sineYawError = math::constrain(sqrtf(SpdErrorVariance) / gpsSpeed, 0.0f, 1.0f); const float yaw_variance_new = sq(asinf(sineYawError)); @@ -472,6 +491,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool // calculate the observed yaw angle and yaw variance float yaw_new; float yaw_new_variance = 0.0f; + if (_control_status.flags.ev_yaw) { yaw_new = getEuler312Yaw(_ev_sample_delayed.quat); @@ -570,7 +590,8 @@ float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const Vector3f airspeed_body = R_to_body * airspeed_earth; - const Vector3f K_pstatic_coef(airspeed_body(0) >= 0.0f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn, + const Vector3f K_pstatic_coef(airspeed_body(0) >= 0.0f ? _params.static_pressure_coef_xp : + _params.static_pressure_coef_xn, airspeed_body(1) >= 0.0f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn, _params.static_pressure_coef_z); @@ -695,10 +716,12 @@ bool Ekf::get_gps_drift_metrics(float drift[3], bool *blocked) { memcpy(drift, _gps_drift_metrics, 3 * sizeof(float)); *blocked = !_control_status.flags.vehicle_at_rest; + if (_gps_drift_updated) { _gps_drift_updated = false; return true; } + return false; } @@ -708,27 +731,27 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) // report absolute accuracy taking into account the uncertainty in location of the origin // If not aiding, return 0 for horizontal position estimate as no estimate is available // TODO - allow for baro drift in vertical position error - float hpos_err = sqrtf(P(7,7) + P(8,8) + sq(_gps_origin_eph)); + float hpos_err = sqrtf(P(7, 7) + P(8, 8) + sq(_gps_origin_eph)); // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations if (_is_dead_reckoning && (_control_status.flags.gps)) { hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1)))); - } - else if (_is_dead_reckoning && (_control_status.flags.ev_pos)) { + + } else if (_is_dead_reckoning && (_control_status.flags.ev_pos)) { hpos_err = math::max(hpos_err, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1)))); } *ekf_eph = hpos_err; - *ekf_epv = sqrtf(P(9,9) + sq(_gps_origin_epv)); + *ekf_epv = sqrtf(P(9, 9) + sq(_gps_origin_epv)); } // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) { // TODO - allow for baro drift in vertical position error - float hpos_err = sqrtf(P(7,7) + P(8,8)); + float hpos_err = sqrtf(P(7, 7) + P(8, 8)); // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors @@ -738,13 +761,13 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) } *ekf_eph = hpos_err; - *ekf_epv = sqrtf(P(9,9)); + *ekf_epv = sqrtf(P(9, 9)); } // get the 1-sigma horizontal and vertical velocity uncertainty void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) { - float hvel_err = sqrtf(P(4,4) + P(5,5)); + float hvel_err = sqrtf(P(4, 4) + P(5, 5)); // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal velocity error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors @@ -759,19 +782,20 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) if (_control_status.flags.gps) { vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1)))); - } - else if (_control_status.flags.ev_pos) { + + } else if (_control_status.flags.ev_pos) { vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1)))); } if (_control_status.flags.ev_vel) { vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_vel_innov(0)) + sq(_ev_vel_innov(1)))); } + hvel_err = math::max(hvel_err, vel_err_conservative); } *ekf_evh = hvel_err; - *ekf_evv = sqrtf(P(6,6)); + *ekf_evv = sqrtf(P(6, 6)); } /* @@ -842,7 +866,7 @@ bool Ekf::reset_imu_bias() _last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us; // Set previous frame values - _prev_dvel_bias_var = P.slice<3,3>(13,13).diag(); + _prev_dvel_bias_var = P.slice<3, 3>(13, 13).diag(); return true; } @@ -852,17 +876,18 @@ bool Ekf::reset_imu_bias() // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. -void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, float &hagl, float &beta) +void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, + float &hagl, float &beta) { // return the integer bitmask containing the consistency check pass/fail status status = _innov_check_fail_status.value; // return the largest magnetometer innovation test ratio - mag = sqrtf(math::max(_yaw_test_ratio,_mag_test_ratio.max())); + mag = sqrtf(math::max(_yaw_test_ratio, _mag_test_ratio.max())); // return the largest velocity innovation test ratio vel = math::max(sqrtf(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1))), sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1)))); // return the largest position innovation test ratio - pos = math::max(sqrtf(_gps_pos_test_ratio(0)),sqrtf(_ev_pos_test_ratio(0))); + pos = math::max(sqrtf(_gps_pos_test_ratio(0)), sqrtf(_ev_pos_test_ratio(0))); // return the vertical position innovation test ratio hgt = sqrtf(_gps_pos_test_ratio(0)); @@ -897,7 +922,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) *status = soln_status.value; } -void Ekf::fuse(const Vector24f& K, float innovation) +void Ekf::fuse(const Vector24f &K, float innovation) { _state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation; _state.quat_nominal.normalize(); @@ -943,7 +968,8 @@ void Ekf::update_deadreckoning_status() } // report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present - _deadreckon_time_exceeded = (_time_last_aiding == 0) || isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max); + _deadreckon_time_exceeded = (_time_last_aiding == 0) + || isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max); } // calculate the variances for the rotation vector equivalent @@ -1228,6 +1254,7 @@ Vector3f Ekf::getVisionVelocityInEkfFrame() const } break; } + return vel; } @@ -1248,6 +1275,7 @@ Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const } break; } + return ev_vel_cov.diag(); } @@ -1259,12 +1287,6 @@ void Ekf::calcExtVisRotMat() _R_ev_to_ekf = Dcmf(q_error); } -// return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame -matrix::Quatf Ekf::getVisionAlignmentQuaternion() const -{ - return Quatf(_R_ev_to_ekf); -} - // Increase the yaw error variance of the quaternions // Argument is additional yaw variance in rad**2 void Ekf::increaseQuatYawErrVariance(float yaw_variance) @@ -1312,7 +1334,7 @@ void Ekf::saveMagCovData() { // save variances for the D earth axis and XYZ body axis field for (uint8_t index = 0; index <= 3; index ++) { - _saved_mag_bf_variance[index] = P(index + 18,index + 18); + _saved_mag_bf_variance[index] = P(index + 18, index + 18); } // save the NE axis covariance sub-matrix @@ -1323,8 +1345,9 @@ void Ekf::loadMagCovData() { // re-instate variances for the D earth axis and XYZ body axis field for (uint8_t index = 0; index <= 3; index ++) { - P(index + 18,index + 18) = _saved_mag_bf_variance[index]; + P(index + 18, index + 18) = _saved_mag_bf_variance[index]; } + // re-instate the NE axis covariance sub-matrix P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat; } @@ -1380,19 +1403,22 @@ void Ekf::stopGpsYawFusion() _control_status.flags.gps_yaw = false; } -void Ekf::startEvPosFusion() { +void Ekf::startEvPosFusion() +{ _control_status.flags.ev_pos = true; resetHorizontalPosition(); ECL_INFO_TIMESTAMPED("starting vision pos fusion"); } -void Ekf::startEvVelFusion() { +void Ekf::startEvVelFusion() +{ _control_status.flags.ev_vel = true; resetVelocity(); ECL_INFO_TIMESTAMPED("starting vision vel fusion"); } -void Ekf::startEvYawFusion() { +void Ekf::startEvYawFusion() +{ // reset the yaw angle to the value from the vision quaternion const float yaw = getEuler321Yaw(_ev_sample_delayed.quat); const float yaw_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)); @@ -1529,6 +1555,7 @@ bool Ekf::resetYawToEKFGSF() if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) { ECL_INFO_TIMESTAMPED("Yaw aligned using IMU and GPS"); + } else { // stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around // and cause another navigation failure @@ -1539,16 +1566,19 @@ bool Ekf::resetYawToEKFGSF() return true; } -bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) +bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], + float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) { - return yawEstimator.getLogData(yaw_composite,yaw_variance,yaw,innov_VN,innov_VE,weight); + return yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight); } void Ekf::runYawEKFGSF() { float TAS; + if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000) && _control_status.flags.fixed_wing) { TAS = _params.EKFGSF_tas_default; + } else { TAS = _airspeed_sample_delayed.true_airspeed; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index d83ebb5ff6..a190ea2b5d 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -123,24 +123,10 @@ class EstimatorInterface virtual matrix::Vector getStateAtFusionHorizonAsVector() const = 0; - virtual Vector2f getWindVelocity() const = 0; - virtual Vector2f getWindVelocityVariance() const = 0; virtual void get_true_airspeed(float *tas) = 0; - // return an array containing the output predictor angular, velocity and position tracking - // error magnitudes (rad), (m/s), (m) - virtual Vector3f getOutputTrackingError() const = 0; - - /* - Returns following IMU vibration metrics in the following array locations - 0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) - 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) - 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) - */ - virtual Vector3f getImuVibrationMetrics() const = 0; - /* First argument returns GPS drift metrics in the following array locations 0 : Horizontal position drift rate (m/s) @@ -193,7 +179,7 @@ class EstimatorInterface // return a address to the parameters struct // in order to give access to the application - parameters *getParamHandle() {return &_params;} + parameters *getParamHandle() { return &_params; } // set vehicle landed status data void set_in_air_status(bool in_air) {_control_status.flags.in_air = in_air;} @@ -285,10 +271,7 @@ class EstimatorInterface // return true if the local position estimate is valid bool local_position_is_valid(); - const matrix::Quatf getQuaternion() const { return _output_new.quat_nominal; } - - // return the quaternion defining the rotation from the EKF to the External Vision reference frame - virtual matrix::Quatf getVisionAlignmentQuaternion() const = 0; + const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; } // get the velocity of the body frame origin in local NED earth frame Vector3f getVelocity() const @@ -297,19 +280,11 @@ class EstimatorInterface return vel_earth; } - virtual Vector3f getVelocityVariance() const = 0; - // get the velocity derivative in earth frame - Vector3f getVelocityDerivative() const - { - return _vel_deriv; - } + const Vector3f &getVelocityDerivative() const { return _vel_deriv; } // get the derivative of the vertical position of the body frame origin in local NED earth frame - float getVerticalPositionDerivative() const - { - return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); - } + float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); } // get the position of the body frame origin in local earth frame Vector3f getPosition() const @@ -320,15 +295,11 @@ class EstimatorInterface return _output_new.pos - pos_offset_earth; } - virtual Vector3f getPositionVariance() const = 0; - // Get the value of magnetic declination in degrees to be saved for use at the next startup // Returns true when the declination can be saved // At the next startup, set param.mag_declination_deg to the value saved bool get_mag_decl_deg(float *val) { - *val = 0.0f; - if (_NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL)) { *val = math::degrees(_mag_declination_gps); return true; @@ -338,20 +309,11 @@ class EstimatorInterface } } - virtual Vector3f getAccelBias() const = 0; - virtual Vector3f getGyroBias() const = 0; - // get EKF mode status - void get_control_mode(uint32_t *val) - { - *val = _control_status.value; - } + void get_control_mode(uint32_t *val) { *val = _control_status.value; } // get EKF internal fault status - void get_filter_fault_status(uint16_t *val) - { - *val = _fault_status.value; - } + void get_filter_fault_status(uint16_t *val) { *val = _fault_status.value; } bool isVehicleAtRest() const { return _control_status.flags.vehicle_at_rest; } @@ -388,16 +350,10 @@ class EstimatorInterface float get_dt_imu_avg() const { return _dt_imu_avg; } // Getter for the imu sample on the delayed time horizon - imuSample get_imu_sample_delayed() - { - return _imu_sample_delayed; - } + const imuSample &get_imu_sample_delayed() { return _imu_sample_delayed; } // Getter for the baro sample on the delayed time horizon - baroSample get_baro_sample_delayed() - { - return _baro_sample_delayed; - } + const baroSample &get_baro_sample_delayed() { return _baro_sample_delayed; } void print_status();