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