Skip to content

Commit

Permalink
ekf2: cleanup output predictor init
Browse files Browse the repository at this point in the history
 - output predictor align also init _output_vert_new
 - output predictor reset handle all fields
 - output predictor resetQuaternion() normalize
 - output predictor calculateOutputStates() paranoid timestamp check
 - output predictor correctOutputStates() paranoid timestamp checks and
explicitly pop delayed sample at correct time
  • Loading branch information
dagar committed Jan 27, 2023
1 parent 48f2b42 commit 284073f
Show file tree
Hide file tree
Showing 8 changed files with 799 additions and 655 deletions.
37 changes: 20 additions & 17 deletions src/modules/ekf2/EKF/ekf.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -81,15 +81,19 @@ void Ekf::reset()
resetGpsDriftCheckFilters();

_output_predictor.reset();

_filter_initialised = false;
}

bool Ekf::update()
{
bool updated = false;

if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
if (initialiseFilter()) {
_filter_initialised = true;

if (!_filter_initialised) {
return false;
_output_predictor.resetAlignment();
}
}

Expand All @@ -101,23 +105,25 @@ bool Ekf::update()
// TODO: explicitly pop at desired time horizon
const imuSample imu_sample_delayed = _imu_buffer.get_oldest();

// perform state and covariance prediction for the main filter
predictCovariance(imu_sample_delayed);
predictState(imu_sample_delayed);
if (_filter_initialised) {
// perform state and covariance prediction for the main filter
predictCovariance(imu_sample_delayed);
predictState(imu_sample_delayed);

// control fusion of observation data
controlFusionModes(imu_sample_delayed);
// control fusion of observation data
controlFusionModes(imu_sample_delayed);

// run a separate filter for terrain estimation
runTerrainEstimator(imu_sample_delayed);
// run a separate filter for terrain estimation
runTerrainEstimator(imu_sample_delayed);

_output_predictor.correctOutputStates(imu_sample_delayed.time_us, getGyroBias(), getAccelBias(),
_state.quat_nominal, _state.vel, _state.pos);
updated = true;
}

return true;
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, getGyroBias(), getAccelBias(),
_state.quat_nominal, _state.vel, _state.pos);
}

return false;
return updated;
}

bool Ekf::initialiseFilter()
Expand Down Expand Up @@ -203,9 +209,6 @@ bool Ekf::initialiseFilter()
_time_last_hor_pos_fuse = _time_delayed_us;
_time_last_hor_vel_fuse = _time_delayed_us;

// reset the output predictor state history to match the EKF initial values
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);

return true;
}

Expand Down
13 changes: 7 additions & 6 deletions src/modules/ekf2/EKF/ekf_helper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f
P.uncorrelateCovarianceSetVariance<1>(5, math::max(sq(0.01f), new_horz_vel_var(1)));
}

_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
_output_predictor.resetHorizontalVelocityTo(new_horz_vel);

// record the state change
if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
Expand Down Expand Up @@ -168,19 +168,17 @@ bool Ekf::isHeightResetRequired() const

void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
{
const float old_vert_pos = _state.pos(2);
const float delta_z = new_vert_pos - _state.pos(2);
_state.pos(2) = new_vert_pos;

if (PX4_ISFINITE(new_vert_pos_var)) {
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(9, math::max(sq(0.01f), new_vert_pos_var));
}

const float delta_z = new_vert_pos - old_vert_pos;

// apply the change in height / height rate to our newest height / height rate estimate
// which have already been taken out from the output buffer
_output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z);
_output_predictor.resetVerticalPositionTo(new_vert_pos);

// record the state change
if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
Expand Down Expand Up @@ -1292,8 +1290,11 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
increaseQuatYawErrVariance(yaw_variance);
}

// apply change to velocity state
_state.vel = q_error.rotateVector(_state.vel);

// add the reset amount to the output observer buffered data
_output_predictor.resetQuaternion(q_error);
_output_predictor.resetQuaternion(quat_after_reset);

// record the state change
if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) {
Expand Down
2 changes: 1 addition & 1 deletion src/modules/ekf2/EKF/estimator_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -448,7 +448,7 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
_system_flag_buffer->push(system_flags_new);

} else {
ECL_WARN("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
//ECL_WARN("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
}
}

Expand Down
Loading

0 comments on commit 284073f

Please sign in to comment.