diff --git a/EKF/common.h b/EKF/common.h index 474d7d2b13..f8840b5267 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -229,6 +229,10 @@ struct parameters { Vector3f rng_pos_body; // xyz position of range sensor in body frame (m) Vector3f flow_pos_body; // xyz position of range sensor focal point in body frame (m) + // output complementary filter tuning + float vel_Tau; // velocity state correction time constant (1/sec) + float pos_Tau; // postion state correction time constant (1/sec) + // Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility. parameters() { @@ -307,6 +311,10 @@ struct parameters { gps_pos_body = {}; rng_pos_body = {}; flow_pos_body = {}; + + // output complementary filter tuning time constants + vel_Tau = 0.5f; + pos_Tau = 0.25f; } }; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index e660014498..5578f38ec6 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -120,8 +120,6 @@ Ekf::Ekf(): memset(_mag_innov_var, 0, sizeof(_mag_innov_var)); memset(_flow_innov_var, 0, sizeof(_flow_innov_var)); _delta_angle_corr.setZero(); - _delta_vel_corr.setZero(); - _vel_corr.setZero(); _last_known_posNE.setZero(); _imu_down_sampled = {}; _q_down_sampled.setZero(); @@ -154,9 +152,6 @@ bool Ekf::init(uint64_t timestamp) _output_new.quat_nominal = matrix::Quaternion(); _delta_angle_corr.setZero(); - _delta_vel_corr.setZero(); - _vel_corr.setZero(); - _imu_down_sampled.delta_ang.setZero(); _imu_down_sampled.delta_vel.setZero(); _imu_down_sampled.delta_ang_dt = 0.0f; @@ -657,8 +652,7 @@ void Ekf::calculateOutputStates() _R_to_earth_now = quat_to_invrotmat(_output_new.quat_nominal); // rotate the delta velocity to earth frame - // apply a delta velocity correction required to track the velocity states at the EKF fusion time horizon - Vector3f delta_vel_NED = _R_to_earth_now * delta_vel + _delta_vel_corr; + Vector3f delta_vel_NED = _R_to_earth_now * delta_vel; // corrrect for measured accceleration due to gravity delta_vel_NED(2) += _gravity_mss * imu_new.delta_vel_dt; @@ -670,50 +664,68 @@ void Ekf::calculateOutputStates() _output_new.vel += delta_vel_NED; // use trapezoidal integration to calculate the INS position states - // apply a velocity correction required to track the position states at the EKF fusion time horizon - _output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt; + _output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f); // store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer if (_imu_updated) { _output_buffer.push(_output_new); _imu_updated = false; + + // get the oldest INS state data from the ring buffer + // this data will be at the EKF fusion time horizon + _output_sample_delayed = _output_buffer.get_oldest(); + + // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon + Quaternion quat_inv = _state.quat_nominal.inversed(); + Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv; + q_error.normalize(); + + // convert the quaternion delta to a delta angle + Vector3f delta_ang_error; + float scalar; + if (q_error(0) >= 0.0f) { + scalar = -2.0f; + + } else { + 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); + + // calculate a gain that provides tight tracking of the estimator attitude states and + // adjust for changes in time delay to mantain 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; + + // 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 gains that will be used to make the INS states converge on the EKF states + 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); + + // calculate velocity and position corrections at the EKF fusion time horizon + Vector3f vel_delta = (_state.vel - _output_sample_delayed.vel) * vel_gain; + Vector3f pos_delta = (_state.pos - _output_sample_delayed.pos) * pos_gain; + + // loop through the output filter state history and apply the corrections to the translational states + // this method is too expensive to use for the attitude states due to the quaternion operations required + // but does not introudce a time dela in the 'correction loop' and allows smaller trackin gtime constants + // to be used + outputSample output_states; + unsigned output_length = _output_buffer.get_length(); + for (unsigned i=0; i < output_length; i++) { + output_states = _output_buffer.get_from_index(i); + output_states.vel += vel_delta; + output_states.pos += pos_delta; + _output_buffer.push_to_index(i,output_states); + } + + // update output state to corrected values + _output_new = _output_buffer.get_newest(); + } - - // get the oldest INS state data from the ring buffer - // this data will be at the EKF fusion time horizon - _output_sample_delayed = _output_buffer.get_oldest(); - - // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon - Quaternion quat_inv = _state.quat_nominal.inversed(); - Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv; - q_error.normalize(); - - // convert the quaternion delta to a delta angle - Vector3f delta_ang_error; - float scalar; - if (q_error(0) >= 0.0f) { - scalar = -2.0f; - - } else { - 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); - - // calculate gains that provides tight tracking of the estimator states and - // adjust for changes in time delay to mantain consistent overshoot - float omega = 1e6f / (_imu_sample_new.time_us - _imu_sample_delayed.time_us); - - // calculate a corrrection to the delta angle - // that will cause the INS to track the EKF quaternions - _delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt * omega * 0.5f; - - // calculate a correction to the delta velocity - // that will cause the INS to track the EKF velocity - _delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt * omega * 0.5f; - - // calculate a correction to the INS velocity - // that will cause the INS to track the EKF position - _vel_corr = (_state.pos - _output_sample_delayed.pos) * omega * 0.6f; } diff --git a/EKF/ekf.h b/EKF/ekf.h index 7c12ac7475..85d85d94c6 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -192,8 +192,6 @@ private: // complementary filter states Vector3f _delta_angle_corr; // delta angle correction vector - Vector3f _delta_vel_corr; // delta velocity correction vector - Vector3f _vel_corr; // velocity correction vector imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate) Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps)