From fed4a9bc5a7674d409272a4470b0cb417b2b84df Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 23 Apr 2017 09:04:00 +1000 Subject: [PATCH] EKF: add vertical position derivative output Add calculation of a vertical position derivative to the output predictor. This will have degraded tracking relative to the EKF states, but the velocity will be closer to the first derivative of the position and reduce the effect inertial prediction errors on control loops that are operating in a pure velocity feedback mode. Move calculation of IMU offset angular rate correction out of velocity accessor and into output predictor. Provide separate accessor for vertical position derivative. --- EKF/RingBuffer.h | 5 ++ EKF/common.h | 13 +++- EKF/ekf.cpp | 146 +++++++++++++++++++++++++++++------- EKF/estimator_interface.cpp | 4 +- EKF/estimator_interface.h | 32 ++++---- 5 files changed, 155 insertions(+), 45 deletions(-) diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index 4194f78d4a..6d5d7650a1 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -113,6 +113,11 @@ public: return _buffer[_tail]; } + unsigned get_oldest_index() + { + return _tail; + } + inline data_type get_newest() { return _buffer[_head]; diff --git a/EKF/common.h b/EKF/common.h index 6d6537e7c4..45b3a7559d 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -79,9 +79,16 @@ struct ext_vision_message { struct outputSample { Quaternion quat_nominal; // nominal quaternion describing vehicle attitude - Vector3f vel; // NED velocity estimate in earth frame in m/s - Vector3f pos; // NED position estimate in earth frame in m/s - uint64_t time_us; // timestamp in microseconds + Vector3f vel; // NED velocity estimate in earth frame in m/s + Vector3f pos; // NED position estimate in earth frame in m/s + uint64_t time_us; // timestamp in microseconds +}; + +struct outputVert { + float vel_d; // D velocity calculated using alternative algorithm + float vel_d_integ; // Integral of vel_d + float dt; // delta time in seconds + uint64_t time_us; // timestamp in microseconds }; struct imuSample { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index e46be27fcb..2c35687f86 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -478,10 +478,10 @@ bool Ekf::collect_imu(imuSample &imu) */ void Ekf::calculateOutputStates() { - // use latest IMU data + // Use full rate IMU data at the current time horizon imuSample imu_new = _imu_sample_new; - // correct delta angles for bias offsets and scale factors + // 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; @@ -528,19 +528,38 @@ void Ekf::calculateOutputStates() 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_NED; + _output_vert_new.vel_d += delta_vel_NED(2); // use trapezoidal integration to calculate the INS position states - _output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f); + // 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); + _output_new.pos += delta_pos_NED; + _output_vert_new.vel_d_integ += delta_pos_NED(2); - // store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer + // accumulate the time for each update + _output_vert_new.dt += imu_new.delta_vel_dt; + + // 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); + + // calculate the velocity of the IMU relative to the body origin + 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 _output_sample_delayed = _output_buffer.get_oldest(); + _output_vert_delayed = _output_vert_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(); @@ -581,37 +600,110 @@ void Ekf::calculateOutputStates() _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 + /* + * 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 becasue 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 float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f); - _vel_err_integ += vel_err; - 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 float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f); - _pos_err_integ += pos_err; - Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f; + { + /* + * Calculate a correction to be applied to vel_d that casues vel_d_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 vel_d + * state history and propagates vel_d_integ forward in time using the corrected vel_d 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. + */ - // 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 does not introduce a time delay in the 'correction loop' and allows smaller tracking time constants - // to be used - outputSample output_states = {}; - unsigned max_index = _output_buffer.get_length() - 1; + // 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); - for (unsigned index = 0; index <= max_index; index++) { - output_states = _output_buffer.get_from_index(index); + // 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; - // a constant velocity correction is applied - output_states.vel += 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. + */ - // a constant position correction is applied - output_states.pos += pos_correction; + // loop through the vertical output filter state history starting at the oldest and apply the corrections to the + // vel_d states and propagate vel_d_integ forward using the corrected vel_d + outputVert current_state; + outputVert next_state; + unsigned index = _output_vert_buffer.get_oldest_index(); + unsigned index_next; + unsigned size = _output_vert_buffer.get_length(); + for (unsigned counter=0; counter < (size - 1); counter++) { + index_next = (index + 1) % size; + current_state = _output_vert_buffer.get_from_index(index); + next_state = _output_vert_buffer.get_from_index(index_next); - // push the updated data to the buffer - _output_buffer.push_to_index(index, output_states); + // correct the velocity + if (counter == 0) { + current_state.vel_d += vel_d_correction; + _output_vert_buffer.push_to_index(index,current_state); + } + next_state.vel_d += vel_d_correction; + + // position is propagated forward using the corrected velocity and a trapezoidal integrator + next_state.vel_d_integ = current_state.vel_d_integ + (current_state.vel_d + next_state.vel_d) * 0.5f * next_state.dt; + + // push the updated data to the buffer + _output_vert_buffer.push_to_index(index_next,next_state); + + // 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; } - // update output state to corrected values - _output_new = _output_buffer.get_newest(); + { + /* + * 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. + */ + + // 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; + + // 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; + + // loop through the output filter state history and apply the corrections to the velocity and position states + outputSample output_states; + unsigned max_index = _output_buffer.get_length() - 1; + for (unsigned index=0; index <= max_index; index++) { + output_states = _output_buffer.get_from_index(index); + + // a constant velocity correction is applied + output_states.vel += vel_correction; + + // a constant position correction is applied + output_states.pos += pos_correction; + + // push the updated data to the buffer + _output_buffer.push_to_index(index,output_states); + + } + + // update output state to corrected values + _output_new = _output_buffer.get_newest(); + } } } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 94a51b9cdc..34a4bc8f5c 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -379,7 +379,8 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _flow_buffer.allocate(_obs_buffer_length) && _ext_vision_buffer.allocate(_obs_buffer_length) && _drag_buffer.allocate(_obs_buffer_length) && - _output_buffer.allocate(_imu_buffer_length))) { + _output_buffer.allocate(_imu_buffer_length) && + _output_vert_buffer.allocate(_imu_buffer_length))) { ECL_ERR("EKF buffer allocation failed!"); unallocate_buffers(); return false; @@ -448,6 +449,7 @@ void EstimatorInterface::unallocate_buffers() _flow_buffer.unallocate(); _ext_vision_buffer.unallocate(); _output_buffer.unallocate(); + _output_vert_buffer.unallocate(); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index dc5b2874b4..a8e61effc1 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -200,30 +200,29 @@ public: // return true if the local position estimate is valid bool local_position_is_valid(); - void copy_quaternion(float *quat) { for (unsigned i = 0; i < 4; i++) { quat[i] = _output_new.quat_nominal(i); } } + // get the velocity of the body frame origin in local NED earth frame void get_velocity(float *vel) { - // 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); - // calculate the velocity of the relative to the body origin - // Note % operator has been overloaded to performa cross product - Vector3f vel_imu_rel_body = cross_product(ang_rate, _params.imu_pos_body); - // rotate the relative velocty into earth frame and subtract from the EKF velocity - // (which is at the IMU) to get velocity of the body origin - Vector3f vel_earth = _output_new.vel - _R_to_earth_now * vel_imu_rel_body; - - // copy to output + Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned; for (unsigned i = 0; i < 3; i++) { vel[i] = vel_earth(i); } } + + // get the derivative of the vertical position of the body frame origin in local NED earth frame + void get_pos_d_deriv(float *pos_d_deriv) + { + float var = _output_vert_new.vel_d - _vel_imu_rel_body_ned(2); + *pos_d_deriv = var; + } + // get the position of the body frame origin in local NED earth frame void get_position(float *pos) { @@ -333,10 +332,14 @@ protected: float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec) float _air_density{1.225f}; // air density (kg/m**3) + // Output Predictor outputSample _output_sample_delayed{}; // filter output on the delayed time horizon - outputSample _output_new{}; // filter output on the non-delayed time horizon - imuSample _imu_sample_new{}; // imu sample capturing the newest imu data - Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time + outputSample _output_new{}; // filter output on the non-delayed time horizon + outputVert _output_vert_delayed{}; // vertical filter output on the delayed time horizon + outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon + imuSample _imu_sample_new{}; // 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 uint64_t _imu_ticks{0}; // counter for imu updates @@ -381,6 +384,7 @@ protected: RingBuffer _flow_buffer; RingBuffer _ext_vision_buffer; RingBuffer _output_buffer; + RingBuffer _output_vert_buffer; RingBuffer _drag_buffer; uint64_t _time_last_imu{0}; // timestamp of last imu sample in microseconds