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

Update interface of applyCorrectionsToOuputStates

Browse Source
This commit is contained in:
kamilritz
2020-07-26 11:41:41 +02:00
committed by Paul Riseborough
parent 64ab2b087a
commit da9bfe4316
2 changed files with 32 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+30 -27
View File
@@ -426,8 +426,32 @@ void Ekf::calculateOutputStates()
const float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f); const float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
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);
applyCorrectionToVerticalOutputBuffer(vel_gain, pos_gain); // calculate down velocity and position tracking errors
applyCorrectionToOutputBuffer(vel_gain, pos_gain); 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
const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f;
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);
_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
_vel_err_integ += vel_err;
const 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;
const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
applyCorrectionToOutputBuffer(vel_correction, pos_correction);
} }
} }
@@ -439,15 +463,8 @@ void Ekf::calculateOutputStates()
* This provides an alternative vertical velocity output that is closer to the first derivative * 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. * of the position but does degrade tracking relative to the EKF state.
*/ */
void Ekf::applyCorrectionToVerticalOutputBuffer(float vel_gain, float pos_gain){ void Ekf::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction)
// 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);
// calculate a velocity correction that will be applied to the output state history
// using a PD feedback tuned to a 5% overshoot
const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f;
// loop through the vertical output filter state history starting at the oldest and apply the corrections to the // loop through the vertical output filter state history starting at the oldest and apply the corrections to the
// vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel // vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel
uint8_t index = _output_vert_buffer.get_oldest_index(); uint8_t index = _output_vert_buffer.get_oldest_index();
@@ -485,22 +502,8 @@ void Ekf::applyCorrectionToVerticalOutputBuffer(float vel_gain, float pos_gain){
* The vel and pos state history are corrected individually so they track the EKF states at * 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. * the fusion time horizon. This option provides the most accurate tracking of EKF states.
*/ */
void Ekf::applyCorrectionToOutputBuffer(float vel_gain, float pos_gain){ void Ekf::applyCorrectionToOutputBuffer(const Vector3f& vel_correction, const Vector3f& pos_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);
_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
_vel_err_integ += vel_err;
const 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;
const 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 // loop through the output filter state history and apply the corrections to the velocity and position states
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
// a constant velocity correction is applied // a constant velocity correction is applied
EKF/ekf.h
+2 -2
View File
@@ -518,8 +518,8 @@ private:
// update the real time complementary filter states. This includes the prediction // update the real time complementary filter states. This includes the prediction
// and the correction step // and the correction step
void calculateOutputStates(); void calculateOutputStates();
void applyCorrectionToVerticalOutputBuffer(float vel_gain, float pos_gain); void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction);
void applyCorrectionToOutputBuffer(float vel_gain, float pos_gain); void applyCorrectionToOutputBuffer(const Vector3f& vel_correction, const Vector3f& pos_correction);
// initialise filter states of both the delayed ekf and the real time complementary filter // initialise filter states of both the delayed ekf and the real time complementary filter
bool initialiseFilter(void); bool initialiseFilter(void);