fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 03:04:07 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
EKF/ekf.cpp
View File

@ -65,6 +65,7 @@ void Ekf::reset()
_state.wind_vel.setZero();
_state.quat_nominal.setIdentity();
// TODO: who resets the output buffer content?
_output_new.vel.setZero();
_output_new.pos.setZero();
_output_new.quat_nominal.setIdentity();
@ -394,11 +395,13 @@ void Ekf::calculateOutputStates()
// 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();
// TODO: there is no guarantee that data is at delayed fusion horizon
// 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
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
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);
// calculate down velocity and position tracking errors
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_err = (_state.vel(2) - output_vert_delayed.vert_vel);
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
// using a PD feedback tuned to a 5% overshoot
@ -437,8 +440,8 @@ void Ekf::calculateOutputStates()
applyCorrectionToVerticalOutputBuffer(vert_vel_correction);
// calculate velocity and position tracking errors
const Vector3f vel_err(_state.vel - _output_sample_delayed.vel);
const Vector3f pos_err(_state.pos - _output_sample_delayed.pos);
const Vector3f vel_err(_state.vel - output_delayed.vel);
const Vector3f pos_err(_state.pos - output_delayed.pos);
_output_tracking_error(1) = vel_err.norm();
_output_tracking_error(2) = pos_err.norm();

13
EKF/ekf_helper.cpp
View File

@ -141,7 +141,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel) {
_output_vert_buffer[index].vert_vel += 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;
_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
if (vert_pos_reset) {
_output_vert_delayed.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
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
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();
// 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 pos_delta = _state.pos - _output_sample_delayed.pos;
const Vector3f vel_delta = _state.vel - output_delayed.vel;
const Vector3f pos_delta = _state.pos - output_delayed.pos;
// loop through the output filter state history and add the deltas
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.normalize();
_output_sample_delayed.quat_nominal = q_delta * _output_sample_delayed.quat_nominal;
_output_sample_delayed.quat_nominal.normalize();
// TODO: what about vel and pos of _output_new, shouldn't they be aligned too?
}
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.

2
EKF/estimator_interface.h
View File

@ -456,9 +456,7 @@ protected:
float _flow_max_distance{0.0f}; ///< maximum distance that the optical flow sensor can operate at (m)
// Output Predictor
outputSample _output_sample_delayed{}; // filter output on the 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
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