Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ekf2: cleanup output predictor init #20960

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
+982 −800
Open

ekf2: cleanup output predictor init #20960

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b172705
ekf2: cleanup output predictor init
dagar Jan 18, 2023
0290a61
Merge remote-tracking branch 'px4/main' into pr-ekf2_output_predictor…
dagar Oct 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 18 additions & 17 deletions src/modules/ekf2/EKF/ekf.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,8 @@ void Ekf::reset()
_output_predictor.reset();

// Ekf private fields
_filter_initialised = false;

_time_last_horizontal_aiding = 0;
_time_last_v_pos_aiding = 0;
_time_last_v_vel_aiding = 0;
Expand Down Expand Up @@ -161,11 +163,11 @@ void Ekf::reset()

bool Ekf::update()
{
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
bool updated = false;

if (!_filter_initialised) {
return false;
if (!_filter_initialised) {
if (initialiseFilter()) {
_filter_initialised = true;
}
}

Expand All @@ -177,24 +179,26 @@ 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);

#if defined(CONFIG_EKF2_RANGE_FINDER)
// run a separate filter for terrain estimation
runTerrainEstimator(imu_sample_delayed);
// run a separate filter for terrain estimation
runTerrainEstimator(imu_sample_delayed);
#endif // CONFIG_EKF2_RANGE_FINDER

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

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

return false;
return updated;
}

bool Ekf::initialiseFilter()
Expand Down Expand Up @@ -229,9 +233,6 @@ bool Ekf::initialiseFilter()
initHagl();
#endif // CONFIG_EKF2_RANGE_FINDER

// 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
2 changes: 1 addition & 1 deletion src/modules/ekf2/EKF/ekf.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -809,7 +809,7 @@ class Ekf final : public EstimatorInterface
// fuse single velocity and position measurement
bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);

void resetVelocityTo(const Vector3f &vel, const Vector3f &new_vel_var);
void resetVelocityTo(const Vector3f &vel, const Vector3f &new_vel_var = Vector3f(NAN, NAN, NAN));

void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var);
void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, float vel_var) { resetHorizontalVelocityTo(new_horz_vel, Vector2f(vel_var, vel_var)); }
Expand Down
28 changes: 20 additions & 8 deletions src/modules/ekf2/EKF/ekf_helper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,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(_time_delayed_us, new_horz_vel);

// record the state change
if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
Expand All @@ -99,7 +99,7 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
P.uncorrelateCovarianceSetVariance<1>(6, math::max(sq(0.01f), new_vert_vel_var));
}

_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
_output_predictor.resetVerticalVelocityTo(_time_delayed_us, new_vert_vel);

// record the state change
if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
Expand Down Expand Up @@ -139,7 +139,7 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
P.uncorrelateCovarianceSetVariance<1>(8, math::max(sq(0.01f), new_horz_pos_var(1)));
}

_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
_output_predictor.resetHorizontalPositionTo(_time_delayed_us, new_horz_pos);

// record the state change
if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
Expand Down Expand Up @@ -174,19 +174,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(_time_delayed_us, new_vert_pos);

// record the state change
if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
Expand Down Expand Up @@ -1069,7 +1067,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
}

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

#if defined(CONFIG_EKF2_EXTERNAL_VISION)
// update EV attitude error filter
Expand All @@ -1092,6 +1090,20 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
_state_reset_status.reset_count.quat++;

_time_last_heading_fuse = _time_delayed_us;


// apply change to velocity state
const Vector3f new_vel = q_error.rotateVector(_state.vel);
const Vector3f delta_vel = new_vel - _state.vel;
_state.vel = new_vel;

if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
_state_reset_status.velNE_change = delta_vel.xy();

} else {
// there's already a reset this update, accumulate total delta
_state_reset_status.velNE_change += delta_vel.xy();
}
}

bool Ekf::resetYawToEKFGSF()
Expand Down
Loading