diff --git a/EKF/common.h b/EKF/common.h index 68ced5caef..d14ddea111 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -45,6 +45,7 @@ namespace estimator { +using matrix::AxisAnglef; using matrix::Dcmf; using matrix::Eulerf; using matrix::Matrix3f; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 4ae8f19373..a65256c6f9 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -99,6 +99,8 @@ bool Ekf::init(uint64_t timestamp) bool Ekf::update() { + bool updated = false; + if (!_filter_initialised) { _filter_initialised = initialiseFilter(); @@ -120,18 +122,14 @@ bool Ekf::update() // run a separate filter for terrain estimation runTerrainEstimator(); + updated = true; } // the output observer always runs + // Use full rate IMU data at the current time horizon calculateOutputStates(); - // check for NaN or inf on attitude states - if (!ISFINITE(_state.quat_nominal(0)) || !ISFINITE(_output_new.quat_nominal(0))) { - return false; - } - - // We don't have valid data to output until tilt and yaw alignment is complete - return _control_status.flags.tilt_align && _control_status.flags.yaw_align; + return updated; } bool Ekf::initialiseFilter() @@ -365,7 +363,7 @@ void Ekf::predictState() _dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input; } -bool Ekf::collect_imu(imuSample &imu) +bool Ekf::collect_imu(const imuSample &imu) { // accumulate and downsample IMU data across a period FILTER_UPDATE_PERIOD_MS long @@ -389,7 +387,7 @@ bool Ekf::collect_imu(imuSample &imu) // accumulate the most recent delta velocity data at the updated rotation frame // assume effective sample time is halfway between the previous and current rotation frame - _imu_down_sampled.delta_vel += (_imu_sample_new.delta_vel + delta_R * _imu_sample_new.delta_vel) * 0.5f; + _imu_down_sampled.delta_vel += (imu.delta_vel + delta_R * imu.delta_vel) * 0.5f; // if the target time delta between filter prediction steps has been exceeded // write the accumulated IMU data to the ring buffer @@ -402,11 +400,23 @@ bool Ekf::collect_imu(imuSample &imu) _imu_collection_time_adj += 0.01f * (_imu_down_sampled.delta_ang_dt - target_dt); _imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * target_dt, 0.5f * target_dt); - imu.delta_ang = _q_down_sampled.to_axis_angle(); - imu.delta_vel = _imu_down_sampled.delta_vel; - imu.delta_ang_dt = _imu_down_sampled.delta_ang_dt; - imu.delta_vel_dt = _imu_down_sampled.delta_vel_dt; + imuSample imu_sample_new; + imu_sample_new.delta_ang = _q_down_sampled.to_axis_angle(); + imu_sample_new.delta_vel = _imu_down_sampled.delta_vel; + imu_sample_new.delta_ang_dt = _imu_down_sampled.delta_ang_dt; + imu_sample_new.delta_vel_dt = _imu_down_sampled.delta_vel_dt; + imu_sample_new.time_us = imu.time_us; + _imu_buffer.push(imu_sample_new); + + // get the oldest data from the buffer + _imu_sample_delayed = _imu_buffer.get_oldest(); + + // 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_new.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1); + + // reset _imu_down_sampled.delta_ang.setZero(); _imu_down_sampled.delta_vel.setZero(); _imu_down_sampled.delta_ang_dt = 0.0f; @@ -414,10 +424,13 @@ bool Ekf::collect_imu(imuSample &imu) _q_down_sampled(0) = 1.0f; _q_down_sampled(1) = _q_down_sampled(2) = _q_down_sampled(3) = 0.0f; - return true; + _imu_updated = true; + + } else { + _imu_updated = false; } - return false; + return _imu_updated; } /* @@ -433,37 +446,30 @@ bool Ekf::collect_imu(imuSample &imu) void Ekf::calculateOutputStates() { // Use full rate IMU data at the current time horizon - const imuSample &imu_new = _imu_sample_new; + const imuSample &imu = _imu_sample_new; // correct delta angles for bias offsets - Vector3f delta_angle; - float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg; - delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0) * dt_scale_correction; - delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1) * dt_scale_correction; - delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2) * dt_scale_correction; - - // calculate a yaw change about the earth frame vertical - float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) + - _R_to_earth_now(2, 1) * delta_angle(1) + - _R_to_earth_now(2, 2) * delta_angle(2); - _yaw_delta_ef += spin_del_ang_D; - - // Calculate filtered yaw rate to be used by the magnetomer 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_sample_new.delta_ang_dt; - - // correct delta velocity for bias offsets - Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias * dt_scale_correction; + const float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg; // Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon - delta_angle += _delta_angle_corr; + const Vector3f delta_angle{imu.delta_ang - _state.gyro_bias * dt_scale_correction + _delta_angle_corr}; + + // calculate a yaw change about the earth frame vertical + const float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) + + _R_to_earth_now(2, 1) * delta_angle(1) + + _R_to_earth_now(2, 2) * delta_angle(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; // convert the delta angle to an equivalent delta quaternions Quatf dq; dq.from_axis_angle(delta_angle); // rotate the previous INS quaternion by the delta quaternions - _output_new.time_us = imu_new.time_us; + _output_new.time_us = imu.time_us; _output_new.quat_nominal = _output_new.quat_nominal * dq; // the quaternions must always be normalised afer modification @@ -472,19 +478,22 @@ void Ekf::calculateOutputStates() // calculate the rotation matrix from body to earth frame _R_to_earth_now = quat_to_invrotmat(_output_new.quat_nominal); - // rotate the delta velocity to earth frame - Vector3f delta_vel_NED = _R_to_earth_now * delta_vel; + // correct delta velocity for bias offsets + const Vector3f delta_vel{imu.delta_vel - _state.accel_bias * dt_scale_correction}; - // corrrect for measured accceleration due to gravity - delta_vel_NED(2) += CONSTANTS_ONE_G * imu_new.delta_vel_dt; + // rotate the delta velocity to earth frame + Vector3f delta_vel_NED{_R_to_earth_now * delta_vel}; + + // correct for measured acceleration due to gravity + delta_vel_NED(2) += CONSTANTS_ONE_G * imu.delta_vel_dt; // calculate the earth frame velocity derivatives - if (imu_new.delta_vel_dt > 1e-4f) { - _vel_deriv_ned = delta_vel_NED * (1.0f / imu_new.delta_vel_dt); + if (imu.delta_vel_dt > 1e-4f) { + _vel_deriv_ned = delta_vel_NED * (1.0f / imu.delta_vel_dt); } // save the previous velocity so we can use trapezoidal integration - Vector3f vel_last = _output_new.vel; + 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 @@ -493,31 +502,29 @@ void Ekf::calculateOutputStates() // use trapezoidal integration to calculate the INS position states // do the same for vertical state used by alternative correction algorithm - Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f); + const Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (imu.delta_vel_dt * 0.5f); _output_new.pos += delta_pos_NED; _output_vert_new.vel_d_integ += delta_pos_NED(2); // accumulate the time for each update - _output_vert_new.dt += imu_new.delta_vel_dt; + _output_vert_new.dt += imu.delta_vel_dt; // correct velocity for IMU offset - if (_imu_sample_new.delta_ang_dt > 1e-4f) { + if (imu.delta_ang_dt > 1e-4f) { // calculate the average angular rate across the last IMU update - Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f / _imu_sample_new.delta_ang_dt); + const Vector3f ang_rate = imu.delta_ang * (1.0f / imu.delta_ang_dt); // calculate the velocity of the IMU relative to the body origin - Vector3f vel_imu_rel_body = cross_product(ang_rate, _params.imu_pos_body); + const Vector3f vel_imu_rel_body = cross_product(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); - _imu_updated = false; // get the oldest INS state data from the ring buffer // this data will be at the EKF fusion time horizon @@ -530,7 +537,6 @@ void Ekf::calculateOutputStates() q_error.normalize(); // convert the quaternion delta to a delta angle - Vector3f delta_ang_error; float scalar; if (q_error(0) >= 0.0f) { @@ -540,23 +546,20 @@ void Ekf::calculateOutputStates() scalar = 2.0f; } - delta_ang_error(0) = scalar * q_error(1); - delta_ang_error(1) = scalar * q_error(2); - delta_ang_error(2) = scalar * q_error(3); + 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 - float time_delay = 1e-6f * (float)(_imu_sample_new.time_us - _imu_sample_delayed.time_us); - time_delay = fmaxf(time_delay, _dt_imu_avg); - float att_gain = 0.5f * _dt_imu_avg / time_delay; + 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; // calculate velocity and position tracking errors - Vector3f vel_err = (_state.vel - _output_sample_delayed.vel); - Vector3f pos_err = (_state.pos - _output_sample_delayed.pos); + const Vector3f vel_err{_state.vel - _output_sample_delayed.vel}; + const Vector3f pos_err{_state.pos - _output_sample_delayed.pos}; // collect magnitude tracking error for diagnostics _output_tracking_error[0] = delta_ang_error.norm(); @@ -571,8 +574,8 @@ void Ekf::calculateOutputStates() */ // Complementary filter gains - float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f); - float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f); + 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 a correction to be applied to vel_d that casues vel_d_integ to track the EKF @@ -584,12 +587,12 @@ void Ekf::calculateOutputStates() */ // calculate down velocity and position tracking errors - float vel_d_err = (_state.vel(2) - _output_vert_delayed.vel_d); - float pos_d_err = (_state.pos(2) - _output_vert_delayed.vel_d_integ); + const float vel_d_err = (_state.vel(2) - _output_vert_delayed.vel_d); + const float pos_d_err = (_state.pos(2) - _output_vert_delayed.vel_d_integ); // calculate a velocity correction that will be applied to the output state history // using a PD feedback tuned to a 5% overshoot - float vel_d_correction = pos_d_err * pos_gain + vel_d_err * pos_gain * 1.1f; + const float vel_d_correction = pos_d_err * pos_gain + vel_d_err * pos_gain * 1.1f; /* * Calculate corrections to be applied to vel and pos output state history. @@ -638,11 +641,11 @@ void Ekf::calculateOutputStates() // calculate a velocity correction that will be applied to the output state history _vel_err_integ += vel_err; - Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f; + 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; - Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f; + const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f; // 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++) { @@ -658,3 +661,17 @@ void Ekf::calculateOutputStates() } } } + +/* + * 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_angle{_imu_sample_new.delta_ang - _state.gyro_bias * (_dt_imu_avg / _dt_ekf_avg) + _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(); +} diff --git a/EKF/ekf.h b/EKF/ekf.h index b7b2bcfd71..942a46e9c9 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -125,7 +125,8 @@ public: // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined bool collect_gps(uint64_t time_usec, struct gps_message *gps); - bool collect_imu(imuSample &imu); + + bool collect_imu(const imuSample &imu); // get the ekf WGS-84 origin position and height and the system time it was last set // return true if the origin is valid @@ -230,6 +231,9 @@ public: // return the quaternion defining the rotation from the EKF to the External Vision reference frame void get_ekf2ev_quaternion(float *quat); + // use the latest IMU data at the current time horizon. + Quatf calculate_quaternion() const; + private: static constexpr uint8_t _k_num_states{24}; ///< number of EKF states diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 617684bfcd..9e1635df4f 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -62,16 +62,12 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u _dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt; } - // copy data imuSample imu_sample_new; imu_sample_new.delta_ang = Vector3f(delta_ang); imu_sample_new.delta_vel = Vector3f(delta_vel); - - // convert time from us to secs - imu_sample_new.delta_ang_dt = delta_ang_dt / 1e6f; - imu_sample_new.delta_vel_dt = delta_vel_dt / 1e6f; + imu_sample_new.delta_ang_dt = delta_ang_dt * 1e-6f; + imu_sample_new.delta_vel_dt = delta_vel_dt * 1e-6f; imu_sample_new.time_us = time_usec; - _imu_ticks++; // calculate a metric which indicates the amount of coning vibration Vector3f temp = cross_product(imu_sample_new.delta_ang, _delta_ang_prev); @@ -92,26 +88,19 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u if ((_vibe_metrics[1] * 4.0E4f > _params.is_moving_scaler) || (_vibe_metrics[2] * 2.1E2f > _params.is_moving_scaler) || ((imu_sample_new.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) { - _time_last_move_detect_us = _imu_sample_new.time_us; + + _time_last_move_detect_us = imu_sample_new.time_us; } - _vehicle_at_rest = ((_imu_sample_new.time_us - _time_last_move_detect_us) > (uint64_t)1E6); + + _vehicle_at_rest = ((imu_sample_new.time_us - _time_last_move_detect_us) > (uint64_t)1E6); + } else { - _time_last_move_detect_us = _imu_sample_new.time_us; + _time_last_move_detect_us = imu_sample_new.time_us; _vehicle_at_rest = false; } // accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available if (collect_imu(imu_sample_new)) { - _imu_buffer.push(imu_sample_new); - _imu_ticks = 0; - _imu_updated = true; - - // get the oldest data from the buffer - _imu_sample_delayed = _imu_buffer.get_oldest(); - - // 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_new.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1); // down-sample the drag specific force data by accumulating and calculating the mean when // sufficient samples have been collected @@ -157,13 +146,8 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u _drag_down_sampled.accelXY.zero(); _drag_down_sampled.time_us = 0; _drag_sample_time_dt = 0.0f; - } } - - } else { - _imu_updated = false; - } } @@ -528,8 +512,6 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _imu_sample_delayed.delta_vel_dt = 0.0f; _imu_sample_delayed.time_us = timestamp; - _imu_ticks = 0; - _initialised = false; _time_last_imu = 0; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 4e72c4bcb5..207b360fd9 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -42,6 +42,7 @@ #include "common.h" #include "RingBuffer.h" +#include #include #include #include @@ -159,7 +160,7 @@ public: virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; } // accumulate and downsample IMU data to the EKF prediction rate - virtual bool collect_imu(imuSample &imu) { return true; } + virtual bool collect_imu(const imuSample &imu) = 0; // set delta angle imu data void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float (&delta_ang)[3], float (&delta_vel)[3]); @@ -203,6 +204,9 @@ public: */ virtual bool reset_imu_bias() = 0; + // return true if the attitude is usable + bool attitude_valid() { return ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; } + // get vehicle landed status data bool get_in_air_status() {return _control_status.flags.in_air;} @@ -361,10 +365,7 @@ public: virtual void get_ekf_soln_status(uint16_t *status) = 0; // Getter for the average imu update period in s - float get_dt_imu_avg() - { - return _dt_imu_avg; - } + float get_dt_imu_avg() const { return _dt_imu_avg; } // Getter for the imu sample on the delayed time horizon imuSample get_imu_sample_delayed() @@ -378,12 +379,6 @@ public: return _baro_sample_delayed; } - // Getter for a flag indicating if the ekf should update (completed downsampling process) - bool get_imu_updated() - { - return _imu_updated; - } - void print_status(); static const unsigned FILTER_UPDATE_PERIOD_MS = 8; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta @@ -449,8 +444,6 @@ protected: Vector3f _vel_imu_rel_body_ned; // velocity of IMU relative to body origin in NED earth frame Vector3f _vel_deriv_ned; // velocity derivative at the IMU in NED earth frame (m/s/s) - uint64_t _imu_ticks{0}; // counter for imu updates - 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 diff --git a/EKF/tests/pytest/test_basics.py b/EKF/tests/pytest/test_basics.py index dd7e825695..46b8294e2b 100644 --- a/EKF/tests/pytest/test_basics.py +++ b/EKF/tests/pytest/test_basics.py @@ -54,7 +54,7 @@ def test_filter_initialized(initialized_ekf): """Make sure the EKF updates after a few IMU, Mag and Baro updates """ ekf, _ = initialized_ekf - assert ekf.update() + assert ekf.attitude_valid() @pytest.mark.benchmark