fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 15:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Refactor velocity reset (#788)

Browse Source 
* Refactor velocity reset

* Add unit tests for velocity resets

* Expand updates to vertical buffer to velocity resets outside of resetHeight

* Improve matrix library usage

* Improve naming of vertical output samples

* Fix update of output_vert_new during reset

* Improve naming of vertical output samples 2
This commit is contained in:
kritz
2020-04-08 16:39:15 +02:00
committed by GitHub
parent 47624a0f02
commit fa5a00d871
13 changed files with 259 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+2 -2
View File
@@ -81,8 +81,8 @@ struct outputSample {
};
struct outputVert {
float vel_d; ///< D velocity calculated using alternative algorithm (m/sec)
float vel_d_integ; ///< Integral of vel_d (m)
float vert_vel; ///< Vertical velocity calculated using alternative algorithm (m/sec)
float vert_vel_integ; ///< Integral of vertical velocity (m)
float dt; ///< delta time (sec)
uint64_t time_us; ///< timestamp of the measurement (uSec)
};
EKF/control.cpp
+3
View File
@@ -496,6 +496,9 @@ void Ekf::controlOpticalFlowFusion()
// if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set
const bool flow_aid_only = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow);
if (flow_aid_only) {
// TODO: Issue: First time we want to reset the optical flow velocity
// we will use _flow_compensated_XY_rad in the resetVelocity() function,
// but _flow_compensated_XY_rad is this zero as it gets updated for the first time below here.
resetVelocity();
resetPosition();
EKF/ekf.cpp
+12 -12
View File
@@ -369,13 +369,13 @@ void Ekf::calculateOutputStates()
// 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_earth;
_output_vert_new.vel_d += delta_vel_earth(2);
_output_vert_new.vert_vel += delta_vel_earth(2);
// use trapezoidal integration to calculate the INS position states
// do the same for vertical state used by alternative correction algorithm
const Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (imu.delta_vel_dt * 0.5f);
_output_new.pos += delta_pos_NED;
_output_vert_new.vel_d_integ += delta_pos_NED(2);
_output_vert_new.vert_vel_integ += delta_pos_NED(2);
// accumulate the time for each update
_output_vert_new.dt += imu.delta_vel_dt;
@@ -447,21 +447,21 @@ void Ekf::calculateOutputStates()
const float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
{
/*
* Calculate a correction to be applied to vel_d that casues vel_d_integ to track the EKF
* Calculate a correction to be applied to vert_vel that casues vert_vel_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.
* is used for the vel and pos state tracking. The algorithm applies a correction to the vert_vel
* state history and propagates vert_vel_integ forward in time using the corrected vert_vel 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.
*/
// calculate down velocity and position tracking errors
const float vel_d_err = (_state.vel(2) - _output_vert_delayed.vel_d);
const float pos_d_err = (_state.pos(2) - _output_vert_delayed.vel_d_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
const float vel_d_correction = pos_d_err * pos_gain + vel_d_err * pos_gain * 1.1f;
const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * pos_gain * 1.1f;
/*
* Calculate corrections to be applied to vel and pos output state history.
@@ -470,7 +470,7 @@ void Ekf::calculateOutputStates()
*/
// 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
// vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel
uint8_t index = _output_vert_buffer.get_oldest_index();
const uint8_t size = _output_vert_buffer.get_length();
@@ -482,13 +482,13 @@ void Ekf::calculateOutputStates()
// correct the velocity
if (counter == 0) {
current_state.vel_d += vel_d_correction;
current_state.vert_vel += vert_vel_correction;
}
next_state.vel_d += vel_d_correction;
next_state.vert_vel += vert_vel_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;
next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * next_state.dt;
// advance the index
index = (index + 1) % size;
EKF/ekf.h
+6
View File
@@ -577,6 +577,12 @@ private:
// reset velocity states of the ekf
bool resetVelocity();
void resetVelocityTo(const Vector3f &vel);
inline void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);
inline void resetVerticalVelocityTo(float new_vert_vel);
// fuse optical flow line of sight rate measurements
void fuseOptFlow();
EKF/ekf_helper.cpp
+43 -59
View File
@@ -49,16 +49,11 @@
// gps measurement then use for velocity initialisation
bool Ekf::resetVelocity()
{
// used to calculate the velocity change due to the reset
const Vector3f vel_before_reset = _state.vel;
// reset EKF states
if (_control_status.flags.gps && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
ECL_INFO_TIMESTAMPED("reset velocity to GPS");
// this reset is only called if we have new gps data at the fusion time horizon
_state.vel = _gps_sample_delayed.vel;
resetVelocityTo(_gps_sample_delayed.vel);
// use GPS accuracy to reset variances
P.uncorrelateCovarianceSetVariance<3>(4, sq(_gps_sample_delayed.sacc));
} else if (_control_status.flags.opt_flow) {
@@ -78,16 +73,11 @@ bool Ekf::resetVelocity()
vel_optflow_body(2) = 0.0f;
// rotate from body to earth frame
Vector3f vel_optflow_earth;
vel_optflow_earth = _R_to_earth * vel_optflow_body;
// take x and Y components
_state.vel(0) = vel_optflow_earth(0);
_state.vel(1) = vel_optflow_earth(1);
const Vector3f vel_optflow_earth = _R_to_earth * vel_optflow_body;
resetHorizontalVelocityTo(Vector2f(vel_optflow_earth));
} else {
_state.vel(0) = 0.0f;
_state.vel(1) = 0.0f;
resetHorizontalVelocityTo(Vector2f{0.f, 0.f});
}
// reset the horizontal velocity variance using the optical flow noise variance
@@ -99,35 +89,53 @@ bool Ekf::resetVelocity()
if(_params.fusion_mode & MASK_ROTATE_EV){
_ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel;
}
_state.vel = _ev_vel;
resetVelocityTo(_ev_vel);
P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velVar);
} else {
ECL_INFO_TIMESTAMPED("reset velocity to zero");
// Used when falling back to non-aiding mode of operation
_state.vel(0) = 0.0f;
_state.vel(1) = 0.0f;
resetHorizontalVelocityTo(Vector2f{0.f, 0.f});
P.uncorrelateCovarianceSetVariance<2>(4, 25.0f);
}
// calculate the change in velocity and apply to the output predictor state history
const Vector3f velocity_change = _state.vel - vel_before_reset;
return true;
}
void Ekf::resetVelocityTo(const Vector3f &new_vel) {
resetHorizontalVelocityTo(Vector2f(new_vel));
resetVerticalVelocityTo(new_vel(2));
}
void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel) {
const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel);
_state.vel.xy() = new_horz_vel;
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
_output_buffer[index].vel += velocity_change;
_output_buffer[index].vel(0) += delta_horz_vel(0);
_output_buffer[index].vel(1) += delta_horz_vel(1);
}
_output_new.vel(0) += delta_horz_vel(0);
_output_new.vel(1) += delta_horz_vel(1);
// apply the change in velocity to our newest velocity estimate
// which was already taken out from the output buffer
_output_new.vel += velocity_change;
// capture the reset event
_state_reset_status.velNE_change(0) = velocity_change(0);
_state_reset_status.velNE_change(1) = velocity_change(1);
_state_reset_status.velD_change = velocity_change(2);
_state_reset_status.velNE_change = delta_horz_vel;
_state_reset_status.velNE_counter++;
_state_reset_status.velD_counter++;
}
return true;
void Ekf::resetVerticalVelocityTo(float new_vert_vel) {
const float delta_vert_vel = new_vert_vel - _state.vel(2);
_state.vel(2) = new_vert_vel;
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
_output_buffer[index].vel(2) += delta_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;
_state_reset_status.velD_counter++;
}
// Reset position states. If we have a recent and valid
@@ -222,8 +230,6 @@ void Ekf::resetHeight()
// store the current vertical position and velocity for reference so we can calculate and publish the reset amount
float old_vert_pos = _state.pos(2);
bool vert_pos_reset = false;
float old_vert_vel = _state.vel(2);
bool vert_vel_reset = false;
// reset the vertical position
if (_control_status.flags.rng_hgt) {
@@ -297,66 +303,44 @@ void Ekf::resetHeight()
// reset the vertical velocity state
if (_control_status.flags.gps && !_gps_hgt_intermittent) {
// If we are using GPS, then use it to reset the vertical velocity
_state.vel(2) = gps_newest.vel(2);
resetVerticalVelocityTo(gps_newest.vel(2));
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_newest.sacc));
} else {
// we don't know what the vertical velocity is, so set it to zero
_state.vel(2) = 0.0f;
resetVerticalVelocityTo(0.0f);
// Set the variance to a value large enough to allow the state to converge quickly
// that does not destabilise the filter
P.uncorrelateCovarianceSetVariance<1>(6, 10.0f);
}
vert_vel_reset = true;
// store the reset amount and time to be published
if (vert_pos_reset) {
_state_reset_status.posD_change = _state.pos(2) - old_vert_pos;
_state_reset_status.posD_counter++;
}
if (vert_vel_reset) {
_state_reset_status.velD_change = _state.vel(2) - old_vert_vel;
_state_reset_status.velD_counter++;
}
// apply the change in height / height rate to our newest height / height rate estimate
// which have already been taken out from the output buffer
if (vert_pos_reset) {
_output_new.pos(2) += _state_reset_status.posD_change;
}
if (vert_vel_reset) {
_output_new.vel(2) += _state_reset_status.velD_change;
}
// add the reset amount to the output observer buffered data
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
if (vert_pos_reset) {
_output_buffer[i].pos(2) += _state_reset_status.posD_change;
_output_vert_buffer[i].vel_d_integ += _state_reset_status.posD_change;
}
if (vert_vel_reset) {
_output_buffer[i].vel(2) += _state_reset_status.velD_change;
_output_vert_buffer[i].vel_d += _state_reset_status.velD_change;
_output_vert_buffer[i].vert_vel_integ += _state_reset_status.posD_change;
}
}
// add the reset amount to the output observer vertical position state
if (vert_pos_reset) {
_output_vert_delayed.vel_d_integ = _state.pos(2);
_output_vert_new.vel_d_integ = _state.pos(2);
}
if (vert_vel_reset) {
_output_vert_delayed.vel_d = _state.vel(2);
_output_vert_new.vel_d = _state.vel(2);
_output_vert_delayed.vert_vel_integ = _state.pos(2);
_output_vert_new.vert_vel_integ = _state.pos(2);
}
}
EKF/estimator_interface.h
+1 -1
View File
@@ -302,7 +302,7 @@ public:
// get the derivative of the vertical position of the body frame origin in local NED earth frame
float getVerticalPositionDerivative() const
{
return _output_vert_new.vel_d - _vel_imu_rel_body_ned(2);
return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2);
}
// get the position of the body frame origin in local earth frame