From 95300d5637a18fb84ceb135be4220c0bb06e117b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 18 Jan 2023 10:59:34 -0500 Subject: [PATCH] ekf2: refactor output predictor to class - refactor all EKF backend output predictor pieces into new OutputPredictor class - output states are now calculated immediately with new high rate IMU rather than after EKF update - IMU delayed sample is passed as around as control data to avoid storing an extra copy and make the requirement clear --- src/modules/ekf2/CMakeLists.txt | 1 + src/modules/ekf2/EKF/CMakeLists.txt | 1 + src/modules/ekf2/EKF/RingBuffer.h | 5 + src/modules/ekf2/EKF/airspeed_fusion.cpp | 4 +- src/modules/ekf2/EKF/baro_height_control.cpp | 6 +- src/modules/ekf2/EKF/common.h | 18 - src/modules/ekf2/EKF/control.cpp | 36 +- src/modules/ekf2/EKF/covariance.cpp | 18 +- src/modules/ekf2/EKF/ekf.cpp | 306 ++------------ src/modules/ekf2/EKF/ekf.h | 80 ++-- src/modules/ekf2/EKF/ekf_helper.cpp | 87 +--- src/modules/ekf2/EKF/estimator_interface.cpp | 44 +-- src/modules/ekf2/EKF/estimator_interface.h | 59 +-- src/modules/ekf2/EKF/ev_control.cpp | 2 +- src/modules/ekf2/EKF/ev_height_control.cpp | 6 +- src/modules/ekf2/EKF/ev_pos_control.cpp | 6 +- src/modules/ekf2/EKF/ev_vel_control.cpp | 6 +- src/modules/ekf2/EKF/ev_yaw_control.cpp | 8 +- src/modules/ekf2/EKF/fake_height_control.cpp | 4 +- src/modules/ekf2/EKF/fake_pos_control.cpp | 4 +- src/modules/ekf2/EKF/gnss_height_control.cpp | 4 +- src/modules/ekf2/EKF/gps_checks.cpp | 6 +- src/modules/ekf2/EKF/gps_control.cpp | 8 +- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 6 +- src/modules/ekf2/EKF/height_control.cpp | 17 +- src/modules/ekf2/EKF/mag_control.cpp | 20 +- src/modules/ekf2/EKF/mag_fusion.cpp | 6 +- src/modules/ekf2/EKF/optflow_fusion.cpp | 2 +- src/modules/ekf2/EKF/optical_flow_control.cpp | 30 +- src/modules/ekf2/EKF/output_predictor.cpp | 374 ++++++++++++++++++ src/modules/ekf2/EKF/output_predictor.h | 193 +++++++++ src/modules/ekf2/EKF/range_height_control.cpp | 4 +- src/modules/ekf2/EKF/sideslip_fusion.cpp | 4 +- src/modules/ekf2/EKF/terrain_estimator.cpp | 22 +- src/modules/ekf2/EKF/vel_pos_fusion.cpp | 20 +- src/modules/ekf2/EKF/zero_velocity_update.cpp | 2 +- src/modules/ekf2/EKF2.cpp | 46 ++- src/modules/ekf2/EKF2.hpp | 6 +- .../ekf2/test/sensor_simulator/ekf_logger.cpp | 2 +- 39 files changed, 855 insertions(+), 618 deletions(-) create mode 100644 src/modules/ekf2/EKF/output_predictor.cpp create mode 100644 src/modules/ekf2/EKF/output_predictor.h diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 19ce16f50d..e8e6d16f31 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -108,6 +108,7 @@ px4_add_module( EKF/mag_fusion.cpp EKF/optical_flow_control.cpp EKF/optflow_fusion.cpp + EKF/output_predictor.cpp EKF/range_finder_consistency_check.cpp EKF/range_height_control.cpp EKF/sensor_range_finder.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index e6d668dc1e..7ee0ed1d04 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -59,6 +59,7 @@ add_library(ecl_EKF mag_fusion.cpp optical_flow_control.cpp optflow_fusion.cpp + output_predictor.cpp range_finder_consistency_check.cpp range_height_control.cpp sensor_range_finder.cpp diff --git a/src/modules/ekf2/EKF/RingBuffer.h b/src/modules/ekf2/EKF/RingBuffer.h index 76811e3bea..e11a1f95c2 100644 --- a/src/modules/ekf2/EKF/RingBuffer.h +++ b/src/modules/ekf2/EKF/RingBuffer.h @@ -37,6 +37,9 @@ * Template RingBuffer. */ +#ifndef EKF_RINGBUFFER_H +#define EKF_RINGBUFFER_H + #include #include #include @@ -177,3 +180,5 @@ private: bool _first_write{true}; }; + +#endif // !EKF_RINGBUFFER_H diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 06d4ce8fb0..0f8534dbab 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -127,7 +127,7 @@ void Ekf::fuseAirspeed(estimator_aid_source1d_s &airspeed) _fault_status.flags.bad_airspeed = !is_fused; if (is_fused) { - airspeed.time_last_fuse = _imu_sample_delayed.time_us; + airspeed.time_last_fuse = _time_delayed_us; } } @@ -159,7 +159,7 @@ void Ekf::resetWindUsingAirspeed() resetWindCovarianceUsingAirspeed(); - _aid_src_airspeed.time_last_fuse = _imu_sample_delayed.time_us; + _aid_src_airspeed.time_last_fuse = _time_delayed_us; } void Ekf::resetWindToZero() diff --git a/src/modules/ekf2/EKF/baro_height_control.cpp b/src/modules/ekf2/EKF/baro_height_control.cpp index 259ad15aaa..fb649d5c5f 100644 --- a/src/modules/ekf2/EKF/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/baro_height_control.cpp @@ -49,7 +49,7 @@ void Ekf::controlBaroHeightFusion() baroSample baro_sample; - if (_baro_buffer && _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample)) { + if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) { const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt); const float measurement_var = sq(_params.baro_noise); @@ -135,7 +135,7 @@ void Ekf::controlBaroHeightFusion() // reset vertical velocity resetVerticalVelocityToZero(); - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; } else if (is_fusion_failing) { // Some other height source is still working @@ -164,7 +164,7 @@ void Ekf::controlBaroHeightFusion() bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; bias_est.setFusionActive(); _control_status.flags.baro_hgt = true; } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 6be3b11113..948fd55f9e 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -178,20 +178,6 @@ struct gpsMessage { float pdop{}; ///< position dilution of precision }; -struct outputSample { - uint64_t time_us{}; ///< timestamp of the measurement (uSec) - Quatf quat_nominal{}; ///< nominal quaternion describing vehicle attitude - Vector3f vel{}; ///< NED velocity estimate in earth frame (m/sec) - Vector3f pos{}; ///< NED position estimate in earth frame (m/sec) -}; - -struct outputVert { - uint64_t time_us{}; ///< timestamp of the measurement (uSec) - 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) -}; - struct imuSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) Vector3f delta_ang{}; ///< delta angle in body frame (integrated gyro measurements) (rad) @@ -424,10 +410,6 @@ struct parameters { Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m) Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m) - // output complementary filter tuning - float vel_Tau{0.25f}; ///< velocity state correction time constant (1/sec) - float pos_Tau{0.25f}; ///< position state correction time constant (1/sec) - // accel bias learning control float acc_bias_lim{0.4f}; ///< maximum accel bias magnitude (m/sec**2) float acc_bias_learn_acc_lim{25.0f}; ///< learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 8bfa692a5b..460f4c8371 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -43,7 +43,7 @@ #include "ekf.h" #include -void Ekf::controlFusionModes() +void Ekf::controlFusionModes(const imuSample &imu_delayed) { // Store the status to enable change detection _control_status_prev.value = _control_status.value; @@ -52,7 +52,7 @@ void Ekf::controlFusionModes() if (_system_flag_buffer) { systemFlagUpdate system_flags_delayed; - if (_system_flag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &system_flags_delayed)) { + if (_system_flag_buffer->pop_first_older_than(imu_delayed.time_us, &system_flags_delayed)) { set_vehicle_at_rest(system_flags_delayed.at_rest); set_in_air_status(system_flags_delayed.in_air); @@ -97,7 +97,7 @@ void Ekf::controlFusionModes() if (height_source) { ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)", - (unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); + (unsigned long long)imu_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); } } } @@ -107,7 +107,7 @@ void Ekf::controlFusionModes() // check for arrival of new sensor data at the fusion time horizon _time_prev_gps_us = _gps_sample_delayed.time_us; - _gps_data_ready = _gps_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed); + _gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed); if (_gps_data_ready) { // correct velocity for offset relative to IMU @@ -132,7 +132,7 @@ void Ekf::controlFusionModes() if (_range_buffer) { // Get range data from buffer and check validity - _rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress()); + _rng_data_ready = _range_buffer->pop_first_older_than(imu_delayed.time_us, _range_sensor.getSampleAddress()); _range_sensor.setDataReadiness(_rng_data_ready); // update range sensor angle parameters in case they have changed @@ -140,7 +140,7 @@ void Ekf::controlFusionModes() _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); - _range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth); + _range_sensor.runChecks(imu_delayed.time_us, _R_to_earth); if (_range_sensor.isDataHealthy()) { // correct the range data for position offset relative to the IMU @@ -156,7 +156,7 @@ void Ekf::controlFusionModes() const float var = sq(_params.range_noise) + dist_dependant_var; _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _imu_sample_delayed.time_us); + _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), imu_delayed.time_us); } } else { @@ -179,25 +179,25 @@ 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 && !_hagl_sensor_status.flags.flow)) { - _flow_data_ready = _flow_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed); + _flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed); } } if (_airspeed_buffer) { - _tas_data_ready = _airspeed_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed); + _tas_data_ready = _airspeed_buffer->pop_first_older_than(imu_delayed.time_us, &_airspeed_sample_delayed); } - // run EKF-GSF yaw estimator once per _imu_sample_delayed update after all main EKF data samples available - runYawEKFGSF(); + // run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available + runYawEKFGSF(imu_delayed); // control use of observations for aiding controlMagFusion(); - controlOpticalFlowFusion(); + controlOpticalFlowFusion(imu_delayed); controlGpsFusion(); controlAirDataFusion(); - controlBetaFusion(); + controlBetaFusion(imu_delayed); controlDragFusion(); - controlHeightFusion(); + controlHeightFusion(imu_delayed); // Additional data odometry data from an external estimator can be fused. controlExternalVisionFusion(); @@ -358,7 +358,7 @@ void Ekf::controlAirDataFusion() } } -void Ekf::controlBetaFusion() +void Ekf::controlBetaFusion(const imuSample &imu_delayed) { _control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing && _control_status.flags.in_air && !_control_status.flags.fake_pos; @@ -378,7 +378,7 @@ void Ekf::controlBetaFusion() // activate the wind states _control_status.flags.wind = true; // reset the timeout timers to prevent repeated resets - _aid_src_sideslip.time_last_fuse = _imu_sample_delayed.time_us; + _aid_src_sideslip.time_last_fuse = imu_delayed.time_us; resetWind(); } @@ -404,7 +404,7 @@ void Ekf::controlDragFusion() dragSample drag_sample; - if (_drag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &drag_sample)) { + if (_drag_buffer->pop_first_older_than(_time_delayed_us, &drag_sample)) { fuseDrag(drag_sample); } } @@ -415,7 +415,7 @@ void Ekf::controlAuxVelFusion() if (_auxvel_buffer) { auxVelSample auxvel_sample_delayed; - if (_auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &auxvel_sample_delayed)) { + if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &auxvel_sample_delayed)) { resetEstimatorAidStatus(_aid_src_aux_vel); diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 9e310a0d57..7a8edf0c25 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -98,7 +98,7 @@ void Ekf::initialiseCovariance() } -void Ekf::predictCovariance() +void Ekf::predictCovariance(const imuSample &imu_delayed) { // Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values const float dt = _dt_ekf_avg; @@ -114,9 +114,9 @@ void Ekf::predictCovariance() // xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); const float beta = 1.0f - alpha; - _ang_rate_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt); - _accel_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_magnitude_filt); - _accel_vec_filt = alpha * dt_inv * _imu_sample_delayed.delta_vel + beta * _accel_vec_filt; + _ang_rate_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt); + _accel_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_vel.norm(), beta * _accel_magnitude_filt); + _accel_vec_filt = alpha * dt_inv * imu_delayed.delta_vel + beta * _accel_vec_filt; const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim; @@ -141,7 +141,7 @@ void Ekf::predictCovariance() is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966 } - const bool do_inhibit_axis = do_inhibit_all_axes || _imu_sample_delayed.delta_vel_clipping[index] || !is_bias_observable; + const bool do_inhibit_axis = do_inhibit_all_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable; if (do_inhibit_axis) { // store the bias state variances to be reinstated later @@ -220,7 +220,7 @@ void Ekf::predictCovariance() Vector3f d_vel_var; for (int i = 0; i <= 2; i++) { - if (_fault_status.flags.bad_acc_vertical || _imu_sample_delayed.delta_vel_clipping[i]) { + if (_fault_status.flags.bad_acc_vertical || imu_delayed.delta_vel_clipping[i]) { // Increase accelerometer process noise if bad accel data is detected d_vel_var(i) = sq(dt * BADACC_BIAS_PNOISE); @@ -233,7 +233,7 @@ void Ekf::predictCovariance() SquareMatrix24f nextP; // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states - sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, _imu_sample_delayed.delta_vel, d_vel_var, _imu_sample_delayed.delta_ang, d_ang_var, dt, &nextP); + sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, imu_delayed.delta_vel, d_vel_var, imu_delayed.delta_ang, d_ang_var, dt, &nextP); // process noise contribution for delta angle states can be very small compared to // the variances, therefore use algorithm to minimise numerical error @@ -423,7 +423,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // record the pass/fail if (!bad_acc_bias) { _fault_status.flags.bad_acc_bias = false; - _time_acc_bias_check = _imu_sample_delayed.time_us; + _time_acc_bias_check = _time_delayed_us; } else { _fault_status.flags.bad_acc_bias = true; @@ -435,7 +435,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) P.uncorrelateCovariance<3>(13); - _time_acc_bias_check = _imu_sample_delayed.time_us; + _time_acc_bias_check = _time_delayed_us; _fault_status.flags.bad_acc_bias = false; _warning_events.flags.invalid_accel_bias_cov_reset = true; ECL_WARN("invalid accel bias - covariance reset"); diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 6d1fb8fa7a..82dc180dda 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -61,13 +61,6 @@ void Ekf::reset() _state.wind_vel.setZero(); _state.quat_nominal.setIdentity(); - // TODO: who resets the output buffer content? - _output_new.vel.setZero(); - _output_new.pos.setZero(); - _output_new.quat_nominal.setIdentity(); - - _delta_angle_corr.setZero(); - _range_sensor.setPitchOffset(_params.rng_sens_pitch); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); @@ -86,12 +79,12 @@ void Ekf::reset() _prev_dvel_bias_var.zero(); resetGpsDriftCheckFilters(); + + _output_predictor.reset(); } bool Ekf::update() { - bool updated = false; - if (!_filter_initialised) { _filter_initialised = initialiseFilter(); @@ -102,24 +95,29 @@ bool Ekf::update() // Only run the filter if IMU data in the buffer has been updated if (_imu_updated) { + _imu_updated = false; + + // get the oldest IMU data from the buffer + // TODO: explicitly pop at desired time horizon + const imuSample imu_sample_delayed = _imu_buffer.get_oldest(); + // perform state and covariance prediction for the main filter - predictCovariance(); - predictState(); + predictCovariance(imu_sample_delayed); + predictState(imu_sample_delayed); // control fusion of observation data - controlFusionModes(); + controlFusionModes(imu_sample_delayed); // run a separate filter for terrain estimation - runTerrainEstimator(); + runTerrainEstimator(imu_sample_delayed); - updated = true; + _output_predictor.correctOutputStates(imu_sample_delayed.time_us, getGyroBias(), getAccelBias(), + _state.quat_nominal, _state.vel, _state.pos); + + return true; } - // the output observer always runs - // Use full rate IMU data at the current time horizon - calculateOutputStates(_newest_high_rate_imu_sample); - - return updated; + return false; } bool Ekf::initialiseFilter() @@ -146,7 +144,7 @@ bool Ekf::initialiseFilter() if (_mag_buffer) { magSample mag_sample; - if (_mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample)) { + if (_mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) { if (mag_sample.time_us != 0) { if (_mag_counter == 0) { _mag_lpf.reset(mag_sample.mag); @@ -201,12 +199,12 @@ bool Ekf::initialiseFilter() initHagl(); // reset the essential fusion timeout counters - _time_last_hgt_fuse = _imu_sample_delayed.time_us; - _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; - _time_last_hor_vel_fuse = _imu_sample_delayed.time_us; + _time_last_hgt_fuse = _time_delayed_us; + _time_last_hor_pos_fuse = _time_delayed_us; + _time_last_hor_vel_fuse = _time_delayed_us; // reset the output predictor state history to match the EKF initial values - alignOutputFilter(); + _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos); return true; } @@ -233,14 +231,14 @@ bool Ekf::initialiseTilt() return true; } -void Ekf::predictState() +void Ekf::predictState(const imuSample &imu_delayed) { // apply imu bias corrections - const Vector3f delta_ang_bias_scaled = getGyroBias() * _imu_sample_delayed.delta_ang_dt; - Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - delta_ang_bias_scaled; + const Vector3f delta_ang_bias_scaled = getGyroBias() * imu_delayed.delta_ang_dt; + Vector3f corrected_delta_ang = imu_delayed.delta_ang - delta_ang_bias_scaled; // subtract component of angular rate due to earth rotation - corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; + corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * imu_delayed.delta_ang_dt; const Quatf dq(AxisAnglef{corrected_delta_ang}); @@ -249,15 +247,10 @@ void Ekf::predictState() _R_to_earth = Dcmf(_state.quat_nominal); // Calculate an earth frame delta velocity - const Vector3f delta_vel_bias_scaled = getAccelBias() * _imu_sample_delayed.delta_vel_dt; - const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - delta_vel_bias_scaled; + const Vector3f delta_vel_bias_scaled = getAccelBias() * imu_delayed.delta_vel_dt; + const Vector3f corrected_delta_vel = imu_delayed.delta_vel - delta_vel_bias_scaled; const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel; - // calculate a filtered horizontal acceleration with a 1 sec time constant - // this are used for manoeuvre detection elsewhere - const float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt; - _accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy(); - // save the previous value of velocity so we can use trapzoidal integration const Vector3f vel_last = _state.vel; @@ -265,15 +258,15 @@ void Ekf::predictState() _state.vel += corrected_delta_vel_ef; // compensate for acceleration due to gravity - _state.vel(2) += CONSTANTS_ONE_G * _imu_sample_delayed.delta_vel_dt; + _state.vel(2) += CONSTANTS_ONE_G * imu_delayed.delta_vel_dt; // predict position states via trapezoidal integration of velocity - _state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f; + _state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f; constrainStates(); // calculate an average filter update time - float input = 0.5f * (_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt); + float input = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt); // filter and limit input between -50% and +100% of nominal value const float filter_update_s = 1e-6f * _params.filter_update_interval_us; @@ -283,242 +276,21 @@ void Ekf::predictState() // some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication // protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate // due to insufficient averaging - if (_imu_sample_delayed.delta_ang_dt > 0.25f * _dt_ekf_avg) { - _ang_rate_delayed_raw = _imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt; + if (imu_delayed.delta_ang_dt > 0.25f * _dt_ekf_avg) { + _ang_rate_delayed_raw = imu_delayed.delta_ang / imu_delayed.delta_ang_dt; } -} -/* - * Implement a strapdown INS algorithm using the latest IMU data at the current time horizon. - * Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon. - * Calculate delta angle, delta velocity and velocity corrections from the differences and apply them at the - * current time horizon so that the INS states track the EKF states at the delayed fusion time horizon. - * The inspiration for using a complementary filter to correct for time delays in the EKF - * is based on the work by A Khosravian: - * “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements” - * A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University -*/ -void Ekf::calculateOutputStates(const imuSample &imu) -{ - // Use full rate IMU data at the current time horizon - // correct delta angles for bias offsets - const Vector3f delta_ang_bias_scaled = getGyroBias() * imu.delta_ang_dt; - - // Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon - const Vector3f delta_angle(imu.delta_ang - delta_ang_bias_scaled + _delta_angle_corr); + // calculate a filtered horizontal acceleration with a 1 sec time constant + // this are used for manoeuvre detection elsewhere + const float alpha = 1.0f - imu_delayed.delta_vel_dt; + _accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy(); // calculate a yaw change about the earth frame vertical - const float spin_del_ang_D = delta_angle.dot(Vector3f(_R_to_earth_now.row(2))); + const float spin_del_ang_D = corrected_delta_ang.dot(Vector3f(_R_to_earth.row(2))); _yaw_delta_ef += spin_del_ang_D; // Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic // Note fixed coefficients are used to save operations. The exact time constant is not important. - _yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu.delta_ang_dt; - - - _output_new.time_us = imu.time_us; - _output_vert_new.time_us = imu.time_us; - - const Quatf dq(AxisAnglef{delta_angle}); - - // rotate the previous INS quaternion by the delta quaternions - _output_new.quat_nominal = _output_new.quat_nominal * dq; - - // the quaternions must always be normalised after modification - _output_new.quat_nominal.normalize(); - - // calculate the rotation matrix from body to earth frame - _R_to_earth_now = Dcmf(_output_new.quat_nominal); - - // correct delta velocity for bias offsets - const Vector3f delta_vel_bias_scaled = (_state.delta_vel_bias / _dt_ekf_avg) * _imu_sample_delayed.delta_vel_dt; - const Vector3f delta_vel_body{imu.delta_vel - delta_vel_bias_scaled}; - - // rotate the delta velocity to earth frame - Vector3f delta_vel_earth{_R_to_earth_now * delta_vel_body}; - - // correct for measured acceleration due to gravity - delta_vel_earth(2) += CONSTANTS_ONE_G * imu.delta_vel_dt; - - // calculate the earth frame velocity derivatives - if (imu.delta_vel_dt > 1e-4f) { - _vel_deriv = delta_vel_earth * (1.0f / imu.delta_vel_dt); - } - - // save the previous velocity so we can use trapezoidal integration - const Vector3f vel_last(_output_new.vel); - - // 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.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.vert_vel_integ += delta_pos_NED(2); - - // accumulate the time for each update - _output_vert_new.dt += imu.delta_vel_dt; - - // correct velocity for IMU offset - if (imu.delta_ang_dt > 1e-4f) { - // calculate the average angular rate across the last IMU update - const Vector3f ang_rate = imu.delta_ang * (1.0f / imu.delta_ang_dt); - - // calculate the velocity of the IMU relative to the body origin - const Vector3f vel_imu_rel_body = ang_rate % _params.imu_pos_body; - - // rotate the relative velocity into earth frame - _vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body; - } - - // store the INS states in a ring buffer with the same length and time coordinates as the IMU data buffer - if (_imu_updated) { - _output_buffer.push(_output_new); - _output_vert_buffer.push(_output_vert_new); - - // get the oldest INS state data from the ring buffer - // 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(); - - // 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()); - - // convert the quaternion delta to a delta angle - const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f; - - const Vector3f delta_ang_error{scalar * q_error(1), scalar * q_error(2), scalar * q_error(3)}; - - // calculate a gain that provides tight tracking of the estimator attitude states and - // adjust for changes in time delay to maintain consistent damping ratio of ~0.7 - const float time_delay = fmaxf((imu.time_us - _imu_sample_delayed.time_us) * 1e-6f, _dt_imu_avg); - const float att_gain = 0.5f * _dt_imu_avg / time_delay; - - // calculate a corrrection to the delta angle - // that will cause the INS to track the EKF quaternions - _delta_angle_corr = delta_ang_error * att_gain; - _output_tracking_error(0) = delta_ang_error.norm(); - - /* - * Loop through the output filter state history and apply the corrections to the velocity and position states. - * This method is too expensive to use for the attitude states due to the quaternion operations required - * but because it eliminates the time delay in the 'correction loop' it allows higher tracking gains - * to be used and reduces tracking error relative to EKF states. - */ - - // Complementary filter gains - const float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f); - const float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f); - - // calculate down velocity and position tracking errors - 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 vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f; - - applyCorrectionToVerticalOutputBuffer(vert_vel_correction); - - // calculate velocity and position tracking errors - const Vector3f vel_err(_state.vel - output_delayed.vel); - const Vector3f pos_err(_state.pos - output_delayed.pos); - - _output_tracking_error(1) = vel_err.norm(); - _output_tracking_error(2) = pos_err.norm(); - - // calculate a velocity correction that will be applied to the output state history - _vel_err_integ += vel_err; - const Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f; - - // calculate a position correction that will be applied to the output state history - _pos_err_integ += pos_err; - const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f; - - applyCorrectionToOutputBuffer(vel_correction, pos_correction); - } -} - -/* -* 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 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. -*/ -void Ekf::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction) -{ - // loop through the vertical output filter state history starting at the oldest and apply the corrections to the - // 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(); - - for (uint8_t counter = 0; counter < (size - 1); counter++) { - const uint8_t index_next = (index + 1) % size; - outputVert ¤t_state = _output_vert_buffer[index]; - outputVert &next_state = _output_vert_buffer[index_next]; - - // correct the velocity - if (counter == 0) { - current_state.vert_vel += vert_vel_correction; - } - - next_state.vert_vel += vert_vel_correction; - - // position is propagated forward using the corrected velocity and a trapezoidal integrator - 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; - } - - // update output state to corrected values - _output_vert_new = _output_vert_buffer.get_newest(); - - // reset time delta to zero for the next accumulation of full rate IMU data - _output_vert_new.dt = 0.0f; -} - -/* -* Calculate corrections to be applied to vel and pos output state history. -* The vel and pos state history are corrected individually so they track the EKF states at -* the fusion time horizon. This option provides the most accurate tracking of EKF states. -*/ -void Ekf::applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction) -{ - // loop through the output filter state history and apply the corrections to the velocity and position states - for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { - // a constant velocity correction is applied - _output_buffer[index].vel += vel_correction; - - // a constant position correction is applied - _output_buffer[index].pos += pos_correction; - } - - // update output state to corrected values - _output_new = _output_buffer.get_newest(); -} - -/* - * Predict the previous quaternion output state forward using the latest IMU delta angle data. -*/ -Quatf Ekf::calculate_quaternion() const -{ - // Correct delta angle data for bias errors using bias state estimates from the EKF and also apply - // corrections required to track the EKF quaternion states - const Vector3f delta_ang_bias_scaled = getGyroBias() * _newest_high_rate_imu_sample.delta_ang_dt; - - const Vector3f delta_angle{_newest_high_rate_imu_sample.delta_ang - delta_ang_bias_scaled + _delta_angle_corr}; - - // increment the quaternions using the corrected delta angle vector - // the quaternions must always be normalised after modification - return Quatf{_output_new.quat_nominal * AxisAnglef{delta_angle}}.unit(); + _yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt; } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e0b7a55936..be3d58a515 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -115,7 +115,8 @@ public: const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); } const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; } - void getHeadingInnov(float &heading_innov) const { + void getHeadingInnov(float &heading_innov) const + { if (_control_status.flags.mag_hdg) { heading_innov = _aid_src_mag_heading.innovation; @@ -130,7 +131,8 @@ public: } } - void getHeadingInnovVar(float &heading_innov_var) const { + void getHeadingInnovVar(float &heading_innov_var) const + { if (_control_status.flags.mag_hdg) { heading_innov_var = _aid_src_mag_heading.innovation_variance; @@ -145,7 +147,8 @@ public: } } - void getHeadingInnovRatio(float &heading_innov_ratio) const { + void getHeadingInnovRatio(float &heading_innov_ratio) const + { if (_control_status.flags.mag_hdg) { heading_innov_ratio = _aid_src_mag_heading.test_ratio; @@ -244,10 +247,6 @@ public: 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) - const Vector3f &getOutputTrackingError() const { return _output_tracking_error; } - // First argument returns GPS drift metrics in the following array locations // 0 : Horizontal position drift rate (m/s) // 1 : Vertical position drift rate (m/s) @@ -287,7 +286,7 @@ public: const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg); const bool is_mag_alignment_in_flight_complete = is_using_mag && _control_status.flags.mag_aligned_in_flight - && ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)1e6); + && ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)1e6); return _control_status.flags.yaw_align && (is_mag_alignment_in_flight_complete || !is_using_mag); } @@ -380,9 +379,6 @@ public: // return a bitmask integer that describes which state estimates can be used for flight control void get_ekf_soln_status(uint16_t *status) const; - // use the latest IMU data at the current time horizon. - Quatf calculate_quaternion() const; - // rotate quaternion covariances into variances for an equivalent rotation vector Vector3f calcRotVecVariances() const; @@ -448,8 +444,7 @@ private: void updateHorizontalDeadReckoningstatus(); void updateVerticalDeadReckoningStatus(); - struct StateResetCounts - { + struct StateResetCounts { uint8_t velNE{0}; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) uint8_t velD{0}; ///< number of vertical velocity reset events (allow to wrap if count exceeds 255) uint8_t posNE{0}; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) @@ -576,12 +571,6 @@ private: estimator_aid_source2d_s _aid_src_optical_flow{}; - // output predictor states - Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad) - Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m) - Vector3f _pos_err_integ{}; ///< integral of position tracking error (m.s) - Vector3f _output_tracking_error{}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m) - // variables used for the GPS quality checks Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec) Vector2f _gps_velNE_filt{}; ///< filtered GPS North and East velocity (m/sec) @@ -642,12 +631,6 @@ private: float _height_rate_lpf{0.0f}; - // update the real time complementary filter states. This includes the prediction - // and the correction step - void calculateOutputStates(const imuSample &imu); - void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction); - void applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction); - // initialise filter states of both the delayed ekf and the real time complementary filter bool initialiseFilter(void); @@ -655,10 +638,10 @@ private: void initialiseCovariance(); // predict ekf state - void predictState(); + void predictState(const imuSample &imu_delayed); // predict ekf covariance - void predictCovariance(); + void predictCovariance(const imuSample &imu_delayed); // ekf sequential fusion of magnetometer measurements bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); @@ -745,8 +728,8 @@ private: // initialise the terrain vertical position estimator void initHagl(); - void runTerrainEstimator(); - void predictHagl(); + void runTerrainEstimator(const imuSample &imu_delayed); + void predictHagl(const imuSample &imu_delayed); // update the terrain vertical position estimate using a height above ground measurement from the range finder void controlHaglRngFusion(); @@ -773,9 +756,6 @@ private: // Return the magnetic declination in radians to be used by the alignment and fusion processing float getMagDeclination(); - // modify output filter to match the the EKF state at the fusion time horizon - void alignOutputFilter(); - bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation) { for (unsigned i = 0; i < 3; i++) { @@ -834,23 +814,23 @@ private: bool gps_is_good(const gpsMessage &gps); // Control the filter fusion modes - void controlFusionModes(); + void controlFusionModes(const imuSample &imu_delayed); // control fusion of external vision observations void controlExternalVisionFusion(); - void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src); + void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); - void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s& aid_src); + void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src); void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src); void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src); void stopEvPosFusion(); - void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s& aid_src); - void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src); + void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src); + void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); // control fusion of optical flow observations - void controlOpticalFlowFusion(); + void controlOpticalFlowFusion(const imuSample &imu_delayed); void updateOnGroundMotionForOpticalFlowChecks(); void resetOnGroundMotionForOpticalFlowChecks(); @@ -887,7 +867,7 @@ private: void controlAirDataFusion(); // control fusion of synthetic sideslip observations - void controlBetaFusion(); + void controlBetaFusion(const imuSample &imu_delayed); // control fusion of multi-rotor drag specific force observations void controlDragFusion(); @@ -907,11 +887,11 @@ private: // control fusion of auxiliary velocity observations void controlAuxVelFusion(); - void checkVerticalAccelerationHealth(); + void checkVerticalAccelerationHealth(const imuSample &imu_delayed); Likelihood estimateInertialNavFallingLikelihood() const; // control for combined height fusion mode (implemented for switching between baro and range height) - void controlHeightFusion(); + void controlHeightFusion(const imuSample &imu_delayed); void checkHeightSensorRefFallback(); void controlBaroHeightFusion(); void controlGnssHeightFusion(const gpsSample &gps_sample); @@ -978,17 +958,17 @@ private: bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const { - return last_sensor_timestamp + timeout_period < _imu_sample_delayed.time_us; + return last_sensor_timestamp + timeout_period < _time_delayed_us; } bool isRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const { - return sensor_timestamp + acceptance_interval > _imu_sample_delayed.time_us; + return sensor_timestamp + acceptance_interval > _time_delayed_us; } bool isNewestSampleRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const { - return sensor_timestamp + acceptance_interval > _newest_high_rate_imu_sample.time_us; + return sensor_timestamp + acceptance_interval > _time_latest_us; } void startAirspeedFusion(); @@ -1032,7 +1012,7 @@ private: HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; PositionBiasEstimator _ev_pos_b_est{static_cast(PositionSensor::EV), _position_sensor_ref}; - void runYawEKFGSF(); + void runYawEKFGSF(const imuSample &imu_delayed); // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator // Resets the horizontal velocity and position to the default navigation sensor @@ -1088,7 +1068,10 @@ private: void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const { - if (PX4_ISFINITE(status.innovation) && PX4_ISFINITE(status.innovation_variance) && (status.innovation_variance > 0.f)) { + if (PX4_ISFINITE(status.innovation) + && PX4_ISFINITE(status.innovation_variance) + && (status.innovation_variance > 0.f) + ) { status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance); status.innovation_rejected = (status.test_ratio > 1.f); @@ -1104,7 +1087,10 @@ private: bool innovation_rejected = false; for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) { - if (PX4_ISFINITE(status.innovation[i]) && PX4_ISFINITE(status.innovation_variance[i]) && (status.innovation_variance[i] > 0.f)) { + if (PX4_ISFINITE(status.innovation[i]) + && PX4_ISFINITE(status.innovation_variance[i]) + && (status.innovation_variance[i] > 0.f) + ) { status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]); if (status.test_ratio[i] > 1.f) { diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index b1c797cdf2..00a89966fc 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -71,11 +71,7 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f P.uncorrelateCovarianceSetVariance<1>(5, math::max(sq(0.01f), new_horz_vel_var(1))); } - 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; + _output_predictor.resetHorizontalVelocityTo(delta_horz_vel); // record the state change if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) { @@ -89,7 +85,7 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f _state_reset_status.reset_count.velNE++; // Reset the timout timer - _time_last_hor_vel_fuse = _imu_sample_delayed.time_us; + _time_last_hor_vel_fuse = _time_delayed_us; } void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) @@ -101,13 +97,7 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) P.uncorrelateCovarianceSetVariance<1>(6, math::max(sq(0.01f), new_vert_vel_var)); } - 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_new.vert_vel += delta_vert_vel; + _output_predictor.resetVerticalVelocityTo(delta_vert_vel); // record the state change if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) { @@ -121,7 +111,7 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) _state_reset_status.reset_count.velD++; // Reset the timout timer - _time_last_ver_vel_fuse = _imu_sample_delayed.time_us; + _time_last_ver_vel_fuse = _time_delayed_us; } void Ekf::resetHorizontalPositionToLastKnown() @@ -145,11 +135,7 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f P.uncorrelateCovarianceSetVariance<1>(8, math::max(sq(0.01f), new_horz_pos_var(1))); } - 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; + _output_predictor.resetHorizontalPositionTo(delta_horz_pos); // record the state change if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) { @@ -166,7 +152,7 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); // Reset the timout timer - _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; + _time_last_hor_pos_fuse = _time_delayed_us; } bool Ekf::isHeightResetRequired() const @@ -194,16 +180,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v // apply the change in height / height rate to our newest height / height rate estimate // which have already been taken out from the output buffer - _output_new.pos(2) += delta_z; - - // add the reset amount to the output observer buffered data - for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { - _output_buffer[i].pos(2) += delta_z; - _output_vert_buffer[i].vert_vel_integ += delta_z; - } - - // add the reset amount to the output observer vertical position state - _output_vert_new.vert_vel_integ = _state.pos(2); + _output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z); // record the state change if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { @@ -222,7 +199,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); // Reset the timout timer - _time_last_hgt_fuse = _imu_sample_delayed.time_us; + _time_last_hgt_fuse = _time_delayed_us; } void Ekf::resetVerticalVelocityToZero() @@ -233,35 +210,11 @@ void Ekf::resetVerticalVelocityToZero() resetVerticalVelocityTo(0.0f, 10.f); } -// align output filter states to match EKF states at the fusion time horizon -void Ekf::alignOutputFilter() -{ - const outputSample &output_delayed = _output_buffer.get_oldest(); - - // calculate the quaternion rotation delta from the EKF to output observer states at the EKF fusion time horizon - Quatf q_delta{_state.quat_nominal * output_delayed.quat_nominal.inversed()}; - q_delta.normalize(); - - // calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon - const Vector3f vel_delta = _state.vel - output_delayed.vel; - const Vector3f pos_delta = _state.pos - output_delayed.pos; - - // loop through the output filter state history and add the deltas - for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { - _output_buffer[i].quat_nominal = q_delta * _output_buffer[i].quat_nominal; - _output_buffer[i].quat_nominal.normalize(); - _output_buffer[i].vel += vel_delta; - _output_buffer[i].pos += pos_delta; - } - - _output_new = _output_buffer.get_newest(); -} - // Reset heading and magnetic field states bool Ekf::resetMagHeading() { // prevent a reset being performed more than once on the same frame - if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) { + if ((_flt_mag_align_start_time == _time_delayed_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) { return false; } @@ -298,7 +251,7 @@ bool Ekf::resetMagHeading() resetMagCov(); // record the time for the magnetic field alignment event - _flt_mag_align_start_time = _imu_sample_delayed.time_us; + _flt_mag_align_start_time = _time_delayed_us; return true; } @@ -518,7 +471,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons const float gps_alt_ref_prev = getEkfGlobalOriginAltitude(); // reinitialize map projection to latitude, longitude, altitude, and reset position - _pos_ref.initReference(latitude, longitude, _imu_sample_delayed.time_us); + _pos_ref.initReference(latitude, longitude, _time_delayed_us); _gps_alt_ref = altitude; // minimum change in position or height that triggers a reset @@ -883,8 +836,8 @@ void Ekf::updateHorizontalDeadReckoningstatus() _control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding; if (!_control_status.flags.inertial_dead_reckoning) { - if (_imu_sample_delayed.time_us > _params.no_aid_timeout_max) { - _time_last_horizontal_aiding = _imu_sample_delayed.time_us - _params.no_aid_timeout_max; + if (_time_delayed_us > _params.no_aid_timeout_max) { + _time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max; } } @@ -1328,13 +1281,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) } // add the reset amount to the output observer buffered data - for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { - _output_buffer[i].quat_nominal = q_error * _output_buffer[i].quat_nominal; - } - - // apply the change in attitude quaternion to our newest quaternion estimate - // which was already taken out from the output buffer - _output_new.quat_nominal = q_error * _output_new.quat_nominal; + _output_predictor.resetQuaternion(q_error); // record the state change if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) { @@ -1363,7 +1310,7 @@ bool Ekf::resetYawToEKFGSF() resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); // record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight - _flt_mag_align_start_time = _imu_sample_delayed.time_us; + _flt_mag_align_start_time = _time_delayed_us; _control_status.flags.yaw_align = true; if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { @@ -1400,7 +1347,7 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight); } -void Ekf::runYawEKFGSF() +void Ekf::runYawEKFGSF(const imuSample &imu_delayed) { float TAS = 0.f; @@ -1414,7 +1361,7 @@ void Ekf::runYawEKFGSF() } const Vector3f imu_gyro_bias = getGyroBias(); - _yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias); + _yawEstimator.update(imu_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias); } void Ekf::resetGpsDriftCheckFilters() diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 26e21148c3..ea5f66e38e 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -65,27 +65,24 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) _initialised = init(imu_sample.time_us); } - const float dt = math::constrain((imu_sample.time_us - _newest_high_rate_imu_sample.time_us) / 1e6f, 0.0001f, 0.02f); + _time_latest_us = imu_sample.time_us; - if (_newest_high_rate_imu_sample.time_us > 0) { - _dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt; - } - - _newest_high_rate_imu_sample = imu_sample; - - _imu_updated = _imu_down_sampler.update(imu_sample); + // the output observer always runs + _output_predictor.calculateOutputStates(imu_sample.time_us, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt); // accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available - if (_imu_updated) { + if (_imu_down_sampler.update(imu_sample)) { + + _imu_updated = true; _imu_buffer.push(_imu_down_sampler.getDownSampledImuAndTriggerReset()); // get the oldest data from the buffer - _imu_sample_delayed = _imu_buffer.get_oldest(); + _time_delayed_us = _imu_buffer.get_oldest().time_us; // calculate the minimum interval between observations required to guarantee no loss of data // this will occur if data is overwritten before its time stamp falls behind the fusion time horizon - _min_obs_interval_us = (imu_sample.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1); + _min_obs_interval_us = (imu_sample.time_us - _time_delayed_us) / (_obs_buffer_length - 1); } setDragData(imu_sample); @@ -120,7 +117,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample) mag_sample_new.time_us = time_us; _mag_buffer->push(mag_sample_new); - _time_last_mag_buffer_push = _newest_high_rate_imu_sample.time_us; + _time_last_mag_buffer_push = _time_latest_us; } else { ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us); @@ -193,10 +190,10 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps) } _gps_buffer->push(gps_sample_new); - _time_last_gps_buffer_push = _newest_high_rate_imu_sample.time_us; + _time_last_gps_buffer_push = _time_latest_us; if (PX4_ISFINITE(gps.yaw)) { - _time_last_gps_yaw_buffer_push = _newest_high_rate_imu_sample.time_us; + _time_last_gps_yaw_buffer_push = _time_latest_us; } } else { @@ -233,7 +230,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample) baro_sample_new.time_us = time_us; _baro_buffer->push(baro_sample_new); - _time_last_baro_buffer_push = _newest_high_rate_imu_sample.time_us; + _time_last_baro_buffer_push = _time_latest_us; } else { ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us); @@ -304,7 +301,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample) range_sample_new.time_us = time_us; _range_buffer->push(range_sample_new); - _time_last_range_buffer_push = _newest_high_rate_imu_sample.time_us; + _time_last_range_buffer_push = _time_latest_us; } else { ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us); @@ -377,7 +374,7 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) ev_sample_new.time_us = time_us; _ext_vision_buffer->push(ev_sample_new); - _time_last_ext_vision_buffer_push = _newest_high_rate_imu_sample.time_us; + _time_last_ext_vision_buffer_push = _time_latest_us; } else { ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us); @@ -561,17 +558,14 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) ECL_DEBUG("EKF max time delay %.1f ms, OBS length %d\n", (double)ekf_delay_ms, _obs_buffer_length); - if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length) - || !_output_vert_buffer.allocate(_imu_buffer_length)) { + if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_predictor.allocate(_imu_buffer_length)) { printBufferAllocationFailed("IMU and output"); return false; } - _imu_sample_delayed.time_us = timestamp; - _imu_sample_delayed.delta_vel_clipping[0] = false; - _imu_sample_delayed.delta_vel_clipping[1] = false; - _imu_sample_delayed.delta_vel_clipping[2] = false; + _time_delayed_us = timestamp; + _time_latest_us = timestamp; _fault_status.value = 0; @@ -654,7 +648,6 @@ void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name) void EstimatorInterface::print_status() { - printf("IMU average dt: %.6f seconds\n", (double)_dt_imu_avg); printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg); printf("IMU buffer: %d (%d Bytes)\n", _imu_buffer.get_length(), _imu_buffer.get_total_size()); @@ -693,6 +686,5 @@ void EstimatorInterface::print_status() printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size()); } - printf("output buffer: %d/%d (%d Bytes)\n", _output_buffer.entries(), _output_buffer.get_length(), _output_buffer.get_total_size()); - printf("output vert buffer: %d/%d (%d Bytes)\n", _output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size()); + _output_predictor.print_status(); } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 92a3034b15..7a08d3329b 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -67,6 +67,7 @@ #include "range_finder_consistency_check.hpp" #include "sensor_range_finder.hpp" #include "utils.hpp" +#include "output_predictor.h" #include #include @@ -111,10 +112,10 @@ public: void set_in_air_status(bool in_air) { if (!in_air) { - _time_last_on_ground_us = _imu_sample_delayed.time_us; + _time_last_on_ground_us = _time_delayed_us; } else { - _time_last_in_air = _imu_sample_delayed.time_us; + _time_last_in_air = _time_delayed_us; } _control_status.flags.in_air = in_air; @@ -123,7 +124,7 @@ public: void set_vehicle_at_rest(bool at_rest) { _control_status.flags.vehicle_at_rest = at_rest; } // return true if the attitude is usable - bool attitude_valid() const { return _output_new.quat_nominal.isAllFinite() && _control_status.flags.tilt_align; } + bool attitude_valid() const { return _control_status.flags.tilt_align; } // get vehicle landed status data bool get_in_air_status() const { return _control_status.flags.in_air; } @@ -140,7 +141,7 @@ public: void set_gnd_effect() { _control_status.flags.gnd_effect = true; - _time_last_gnd_effect_on = _imu_sample_delayed.time_us; + _time_last_gnd_effect_on = _time_delayed_us; } // set air density used by the multi-rotor specific drag force fusion @@ -190,25 +191,12 @@ public: bool isVerticalVelocityAidingActive() const; int getNumberOfActiveVerticalVelocityAidingSources() const; - 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 { return _output_new.vel - _vel_imu_rel_body_ned; } - - // get the velocity derivative in earth frame - 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); } - - // get the position of the body frame origin in local earth frame - Vector3f getPosition() const - { - // rotate the position of the IMU relative to the boy origin into earth frame - const Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body; - // subtract from the EKF position (which is at the IMU) to get position at the body origin - return _output_new.pos - pos_offset_earth; - } + const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); } + Vector3f getVelocity() const { return _output_predictor.getVelocity(); } + const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); } + float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); } + Vector3f getPosition() const { return _output_predictor.getPosition(); } + const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); } // 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 @@ -246,15 +234,12 @@ public: const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; } void clear_information_events() { _information_events.value = 0; } - // Getter for the average imu update period in s - float get_dt_imu_avg() const { return _dt_imu_avg; } - // Getter for the average EKF update period in s float get_dt_ekf_avg() const { return _dt_ekf_avg; } // Getters for samples on the delayed time horizon - const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; } - const imuSample &get_imu_sample_newest() const { return _newest_high_rate_imu_sample; } + const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); } + const uint64_t &time_delayed_us() const { return _time_delayed_us; } const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } @@ -268,6 +253,8 @@ public: float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; } float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; } + OutputPredictor &output_predictor() { return _output_predictor; }; + protected: EstimatorInterface() = default; @@ -294,10 +281,12 @@ protected: */ uint8_t _imu_buffer_length{0}; - float _dt_imu_avg{0.005f}; // average imu update period in s float _dt_ekf_avg{0.010f}; ///< average update rate of the ekf in s - imuSample _imu_sample_delayed{}; // captures the imu sample on the delayed time horizon + uint64_t _time_delayed_us{0}; // captures the imu sample on the delayed time horizon + uint64_t _time_latest_us{0}; // imu sample capturing the newest imu data + + OutputPredictor _output_predictor{}; // measurement samples capturing measurements on the delayed time horizon gpsSample _gps_sample_delayed{}; @@ -316,14 +305,6 @@ protected: float _flow_min_distance{0.0f}; ///< minimum distance that the optical flow sensor can operate at (m) float _flow_max_distance{10.f}; ///< maximum distance that the optical flow sensor can operate at (m) - // Output Predictor - outputSample _output_new{}; // filter output on the non-delayed time horizon - outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon - imuSample _newest_high_rate_imu_sample{}; // imu sample capturing the newest imu data - Matrix3f _R_to_earth_now{}; // rotation matrix from body to earth frame at current time - Vector3f _vel_imu_rel_body_ned{}; // velocity of IMU relative to body origin in NED earth frame - Vector3f _vel_deriv{}; // velocity derivative at the IMU in NED earth frame (m/s/s) - bool _imu_updated{false}; // true if the ekf should update (completed downsampling process) bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized @@ -356,8 +337,6 @@ protected: // data buffer instances static constexpr uint8_t kBufferLengthDefault = 12; RingBuffer _imu_buffer{kBufferLengthDefault}; - RingBuffer _output_buffer{kBufferLengthDefault}; - RingBuffer _output_vert_buffer{kBufferLengthDefault}; RingBuffer *_gps_buffer{nullptr}; RingBuffer *_mag_buffer{nullptr}; diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/ev_control.cpp index b5fbafb778..da27f11924 100644 --- a/src/modules/ekf2/EKF/ev_control.cpp +++ b/src/modules/ekf2/EKF/ev_control.cpp @@ -45,7 +45,7 @@ void Ekf::controlExternalVisionFusion() // Check for new external vision data extVisionSample ev_sample; - if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &ev_sample)) { + if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_time_delayed_us, &ev_sample)) { bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter); diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index f03204e394..aefd7e1681 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -123,7 +123,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com bias_est.setBias(-_state.pos(2) + measurement); } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; } else { // EV has reset, but quality isn't sufficient @@ -175,7 +175,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com resetVerticalVelocityToZero(); } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; } else if (is_fusion_failing) { // A reset did not fix the issue but all the starting checks are not passing @@ -206,7 +206,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com bias_est.setBias(-_state.pos(2) + measurement); } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; bias_est.setFusionActive(); _control_status.flags.ev_hgt = true; } diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp index bdbbe034ff..980a488672 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -214,7 +214,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem _ev_pos_b_est.reset(); } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; _nb_ev_pos_reset_available = 5; _information_events.flags.starting_vision_pos_fusion = true; @@ -237,7 +237,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; } else { // EV has reset, but quality isn't sufficient @@ -279,7 +279,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure } } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; if (_control_status.flags.in_air) { _nb_ev_pos_reset_available--; diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp index f1bddeb03a..66dd27661f 100644 --- a/src/modules/ekf2/EKF/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/ev_vel_control.cpp @@ -149,7 +149,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common ECL_INFO("reset to %s", AID_SRC_NAME); _information_events.flags.reset_vel_to_vision = true; resetVelocityTo(measurement, measurement_var); - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; } else { // EV has reset, but quality isn't sufficient @@ -174,7 +174,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common _information_events.flags.reset_vel_to_vision = true; ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); resetVelocityTo(measurement, measurement_var); - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; if (_control_status.flags.in_air) { _nb_ev_vel_reset_available--; @@ -213,7 +213,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common ECL_INFO("starting %s fusion", AID_SRC_NAME); } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; _nb_ev_vel_reset_available = 5; _information_events.flags.starting_vision_vel_fusion = true; diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/ev_yaw_control.cpp index 64e85cfedc..c75f6a90d6 100644 --- a/src/modules/ekf2/EKF/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/ev_yaw_control.cpp @@ -82,7 +82,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common ECL_INFO("reset to %s", AID_SRC_NAME); //_information_events.flags.reset_yaw_to_vision = true; // TODO resetQuatStateYaw(aid_src.observation, aid_src.observation_variance); - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; } else { // EV has reset, but quality isn't sufficient @@ -106,7 +106,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common //_information_events.flags.reset_yaw_to_vision = true; // TODO ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); resetQuatStateYaw(aid_src.innovation, aid_src.observation_variance); - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; if (_control_status.flags.in_air) { _nb_ev_yaw_reset_available--; @@ -148,7 +148,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common _control_status.flags.yaw_align = true; } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; _information_events.flags.starting_vision_yaw_fusion = true; _control_status.flags.ev_yaw = true; @@ -162,7 +162,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common // reset yaw to EV resetQuatStateYaw(aid_src.observation, aid_src.observation_variance); - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; _information_events.flags.starting_vision_yaw_fusion = true; _control_status.flags.yaw_align = false; diff --git a/src/modules/ekf2/EKF/fake_height_control.cpp b/src/modules/ekf2/EKF/fake_height_control.cpp index 9763e6425c..c11f22cd33 100644 --- a/src/modules/ekf2/EKF/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/fake_height_control.cpp @@ -51,7 +51,7 @@ void Ekf::controlFakeHgtFusion() const float obs_var = sq(_params.pos_noaid_noise); const float innov_gate = 3.f; - updateVerticalPositionAidSrcStatus(_imu_sample_delayed.time_us, _last_known_pos(2), obs_var, innov_gate, aid_src); + updateVerticalPositionAidSrcStatus(_time_delayed_us, _last_known_pos(2), obs_var, innov_gate, aid_src); const bool continuing_conditions_passing = !isVerticalAidingActive(); @@ -98,7 +98,7 @@ void Ekf::resetFakeHgtFusion() resetVerticalVelocityToZero(); resetHeightToLastKnown(); - _aid_src_fake_hgt.time_last_fuse = _imu_sample_delayed.time_us; + _aid_src_fake_hgt.time_last_fuse = _time_delayed_us; } void Ekf::resetHeightToLastKnown() diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp index f6a013d086..afe33b5fec 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/fake_pos_control.cpp @@ -65,7 +65,7 @@ void Ekf::controlFakePosFusion() const float innov_gate = 3.f; - updateHorizontalPositionAidSrcStatus(_imu_sample_delayed.time_us, Vector2f(_last_known_pos), obs_var, innov_gate, aid_src); + updateHorizontalPositionAidSrcStatus(_time_delayed_us, Vector2f(_last_known_pos), obs_var, innov_gate, aid_src); const bool continuing_conditions_passing = !isHorizontalAidingActive(); @@ -118,7 +118,7 @@ void Ekf::resetFakePosFusion() resetHorizontalPositionToLastKnown(); resetHorizontalVelocityToZero(); - _aid_src_fake_pos.time_last_fuse = _imu_sample_delayed.time_us; + _aid_src_fake_pos.time_last_fuse = _time_delayed_us; } void Ekf::stopFakePosFusion() diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index 15e3669cf7..56b75a054b 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -123,7 +123,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) resetVerticalVelocityToZero(); } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; } else if (is_fusion_failing) { // Some other height source is still working @@ -151,7 +151,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) bias_est.setBias(_state.pos(2) + measurement); } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; bias_est.setFusionActive(); _control_status.flags.gps_hgt = true; } diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index 97385fbd25..61d8546329 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -239,7 +239,7 @@ bool Ekf::gps_is_good(const gpsMessage &gps) // assume failed first time through if (_last_gps_fail_us == 0) { - _last_gps_fail_us = _imu_sample_delayed.time_us; + _last_gps_fail_us = _time_delayed_us; } // if any user selected checks have failed, record the fail time @@ -255,10 +255,10 @@ bool Ekf::gps_is_good(const gpsMessage &gps) (_gps_check_fail_status.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) || (_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD)) ) { - _last_gps_fail_us = _imu_sample_delayed.time_us; + _last_gps_fail_us = _time_delayed_us; } else { - _last_gps_pass_us = _imu_sample_delayed.time_us; + _last_gps_pass_us = _time_delayed_us; } // continuous period without fail of x seconds required to return a healthy status diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 481319023c..0685cca385 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -185,13 +185,13 @@ void Ekf::controlGpsFusion() // reset velocity _information_events.flags.reset_vel_to_gps = true; resetVelocityTo(velocity, vel_obs_var); - _aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us; + _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; } // reset position _information_events.flags.reset_pos_to_gps = true; resetHorizontalPositionTo(position, pos_obs_var); - _aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us; + _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; _control_status.flags.gps = true; @@ -204,12 +204,12 @@ void Ekf::controlGpsFusion() // reset velocity _information_events.flags.reset_vel_to_gps = true; resetVelocityTo(velocity, vel_obs_var); - _aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us; + _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; // reset position _information_events.flags.reset_pos_to_gps = true; resetHorizontalPositionTo(position, pos_obs_var); - _aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us; + _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; } } } diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 61363ce683..e12bf1bf26 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -137,8 +137,8 @@ void Ekf::fuseGpsYaw() gnss_yaw.fused = is_fused; if (is_fused) { - _time_last_heading_fuse = _imu_sample_delayed.time_us; - gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us; + _time_last_heading_fuse = _time_delayed_us; + gnss_yaw.time_last_fuse = _time_delayed_us; } } @@ -159,7 +159,7 @@ bool Ekf::resetYawToGps(const float gnss_yaw) const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f)); resetQuatStateYaw(measured_yaw, yaw_variance); - _aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us; + _aid_src_gnss_yaw.time_last_fuse = _time_delayed_us; _gnss_yaw_signed_test_ratio_lpf.reset(0.f); return true; diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 82dc3174f6..d8eaf90484 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -37,9 +37,9 @@ #include "ekf.h" -void Ekf::controlHeightFusion() +void Ekf::controlHeightFusion(const imuSample &imu_delayed) { - checkVerticalAccelerationHealth(); + checkVerticalAccelerationHealth(imu_delayed); updateGroundEffect(); @@ -61,6 +61,7 @@ void Ekf::checkHeightSensorRefFallback() switch (_params.height_sensor_ref) { default: + /* FALLTHROUGH */ case HeightSensor::UNKNOWN: fallback_list[0] = HeightSensor::GNSS; @@ -110,7 +111,7 @@ void Ekf::checkHeightSensorRefFallback() } } -void Ekf::checkVerticalAccelerationHealth() +void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed) { // Check for IMU accelerometer vibration induced clipping as evidenced by the vertical // innovations being positive and not stale. @@ -121,9 +122,9 @@ void Ekf::checkVerticalAccelerationHealth() // Check for more than 50% clipping affected IMU samples within the past 1 second const uint16_t clip_count_limit = 1.f / _dt_ekf_avg; - const bool is_clipping = _imu_sample_delayed.delta_vel_clipping[0] || - _imu_sample_delayed.delta_vel_clipping[1] || - _imu_sample_delayed.delta_vel_clipping[2]; + const bool is_clipping = imu_delayed.delta_vel_clipping[0] || + imu_delayed.delta_vel_clipping[1] || + imu_delayed.delta_vel_clipping[2]; if (is_clipping && _clip_counter < clip_count_limit) { _clip_counter++; @@ -141,10 +142,10 @@ void Ekf::checkVerticalAccelerationHealth() || (inertial_nav_falling_likelihood == Likelihood::HIGH); if (bad_vert_accel) { - _time_bad_vert_accel = _imu_sample_delayed.time_us; + _time_bad_vert_accel = imu_delayed.time_us; } else { - _time_good_vert_accel = _imu_sample_delayed.time_us; + _time_good_vert_accel = imu_delayed.time_us; } // declare a bad vertical acceleration measurement and make the declaration persist diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index f03ad0ee32..8af9127764 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -46,7 +46,7 @@ void Ekf::controlMagFusion() magSample mag_sample; if (_mag_buffer) { - mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample); + mag_data_ready = _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample); if (mag_data_ready) { @@ -129,7 +129,7 @@ void Ekf::controlMagFusion() if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) { if (shouldInhibitMag()) { - if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) { + if (uint32_t(_time_delayed_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) { // If magnetometer use has been inhibited continuously then stop the fusion stopMagFusion(); } @@ -137,7 +137,7 @@ void Ekf::controlMagFusion() return; } else { - _mag_use_not_inhibit_us = _imu_sample_delayed.time_us; + _mag_use_not_inhibit_us = _time_delayed_us; } const bool mag_enabled_previously = _control_status_prev.flags.mag_hdg || _control_status_prev.flags.mag_3D; @@ -216,7 +216,7 @@ void Ekf::runOnGroundYawReset() void Ekf::runInAirYawReset() { // prevent a reset being performed more than once on the same frame - if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us) + if ((_flt_mag_align_start_time == _time_delayed_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) { return; } @@ -269,7 +269,7 @@ void Ekf::runInAirYawReset() _control_status.flags.mag_aligned_in_flight = true; // record the time for the magnetic field alignment event - _flt_mag_align_start_time = _imu_sample_delayed.time_us; + _flt_mag_align_start_time = _time_delayed_us; } } } @@ -286,7 +286,7 @@ void Ekf::check3DMagFusionSuitability() checkMagBiasObservability(); if (_mag_bias_observable || _yaw_angle_observable) { - _time_last_mov_3d_mag_suitable = _imu_sample_delayed.time_us; + _time_last_mov_3d_mag_suitable = _time_delayed_us; } } @@ -316,13 +316,13 @@ void Ekf::checkMagBiasObservability() } else if (_mag_bias_observable) { // require sustained yaw motion of 50% the initial yaw rate threshold - const float yaw_dt = 1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started); + const float yaw_dt = 1e-6f * (float)(_time_delayed_us - _time_yaw_started); const float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * yaw_dt; _mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req; } _yaw_delta_ef = 0.0f; - _time_yaw_started = _imu_sample_delayed.time_us; + _time_yaw_started = _time_delayed_us; } bool Ekf::canUse3DMagFusion() const @@ -330,7 +330,7 @@ bool Ekf::canUse3DMagFusion() const // Use of 3D fusion requires an in-air heading alignment but it should not // be used when the heading and mag biases are not observable for more than 2 seconds return _control_status.flags.mag_aligned_in_flight - && ((_imu_sample_delayed.time_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6); + && ((_time_delayed_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6); } void Ekf::checkMagDeclRequired() @@ -411,7 +411,7 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag) { // For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise // before they are used to constrain heading drift - const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6); + const bool update_all_states = ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)5e6); if (!_mag_decl_cov_reset) { // After any magnetic field covariance reset event the earth field state diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index bfd9a97175..a6f2138b31 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -215,7 +215,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo if (fused[0] && fused[1] && fused[2]) { aid_src_mag.fused = true; - aid_src_mag.time_last_fuse = _imu_sample_delayed.time_us; + aid_src_mag.time_last_fuse = _time_delayed_us; return true; } @@ -313,9 +313,9 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { - _time_last_heading_fuse = _imu_sample_delayed.time_us; + _time_last_heading_fuse = _time_delayed_us; - aid_src_status.time_last_fuse = _imu_sample_delayed.time_us; + aid_src_status.time_last_fuse = _time_delayed_us; aid_src_status.fused = true; _fault_status.flags.bad_hdg = false; diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 402bfa3647..a061e2c7f1 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -151,7 +151,7 @@ void Ekf::fuseOptFlow() _fault_status.flags.bad_optflow_Y = !fused[1]; if (fused[0] && fused[1]) { - _aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us; + _aid_src_optical_flow.time_last_fuse = _time_delayed_us; _aid_src_optical_flow.fused = true; } } diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index f31f9eb936..c6f32ce2e2 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -38,7 +38,7 @@ #include "ekf.h" -void Ekf::controlOpticalFlowFusion() +void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) { // Check if on ground motion is un-suitable for use of optical flow if (!_control_status.flags.in_air) { @@ -49,15 +49,15 @@ void Ekf::controlOpticalFlowFusion() } // Accumulate autopilot gyro data across the same time interval as the flow sensor - const Vector3f delta_angle(_imu_sample_delayed.delta_ang - (getGyroBias() * _imu_sample_delayed.delta_ang_dt)); + const Vector3f delta_angle(imu_delayed.delta_ang - (getGyroBias() * imu_delayed.delta_ang_dt)); if (_delta_time_of < 0.1f) { _imu_del_ang_of += delta_angle; - _delta_time_of += _imu_sample_delayed.delta_ang_dt; + _delta_time_of += imu_delayed.delta_ang_dt; } else { // reset the accumulators if the time interval is too large _imu_del_ang_of = delta_angle; - _delta_time_of = _imu_sample_delayed.delta_ang_dt; + _delta_time_of = imu_delayed.delta_ang_dt; } if (_flow_data_ready) { @@ -71,10 +71,10 @@ void Ekf::controlOpticalFlowFusion() if (!is_delta_time_good && (_flow_sample_delayed.dt > FLT_EPSILON)) { - if (fabsf(_imu_sample_delayed.delta_ang_dt - _flow_sample_delayed.dt) < 0.1f) { + if (fabsf(imu_delayed.delta_ang_dt - _flow_sample_delayed.dt) < 0.1f) { // reset accumulators to current IMU _imu_del_ang_of = delta_angle; - _delta_time_of = _imu_sample_delayed.delta_ang_dt; + _delta_time_of = imu_delayed.delta_ang_dt; is_delta_time_good = true; } @@ -82,7 +82,7 @@ void Ekf::controlOpticalFlowFusion() if (is_quality_good && !is_delta_time_good) { ECL_DEBUG("Optical flow: bad delta time: OF dt %.6f s (min: %.3f, max: %.3f), IMU dt %.6f s", (double)_flow_sample_delayed.dt, (double)delta_time_min, (double)delta_time_max, - (double)_imu_sample_delayed.delta_ang_dt); + (double)imu_delayed.delta_ang_dt); } } @@ -140,8 +140,8 @@ void Ekf::controlOpticalFlowFusion() // 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))); + && ((_time_delayed_us > (_time_good_motion_us + (uint64_t)1E5)) + || (_time_delayed_us < (_time_bad_motion_us + (uint64_t)5E6))); const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid(); const bool inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required) @@ -184,7 +184,7 @@ void Ekf::controlOpticalFlowFusion() } } - _aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us; + _aid_src_optical_flow.time_last_fuse = _time_delayed_us; _control_status.flags.opt_flow = true; return; @@ -192,7 +192,7 @@ void Ekf::controlOpticalFlowFusion() 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)) { + if (_time_delayed_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)) { @@ -216,7 +216,7 @@ void Ekf::controlOpticalFlowFusion() _information_events.flags.reset_pos_to_last_known = true; resetHorizontalPositionTo(_last_known_pos.xy(), 0.f); - _aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us; + _aid_src_optical_flow.time_last_fuse = _time_delayed_us; } } @@ -237,15 +237,15 @@ void Ekf::updateOnGroundMotionForOpticalFlowChecks() || (_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; + _time_bad_motion_us = _time_delayed_us; } else { - _time_good_motion_us = _imu_sample_delayed.time_us; + _time_good_motion_us = _time_delayed_us; } } void Ekf::resetOnGroundMotionForOpticalFlowChecks() { _time_bad_motion_us = 0; - _time_good_motion_us = _imu_sample_delayed.time_us; + _time_good_motion_us = _time_delayed_us; } diff --git a/src/modules/ekf2/EKF/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor.cpp new file mode 100644 index 0000000000..bdb8077ea0 --- /dev/null +++ b/src/modules/ekf2/EKF/output_predictor.cpp @@ -0,0 +1,374 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4. 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. + * + ****************************************************************************/ + +#include "output_predictor.h" + +using matrix::AxisAnglef; +using matrix::Dcmf; +using matrix::Quatf; +using matrix::Vector2f; +using matrix::Vector3f; + +void OutputPredictor::print_status() +{ + printf("output predictor: IMU dt: %.4f, EKF dt: %.4f\n", (double)_dt_update_states_avg, (double)_dt_correct_states_avg); + + printf("output predictor: tracking error, angular: %.6f rad, velocity: %.3f m/s, position: %.3f m\n", + (double)_output_tracking_error(0), (double)_output_tracking_error(1), (double)_output_tracking_error(2)); + + printf("output buffer: %d/%d (%d Bytes)\n", _output_buffer.entries(), _output_buffer.get_length(), + _output_buffer.get_total_size()); + + printf("output vert buffer: %d/%d (%d Bytes)\n", _output_vert_buffer.entries(), _output_vert_buffer.get_length(), + _output_vert_buffer.get_total_size()); +} + +void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state) +{ + const outputSample &output_delayed = _output_buffer.get_oldest(); + + // calculate the quaternion rotation delta from the EKF to output observer states at the EKF fusion time horizon + Quatf q_delta{quat_state * output_delayed.quat_nominal.inversed()}; + q_delta.normalize(); + + // calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon + const Vector3f vel_delta = vel_state - output_delayed.vel; + const Vector3f pos_delta = pos_state - output_delayed.pos; + + // loop through the output filter state history and add the deltas + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + _output_buffer[i].quat_nominal = q_delta * _output_buffer[i].quat_nominal; + _output_buffer[i].quat_nominal.normalize(); + _output_buffer[i].vel += vel_delta; + _output_buffer[i].pos += pos_delta; + } + + _output_new = _output_buffer.get_newest(); +} + +void OutputPredictor::reset() +{ + // TODO: who resets the output buffer content? + _output_new = {}; + _output_vert_new = {}; + + _accel_bias.setZero(); + _gyro_bias.setZero(); + + _time_last_update_states_us = 0; + _time_last_correct_states_us = 0; + + _R_to_earth_now.setIdentity(); + _vel_imu_rel_body_ned.setZero(); + _vel_deriv.setZero(); + + _delta_angle_corr.setZero(); + + _vel_err_integ.setZero(); + _pos_err_integ.setZero(); + + _output_tracking_error.setZero(); + + for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { + _output_buffer[index] = {}; + } + + for (uint8_t index = 0; index < _output_vert_buffer.get_length(); index++) { + _output_vert_buffer[index] = {}; + } +} + +void OutputPredictor::resetQuaternion(const Quatf &quat_change) +{ + // add the reset amount to the output observer buffered data + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + _output_buffer[i].quat_nominal = quat_change * _output_buffer[i].quat_nominal; + } + + // apply the change in attitude quaternion to our newest quaternion estimate + // which was already taken out from the output buffer + _output_new.quat_nominal = quat_change * _output_new.quat_nominal; +} + +void OutputPredictor::resetHorizontalVelocityTo(const Vector2f &delta_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; +} + +void OutputPredictor::resetVerticalVelocityTo(float delta_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_new.vert_vel += delta_vert_vel; +} + +void OutputPredictor::resetHorizontalPositionTo(const Vector2f &delta_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; +} + +void OutputPredictor::resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change) +{ + // apply the change in height / height rate to our newest height / height rate estimate + // which have already been taken out from the output buffer + _output_new.pos(2) += vert_pos_change; + + // add the reset amount to the output observer buffered data + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + _output_buffer[i].pos(2) += vert_pos_change; + _output_vert_buffer[i].vert_vel_integ += vert_pos_change; + } + + // add the reset amount to the output observer vertical position state + _output_vert_new.vert_vel_integ = new_vert_pos; +} + +void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector3f &delta_angle, + const float delta_angle_dt, const Vector3f &delta_velocity, const float delta_velocity_dt) +{ + // Use full rate IMU data at the current time horizon + if (_time_last_update_states_us != 0) { + const float dt = math::constrain((time_us - _time_last_update_states_us) * 1e-6f, 0.0001f, 0.03f); + _dt_update_states_avg = 0.8f * _dt_update_states_avg + 0.2f * dt; + } + + _time_last_update_states_us = time_us; + + // correct delta angle and delta velocity for bias offsets + // Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon + const Vector3f delta_angle_bias_scaled = _gyro_bias * delta_angle_dt; + const Vector3f delta_angle_corrected(delta_angle - delta_angle_bias_scaled + _delta_angle_corr); + + const Vector3f delta_vel_bias_scaled = _accel_bias * delta_velocity_dt; + const Vector3f delta_velocity_corrected(delta_velocity - delta_vel_bias_scaled); + + _output_new.time_us = time_us; + _output_vert_new.time_us = time_us; + + const Quatf dq(AxisAnglef{delta_angle_corrected}); + + // rotate the previous INS quaternion by the delta quaternions + _output_new.quat_nominal = _output_new.quat_nominal * dq; + + // the quaternions must always be normalised after modification + _output_new.quat_nominal.normalize(); + + // calculate the rotation matrix from body to earth frame + _R_to_earth_now = Dcmf(_output_new.quat_nominal); + + // rotate the delta velocity to earth frame + Vector3f delta_vel_earth{_R_to_earth_now * delta_velocity_corrected}; + + // correct for measured acceleration due to gravity + delta_vel_earth(2) += CONSTANTS_ONE_G * delta_velocity_dt; + + // calculate the earth frame velocity derivatives + if (delta_velocity_dt > 0.001f) { + _vel_deriv = delta_vel_earth / delta_velocity_dt; + } + + // save the previous velocity so we can use trapezoidal integration + const Vector3f vel_last(_output_new.vel); + + // 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.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) * (delta_velocity_dt * 0.5f); + _output_new.pos += delta_pos_NED; + _output_vert_new.vert_vel_integ += delta_pos_NED(2); + + // accumulate the time for each update + _output_vert_new.dt += delta_velocity_dt; + + // correct velocity for IMU offset + if (delta_angle_dt > 0.001f) { + // calculate the average angular rate across the last IMU update + const Vector3f ang_rate = delta_angle_corrected / delta_angle_dt; + + // calculate the velocity of the IMU relative to the body origin + const Vector3f vel_imu_rel_body = ang_rate % _imu_pos_body; + + // rotate the relative velocity into earth frame + _vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body; + } +} + +void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, + const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias, + const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state) +{ + // calculate an average filter update time + if (_time_last_correct_states_us != 0) { + const float dt = math::constrain((time_delayed_us - _time_last_correct_states_us) * 1e-6f, 0.0001f, 0.03f); + _dt_correct_states_avg = 0.8f * _dt_correct_states_avg + 0.2f * dt; + } + + _time_last_correct_states_us = time_delayed_us; + + // store IMU bias for calculateOutputStates + _gyro_bias = gyro_bias; + _accel_bias = accel_bias; + + // store the INS states in a ring buffer with the same length and time coordinates as the IMU data buffer + _output_buffer.push(_output_new); + _output_vert_buffer.push(_output_vert_new); + + // get the oldest INS state data from the ring buffer + // 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(); + + // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon + const Quatf q_error((quat_state.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; + + const Vector3f delta_ang_error{scalar * q_error(1), scalar * q_error(2), scalar * q_error(3)}; + + // calculate a gain that provides tight tracking of the estimator attitude states and + // adjust for changes in time delay to maintain consistent damping ratio of ~0.7 + const uint64_t time_latest_us = _time_last_update_states_us; + const float time_delay = fmaxf((time_latest_us - time_delayed_us) * 1e-6f, _dt_update_states_avg); + const float att_gain = 0.5f * _dt_update_states_avg / time_delay; + + // calculate a corrrection to the delta angle + // that will cause the INS to track the EKF quaternions + _delta_angle_corr = delta_ang_error * att_gain; + _output_tracking_error(0) = delta_ang_error.norm(); + + /* + * Loop through the output filter state history and apply the corrections to the velocity and position states. + * This method is too expensive to use for the attitude states due to the quaternion operations required + * but because it eliminates the time delay in the 'correction loop' it allows higher tracking gains + * to be used and reduces tracking error relative to EKF states. + */ + + // Complementary filter gains + const float vel_gain = _dt_correct_states_avg / math::constrain(_vel_tau, _dt_correct_states_avg, 10.f); + const float pos_gain = _dt_correct_states_avg / math::constrain(_pos_tau, _dt_correct_states_avg, 10.f); + + // calculate down velocity and position tracking errors + const float vert_vel_err = (vel_state(2) - output_vert_delayed.vert_vel); + const float vert_vel_integ_err = (pos_state(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 vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f; + + applyCorrectionToVerticalOutputBuffer(vert_vel_correction); + + // calculate velocity and position tracking errors + const Vector3f vel_err(vel_state - output_delayed.vel); + const Vector3f pos_err(pos_state - output_delayed.pos); + + _output_tracking_error(1) = vel_err.norm(); + _output_tracking_error(2) = pos_err.norm(); + + // calculate a velocity correction that will be applied to the output state history + _vel_err_integ += vel_err; + const Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f; + + // calculate a position correction that will be applied to the output state history + _pos_err_integ += pos_err; + const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f; + + applyCorrectionToOutputBuffer(vel_correction, pos_correction); +} + +void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction) +{ + // loop through the vertical output filter state history starting at the oldest and apply the corrections to the + // 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(); + + for (uint8_t counter = 0; counter < (size - 1); counter++) { + const uint8_t index_next = (index + 1) % size; + outputVert ¤t_state = _output_vert_buffer[index]; + outputVert &next_state = _output_vert_buffer[index_next]; + + // correct the velocity + if (counter == 0) { + current_state.vert_vel += vert_vel_correction; + } + + next_state.vert_vel += vert_vel_correction; + + // position is propagated forward using the corrected velocity and a trapezoidal integrator + 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; + } + + // update output state to corrected values + _output_vert_new = _output_vert_buffer.get_newest(); + + // reset time delta to zero for the next accumulation of full rate IMU data + _output_vert_new.dt = 0.0f; +} + +void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction) +{ + // loop through the output filter state history and apply the corrections to the velocity and position states + for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { + // a constant velocity correction is applied + _output_buffer[index].vel += vel_correction; + + // a constant position correction is applied + _output_buffer[index].pos += pos_correction; + } + + // update output state to corrected values + _output_new = _output_buffer.get_newest(); +} diff --git a/src/modules/ekf2/EKF/output_predictor.h b/src/modules/ekf2/EKF/output_predictor.h new file mode 100644 index 0000000000..5eb3b44e2f --- /dev/null +++ b/src/modules/ekf2/EKF/output_predictor.h @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4. 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. + * + ****************************************************************************/ + +#ifndef EKF_OUTPUT_PREDICTOR_H +#define EKF_OUTPUT_PREDICTOR_H + +#include + +#include "common.h" +#include "RingBuffer.h" + +#include + +class OutputPredictor +{ +public: + OutputPredictor() + { + reset(); + }; + + ~OutputPredictor() = default; + + // modify output filter to match the the EKF state at the fusion time horizon + void alignOutputFilter(const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, + const matrix::Vector3f &pos_state); + /* + * Implement a strapdown INS algorithm using the latest IMU data at the current time horizon. + * Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon. + * Calculate delta angle, delta velocity and velocity corrections from the differences and apply them at the + * current time horizon so that the INS states track the EKF states at the delayed fusion time horizon. + * The inspiration for using a complementary filter to correct for time delays in the EKF + * is based on the work by A Khosravian: + * “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements” + * A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University + */ + void calculateOutputStates(const uint64_t time_us, const matrix::Vector3f &delta_angle, const float delta_angle_dt, + const matrix::Vector3f &delta_velocity, const float delta_velocity_dt); + + void correctOutputStates(const uint64_t time_delayed_us, + const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias, + const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state); + + void resetQuaternion(const matrix::Quatf &quat_change); + + void resetHorizontalVelocityTo(const matrix::Vector2f &delta_horz_vel); + void resetVerticalVelocityTo(float delta_vert_vel); + + void resetHorizontalPositionTo(const matrix::Vector2f &delta_horz_pos); + void resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change); + + void print_status(); + + bool allocate(uint8_t size) + { + if (_output_buffer.allocate(size) && _output_vert_buffer.allocate(size)) { + reset(); + return true; + } + + return false; + } + + void reset(); + + const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; } + + // get the velocity of the body frame origin in local NED earth frame + matrix::Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; } + + // get the velocity derivative in earth frame + const matrix::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); } + + // get the position of the body frame origin in local earth frame + matrix::Vector3f getPosition() const + { + // rotate the position of the IMU relative to the boy origin into earth frame + const matrix::Vector3f pos_offset_earth{_R_to_earth_now * _imu_pos_body}; + // subtract from the EKF position (which is at the IMU) to get position at the body origin + return _output_new.pos - pos_offset_earth; + } + + // return an array containing the output predictor angular, velocity and position tracking + // error magnitudes (rad), (m/sec), (m) + const matrix::Vector3f &getOutputTrackingError() const { return _output_tracking_error; } + + void set_imu_offset(const matrix::Vector3f &offset) { _imu_pos_body = offset; } + void set_pos_correction_tc(const float tau) { _pos_tau = tau; } + void set_vel_correction_tc(const float tau) { _vel_tau = tau; } + +private: + + /* + * 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 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. + */ + void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction); + + /* + * Calculate corrections to be applied to vel and pos output state history. + * The vel and pos state history are corrected individually so they track the EKF states at + * the fusion time horizon. This option provides the most accurate tracking of EKF states. + */ + void applyCorrectionToOutputBuffer(const matrix::Vector3f &vel_correction, const matrix::Vector3f &pos_correction); + + // return the square of two floating point numbers - used in auto coded sections + static constexpr float sq(float var) { return var * var; } + + struct outputSample { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) + matrix::Quatf quat_nominal{1.f, 0.f, 0.f, 0.f}; ///< nominal quaternion describing vehicle attitude + matrix::Vector3f vel{0.f, 0.f, 0.f}; ///< NED velocity estimate in earth frame (m/sec) + matrix::Vector3f pos{0.f, 0.f, 0.f}; ///< NED position estimate in earth frame (m/sec) + }; + + struct outputVert { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) + float vert_vel{0.f}; ///< Vertical velocity calculated using alternative algorithm (m/sec) + float vert_vel_integ{0.f}; ///< Integral of vertical velocity (m) + float dt{0.f}; ///< delta time (sec) + }; + + RingBuffer _output_buffer{12}; + RingBuffer _output_vert_buffer{12}; + + matrix::Vector3f _accel_bias{}; + matrix::Vector3f _gyro_bias{}; + + float _dt_update_states_avg{0.005f}; // average imu update period in s + float _dt_correct_states_avg{0.010f}; // average update rate of the ekf in s + + uint64_t _time_last_update_states_us{0}; ///< last time the output states were updated (uSec) + uint64_t _time_last_correct_states_us{0}; ///< last time the output states were updated (uSec) + + // Output Predictor + outputSample _output_new{}; // filter output on the non-delayed time horizon + outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon + matrix::Matrix3f _R_to_earth_now{}; // rotation matrix from body to earth frame at current time + matrix::Vector3f _vel_imu_rel_body_ned{}; // velocity of IMU relative to body origin in NED earth frame + matrix::Vector3f _vel_deriv{}; // velocity derivative at the IMU in NED earth frame (m/s/s) + + // output predictor states + matrix::Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad) + matrix::Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m) + matrix::Vector3f _pos_err_integ{}; ///< integral of position tracking error (m.s) + + matrix::Vector3f _output_tracking_error{}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m) + + matrix::Vector3f _imu_pos_body{}; ///< xyz position of IMU in body frame (m) + + // output complementary filter tuning + float _vel_tau{0.25f}; ///< velocity state correction time constant (1/sec) + float _pos_tau{0.25f}; ///< position state correction time constant (1/sec) +}; + +#endif // !EKF_OUTPUT_PREDICTOR_H diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 4108098a8d..5965462c0d 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -101,7 +101,7 @@ void Ekf::controlRangeHeightFusion() // reset vertical velocity resetVerticalVelocityToZero(); - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; } else if (is_fusion_failing) { // Some other height source is still working @@ -139,7 +139,7 @@ void Ekf::controlRangeHeightFusion() bias_est.setBias(_state.pos(2) + measurement); } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; bias_est.setFusionActive(); _control_status.flags.rng_hgt = true; } diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 650ca36f75..9125b7b038 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -65,7 +65,7 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const sideslip.fusion_enabled = _control_status.flags.fuse_aspd; - sideslip.timestamp_sample = _imu_sample_delayed.time_us; + sideslip.timestamp_sample = _time_delayed_us; const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f); setEstimatorAidStatusTestRatio(sideslip, innov_gate); @@ -121,6 +121,6 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) _fault_status.flags.bad_sideslip = !is_fused; if (is_fused) { - sideslip.time_last_fuse = _imu_sample_delayed.time_us; + sideslip.time_last_fuse = _time_delayed_us; } } diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 6406088a55..127fe0f3fb 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -55,7 +55,7 @@ void Ekf::initHagl() _terrain_var = sq(_params.rng_gnd_clearance); } -void Ekf::runTerrainEstimator() +void Ekf::runTerrainEstimator(const imuSample &imu_delayed) { // If we are on ground, store the local position and time to use as a reference if (!_control_status.flags.in_air) { @@ -63,7 +63,7 @@ void Ekf::runTerrainEstimator() _control_status.flags.rng_fault = false; } - predictHagl(); + predictHagl(imu_delayed); controlHaglRngFusion(); controlHaglFlowFusion(); @@ -75,15 +75,15 @@ void Ekf::runTerrainEstimator() } } -void Ekf::predictHagl() +void Ekf::predictHagl(const imuSample &imu_delayed) { // predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle // process noise due to errors in vehicle height estimate - _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise); + _terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise); // process noise due to terrain gradient - _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) + _terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1))); // limit the variance to prevent it becoming badly conditioned @@ -104,7 +104,7 @@ void Ekf::controlHaglRngFusion() //const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.rng_hgt; // TODO: should not be fused when using range height const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData() && (_rng_consistency_check.getTestRatio() < 1.f); - _time_last_healthy_rng_data = _imu_sample_delayed.time_us; + _time_last_healthy_rng_data = _time_delayed_us; if (_hagl_sensor_status.flags.range_finder) { if (continuing_conditions_passing) { @@ -193,7 +193,7 @@ void Ekf::resetHaglRng() _terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom(); _terrain_var = getRngVar(); _terrain_vpos_reset_counter++; - _time_last_hagl_fuse = _imu_sample_delayed.time_us; + _time_last_hagl_fuse = _time_delayed_us; } void Ekf::stopHaglRngFusion() @@ -238,7 +238,7 @@ void Ekf::fuseHaglRng() // correct the variance _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); // record last successful fusion event - _time_last_hagl_fuse = _imu_sample_delayed.time_us; + _time_last_hagl_fuse = _time_delayed_us; _innov_check_fail_status.flags.reject_hagl = false; } else { @@ -286,7 +286,7 @@ void Ekf::controlHaglFlowFusion() } } else if (_hagl_sensor_status.flags.flow - && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) { + && (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) { // No data anymore. Stop until it comes back. stopHaglFlowFusion(); } @@ -405,8 +405,8 @@ void Ekf::fuseFlowForTerrain() _terrain_var = fmaxf(_terrain_var - KyHyP, sq(0.01f)); - _time_last_flow_terrain_fuse = _imu_sample_delayed.time_us; - //_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us; // TODO: separate aid source status for OF terrain? + _time_last_flow_terrain_fuse = _time_delayed_us; + //_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain? _aid_src_optical_flow.fused = true; } diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index e7cf2db341..f252579f32 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -137,7 +137,7 @@ void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src) && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 1) ) { aid_src.fused = true; - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; } else { aid_src.fused = false; @@ -154,7 +154,7 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) && fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], 2) ) { aid_src.fused = true; - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; } else { aid_src.fused = false; @@ -170,7 +170,7 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 4) ) { aid_src.fused = true; - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; } else { aid_src.fused = false; @@ -184,7 +184,7 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, 5)) { aid_src.fused = true; - aid_src.time_last_fuse = _imu_sample_delayed.time_us; + aid_src.time_last_fuse = _time_delayed_us; } } } @@ -239,7 +239,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) case 0: if (healthy) { _fault_status.flags.bad_vel_N = false; - _time_last_hor_vel_fuse = _imu_sample_delayed.time_us; + _time_last_hor_vel_fuse = _time_delayed_us; } else { _fault_status.flags.bad_vel_N = true; @@ -250,7 +250,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) case 1: if (healthy) { _fault_status.flags.bad_vel_E = false; - _time_last_hor_vel_fuse = _imu_sample_delayed.time_us; + _time_last_hor_vel_fuse = _time_delayed_us; } else { _fault_status.flags.bad_vel_E = true; @@ -261,7 +261,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) case 2: if (healthy) { _fault_status.flags.bad_vel_D = false; - _time_last_ver_vel_fuse = _imu_sample_delayed.time_us; + _time_last_ver_vel_fuse = _time_delayed_us; } else { _fault_status.flags.bad_vel_D = true; @@ -272,7 +272,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) case 3: if (healthy) { _fault_status.flags.bad_pos_N = false; - _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; + _time_last_hor_pos_fuse = _time_delayed_us; } else { _fault_status.flags.bad_pos_N = true; @@ -283,7 +283,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) case 4: if (healthy) { _fault_status.flags.bad_pos_E = false; - _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; + _time_last_hor_pos_fuse = _time_delayed_us; } else { _fault_status.flags.bad_pos_E = true; @@ -294,7 +294,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) case 5: if (healthy) { _fault_status.flags.bad_pos_D = false; - _time_last_hgt_fuse = _imu_sample_delayed.time_us; + _time_last_hgt_fuse = _time_delayed_us; } else { _fault_status.flags.bad_pos_D = true; diff --git a/src/modules/ekf2/EKF/zero_velocity_update.cpp b/src/modules/ekf2/EKF/zero_velocity_update.cpp index 35c8dddc1a..0897262ea3 100644 --- a/src/modules/ekf2/EKF/zero_velocity_update.cpp +++ b/src/modules/ekf2/EKF/zero_velocity_update.cpp @@ -64,7 +64,7 @@ void Ekf::controlZeroVelocityUpdate() fuseVelPosHeight(innovation(1), innov_var(1), 1); fuseVelPosHeight(innovation(2), innov_var(2), 2); - _time_last_zero_velocity_fuse = _imu_sample_delayed.time_us; + _time_last_zero_velocity_fuse = _time_delayed_us; } } } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index adf96aadeb..6860c88c90 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -151,8 +151,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_ev_pos_z(_params->ev_pos_body(2)), _param_ekf2_arsp_thr(_params->arsp_thr), _param_ekf2_fuse_beta(_params->beta_fusion_enabled), - _param_ekf2_tau_vel(_params->vel_Tau), - _param_ekf2_tau_pos(_params->pos_Tau), _param_ekf2_gbias_init(_params->switch_on_gyro_bias), _param_ekf2_abias_init(_params->switch_on_accel_bias), _param_ekf2_angerr_init(_params->initial_tilt_err), @@ -298,8 +296,8 @@ bool EKF2::multi_init(int imu, int mag) int EKF2::print_status() { - PX4_INFO_RAW("ekf2:%d EKF dt: %.4fs, IMU dt: %.4fs, attitude: %d, local position: %d, global position: %d\n", - _instance, (double)_ekf.get_dt_ekf_avg(), (double)_ekf.get_dt_imu_avg(), _ekf.attitude_valid(), + PX4_INFO_RAW("ekf2:%d EKF dt: %.4fs, attitude: %d, local position: %d, global position: %d\n", + _instance, (double)_ekf.get_dt_ekf_avg(), _ekf.attitude_valid(), _ekf.local_position_is_valid(), _ekf.global_position_is_valid()); perf_print_counter(_ecl_ekf_update_perf); @@ -343,6 +341,13 @@ void EKF2::Run() _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s); + const matrix::Vector3f imu_pos_body(_param_ekf2_imu_pos_x.get(), + _param_ekf2_imu_pos_y.get(), + _param_ekf2_imu_pos_z.get()); + _ekf.output_predictor().set_imu_offset(imu_pos_body); + _ekf.output_predictor().set_pos_correction_tc(_param_ekf2_tau_pos.get()); + _ekf.output_predictor().set_vel_correction_tc(_param_ekf2_tau_vel.get()); + // The airspeed scale factor correcton is only available via parameter as used by the airspeed module param_t param_aspd_scale = param_find("ASPD_SCALE_1"); @@ -604,7 +609,7 @@ void EKF2::Run() perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start)); PublishLocalPosition(now); - PublishOdometry(now); + PublishOdometry(now, imu_sample_new); PublishGlobalPosition(now); PublishWindEstimate(now); @@ -790,8 +795,7 @@ void EKF2::PublishAttitude(const hrt_abstime ×tamp) // generate vehicle attitude quaternion data vehicle_attitude_s att; att.timestamp_sample = timestamp; - const Quatf q{_ekf.calculate_quaternion()}; - q.copyTo(att.q); + _ekf.getQuaternion().copyTo(att.q); _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter); att.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); @@ -916,7 +920,7 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) if (information_event_updated || warning_event_updated) { estimator_event_flags_s event_flags{}; - event_flags.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; + event_flags.timestamp_sample = _ekf.time_delayed_us(); event_flags.information_event_changes = _filter_information_event_changes; event_flags.gps_checks_passed = _ekf.information_event_flags().gps_checks_passed; @@ -1055,7 +1059,7 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) { // publish estimator innovation data estimator_innovations_s innovations{}; - innovations.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; + innovations.timestamp_sample = _ekf.time_delayed_us(); _ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos); _ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos); _ekf.getBaroHgtInnov(innovations.baro_vpos); @@ -1095,7 +1099,7 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing); - _preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations); + _preflt_checker.update(_ekf.get_dt_ekf_avg(), innovations); } } @@ -1103,7 +1107,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) { // publish estimator innovation test ratio data estimator_innovations_s test_ratios{}; - test_ratios.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; + test_ratios.timestamp_sample = _ekf.time_delayed_us(); _ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0], test_ratios.gps_vpos); _ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos); @@ -1129,7 +1133,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) { // publish estimator innovation variance data estimator_innovations_s variances{}; - variances.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; + variances.timestamp_sample = _ekf.time_delayed_us(); _ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos); _ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos); _ekf.getBaroHgtInnovVar(variances.baro_vpos); @@ -1252,11 +1256,11 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) _local_position_pub.publish(lpos); } -void EKF2::PublishOdometry(const hrt_abstime ×tamp) +void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sample) { // generate vehicle odometry data vehicle_odometry_s odom; - odom.timestamp_sample = timestamp; + odom.timestamp_sample = imu_sample.time_us; // position odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; @@ -1270,7 +1274,7 @@ void EKF2::PublishOdometry(const hrt_abstime ×tamp) _ekf.getVelocity().copyTo(odom.velocity); // angular_velocity - const Vector3f rates{_ekf.get_imu_sample_newest().delta_ang / _ekf.get_imu_sample_newest().delta_ang_dt}; + const Vector3f rates{imu_sample.delta_ang / imu_sample.delta_ang_dt}; const Vector3f angular_velocity = rates - _ekf.getGyroBias(); angular_velocity.copyTo(odom.angular_velocity); @@ -1308,7 +1312,7 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) || (timestamp >= _last_sensor_bias_published + 1_s)) { estimator_sensor_bias_s bias{}; - bias.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; + bias.timestamp_sample = _ekf.time_delayed_us(); // take device ids from sensor_selection_s if not using specific vehicle_imu_s if (_device_id_gyro != 0) { @@ -1352,7 +1356,7 @@ void EKF2::PublishStates(const hrt_abstime ×tamp) { // publish estimator states estimator_states_s states; - states.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; + states.timestamp_sample = _ekf.time_delayed_us(); states.n_states = Ekf::_k_num_states; _ekf.getStateAtFusionHorizonAsVector().copyTo(states.states); _ekf.covariances_diagonal().copyTo(states.covariances); @@ -1363,7 +1367,7 @@ void EKF2::PublishStates(const hrt_abstime ×tamp) void EKF2::PublishStatus(const hrt_abstime ×tamp) { estimator_status_s status{}; - status.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; + status.timestamp_sample = _ekf.time_delayed_us(); _ekf.getOutputTrackingError().copyTo(status.output_tracking_error); @@ -1439,7 +1443,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) if (update) { estimator_status_flags_s status_flags{}; - status_flags.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; + status_flags.timestamp_sample = _ekf.time_delayed_us(); status_flags.control_status_changes = _filter_control_status_changes; status_flags.cs_tilt_align = _ekf.control_status_flags().tilt_align; @@ -1529,7 +1533,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp) yaw_est_test_data.weight)) { yaw_est_test_data.yaw_composite_valid = _ekf.isYawEmergencyEstimateAvailable(); - yaw_est_test_data.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; + yaw_est_test_data.timestamp_sample = _ekf.time_delayed_us(); yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _yaw_est_pub.publish(yaw_est_test_data); @@ -1541,7 +1545,7 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) if (_ekf.get_wind_status()) { // Publish wind estimate only if ekf declares them valid wind_s wind{}; - wind.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; + wind.timestamp_sample = _ekf.time_delayed_us(); const Vector2f wind_vel = _ekf.getWindVelocity(); const Vector2f wind_vel_var = _ekf.getWindVelocityVariance(); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index e39ddd53b8..222776f621 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -153,7 +153,7 @@ private: void PublishInnovationTestRatios(const hrt_abstime ×tamp); void PublishInnovationVariances(const hrt_abstime ×tamp); void PublishLocalPosition(const hrt_abstime ×tamp); - void PublishOdometry(const hrt_abstime ×tamp); + void PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sample); void PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_odometry_s &ev_odom); void PublishOpticalFlowVel(const hrt_abstime ×tamp); void PublishSensorBias(const hrt_abstime ×tamp); @@ -576,9 +576,9 @@ private: _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables // output predictor filter time constants - (ParamExtFloat) + (ParamFloat) _param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec) - (ParamExtFloat) + (ParamFloat) _param_ekf2_tau_pos, ///< time constant used by the output position complementary filter (sec) // IMU switch on bias parameters diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp index d3aac9664d..713e274345 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp @@ -48,7 +48,7 @@ void EkfLogger::writeStateToFile() void EkfLogger::writeState() { if (_state_logging_enabled) { - uint64_t time = _ekf->get_imu_sample_delayed().time_us; + uint64_t time = _ekf->time_delayed_us(); _file << time; if (_state_logging_enabled) {