fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 07:07:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Make delayed output stayed not a member variable

Browse Source
This commit is contained in:
kamilritz
2020-07-26 11:44:00 +02:00
committed by Paul Riseborough
parent da9bfe4316
commit 3bd9415498
3 changed files with 15 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+10 -7
View File
@@ -65,6 +65,7 @@ void Ekf::reset()
_state.wind_vel.setZero(); _state.wind_vel.setZero();
_state.quat_nominal.setIdentity(); _state.quat_nominal.setIdentity();
// TODO: who resets the output buffer content?
_output_new.vel.setZero(); _output_new.vel.setZero();
_output_new.pos.setZero(); _output_new.pos.setZero();
_output_new.quat_nominal.setIdentity(); _output_new.quat_nominal.setIdentity();
@@ -394,11 +395,13 @@ void Ekf::calculateOutputStates()
// get the oldest INS state data from the ring buffer // get the oldest INS state data from the ring buffer
// this data will be at the EKF fusion time horizon // this data will be at the EKF fusion time horizon
_output_sample_delayed = _output_buffer.get_oldest(); // TODO: there is no guarantee that data is at delayed fusion horizon
_output_vert_delayed = _output_vert_buffer.get_oldest(); // 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 // 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_sample_delayed.quat_nominal).normalized() ); const Quatf q_error( (_state.quat_nominal.inversed() * output_delayed.quat_nominal).normalized() );
// convert the quaternion delta to a delta angle // convert the quaternion delta to a delta angle
const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f; const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f;
@@ -427,8 +430,8 @@ void Ekf::calculateOutputStates()
const float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_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 // calculate down velocity and position tracking errors
const float vert_vel_err = (_state.vel(2) - _output_vert_delayed.vert_vel); 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); 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 // calculate a velocity correction that will be applied to the output state history
// using a PD feedback tuned to a 5% overshoot // using a PD feedback tuned to a 5% overshoot
@@ -437,8 +440,8 @@ void Ekf::calculateOutputStates()
applyCorrectionToVerticalOutputBuffer(vert_vel_correction); applyCorrectionToVerticalOutputBuffer(vert_vel_correction);
// calculate velocity and position tracking errors // calculate velocity and position tracking errors
const Vector3f vel_err(_state.vel - _output_sample_delayed.vel); const Vector3f vel_err(_state.vel - output_delayed.vel);
const Vector3f pos_err(_state.pos - _output_sample_delayed.pos); const Vector3f pos_err(_state.pos - output_delayed.pos);
_output_tracking_error(1) = vel_err.norm(); _output_tracking_error(1) = vel_err.norm();
_output_tracking_error(2) = pos_err.norm(); _output_tracking_error(2) = pos_err.norm();
EKF/ekf_helper.cpp
+5 -8
View File
@@ -141,7 +141,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel) {
_output_vert_buffer[index].vert_vel += delta_vert_vel; _output_vert_buffer[index].vert_vel += delta_vert_vel;
} }
_output_new.vel(2) += delta_vert_vel; _output_new.vel(2) += delta_vert_vel;
_output_vert_delayed.vert_vel = new_vert_vel;
_output_vert_new.vert_vel += delta_vert_vel; _output_vert_new.vert_vel += delta_vert_vel;
_state_reset_status.velD_change = delta_vert_vel; _state_reset_status.velD_change = delta_vert_vel;
@@ -330,7 +329,6 @@ void Ekf::resetHeight()
// add the reset amount to the output observer vertical position state // add the reset amount to the output observer vertical position state
if (vert_pos_reset) { if (vert_pos_reset) {
_output_vert_delayed.vert_vel_integ = _state.pos(2);
_output_vert_new.vert_vel_integ = _state.pos(2); _output_vert_new.vert_vel_integ = _state.pos(2);
} }
} }
@@ -338,13 +336,14 @@ void Ekf::resetHeight()
// align output filter states to match EKF states at the fusion time horizon // align output filter states to match EKF states at the fusion time horizon
void Ekf::alignOutputFilter() 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 // 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_sample_delayed.quat_nominal.inversed(); Quatf q_delta = _state.quat_nominal * output_delayed.quat_nominal.inversed();
q_delta.normalize(); q_delta.normalize();
// calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon // calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon
const Vector3f vel_delta = _state.vel - _output_sample_delayed.vel; const Vector3f vel_delta = _state.vel - output_delayed.vel;
const Vector3f pos_delta = _state.pos - _output_sample_delayed.pos; const Vector3f pos_delta = _state.pos - output_delayed.pos;
// loop through the output filter state history and add the deltas // loop through the output filter state history and add the deltas
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
@@ -356,9 +355,7 @@ void Ekf::alignOutputFilter()
_output_new.quat_nominal = q_delta * _output_new.quat_nominal; _output_new.quat_nominal = q_delta * _output_new.quat_nominal;
_output_new.quat_nominal.normalize(); _output_new.quat_nominal.normalize();
// TODO: what about vel and pos of _output_new, shouldn't they be aligned too?
_output_sample_delayed.quat_nominal = q_delta * _output_sample_delayed.quat_nominal;
_output_sample_delayed.quat_nominal.normalize();
} }
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS. // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
EKF/estimator_interface.h
-2
View File
@@ -456,9 +456,7 @@ protected:
float _flow_max_distance{0.0f}; ///< maximum distance that the optical flow sensor can operate at (m) float _flow_max_distance{0.0f}; ///< maximum distance that the optical flow sensor can operate at (m)
// Output Predictor // Output Predictor
outputSample _output_sample_delayed{}; // filter output on the delayed time horizon
outputSample _output_new{}; // filter output on the non-delayed time horizon 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 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 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 Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time