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

ekf2: include 0 timestamp checks in helpers (isTimedOut(), isRecent(), etc)

Browse Source 
- EKF isTimedOut(), isRecent(), and isNewestSampleRecent() need to handle the case where the timestamp has never been set
 - reset() more thoroughly reset fields (mainly impacts unit tests)
This commit is contained in:
Daniel Agar
2023-02-03 10:00:51 -05:00
committed by GitHub
parent 264a99fb77
commit 66ad7fd06c
11 changed files with 834 additions and 767 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/airspeed_fusion.cpp
+5
View File
@@ -157,6 +157,8 @@ void Ekf::resetWindUsingAirspeed()
_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);
ECL_INFO("reset wind using airspeed to (%.3f, %.3f)", (double)_state.wind_vel(0), (double)_state.wind_vel(1));
resetWindCovarianceUsingAirspeed();
_aid_src_airspeed.time_last_fuse = _time_delayed_us;
@@ -164,8 +166,11 @@ void Ekf::resetWindUsingAirspeed()
void Ekf::resetWindToZero()
{
ECL_INFO("reset wind to zero");
// If we don't have an airspeed measurement, then assume the wind is zero
_state.wind_vel.setZero();
// start with a small initial uncertainty to improve the initial estimate
P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
}
src/modules/ekf2/EKF/ekf.cpp
+56 -5
View File
@@ -81,6 +81,62 @@ void Ekf::reset()
resetGpsDriftCheckFilters();
_output_predictor.reset();
// Ekf private fields
_time_last_horizontal_aiding = 0;
_time_last_v_pos_aiding = 0;
_time_last_v_vel_aiding = 0;
_time_last_hor_pos_fuse = 0;
_time_last_hgt_fuse = 0;
_time_last_hor_vel_fuse = 0;
_time_last_ver_vel_fuse = 0;
_time_last_heading_fuse = 0;
_time_last_flow_terrain_fuse = 0;
_time_last_zero_velocity_fuse = 0;
_time_last_healthy_rng_data = 0;
_last_known_pos.setZero();
_time_acc_bias_check = 0;
_gps_checks_passed = false;
_gps_alt_ref = NAN;
_baro_counter = 0;
_mag_counter = 0;
_time_bad_vert_accel = 0;
_time_good_vert_accel = 0;
_clip_counter = 0;
resetEstimatorAidStatus(_aid_src_baro_hgt);
resetEstimatorAidStatus(_aid_src_rng_hgt);
resetEstimatorAidStatus(_aid_src_airspeed);
resetEstimatorAidStatus(_aid_src_sideslip);
resetEstimatorAidStatus(_aid_src_fake_pos);
resetEstimatorAidStatus(_aid_src_fake_hgt);
resetEstimatorAidStatus(_aid_src_ev_hgt);
resetEstimatorAidStatus(_aid_src_ev_pos);
resetEstimatorAidStatus(_aid_src_ev_vel);
resetEstimatorAidStatus(_aid_src_ev_yaw);
resetEstimatorAidStatus(_aid_src_gnss_hgt);
resetEstimatorAidStatus(_aid_src_gnss_pos);
resetEstimatorAidStatus(_aid_src_gnss_vel);
resetEstimatorAidStatus(_aid_src_gnss_yaw);
resetEstimatorAidStatus(_aid_src_mag_heading);
resetEstimatorAidStatus(_aid_src_mag);
resetEstimatorAidStatus(_aid_src_aux_vel);
resetEstimatorAidStatus(_aid_src_optical_flow);
resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
}
bool Ekf::update()
@@ -198,11 +254,6 @@ bool Ekf::initialiseFilter()
// Initialise the terrain estimator
initHagl();
// reset the essential fusion timeout counters
_time_last_hgt_fuse = _time_delayed_us;
_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);
src/modules/ekf2/EKF/ekf.h
+3 -3
View File
@@ -973,17 +973,17 @@ private:
bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const
{
return last_sensor_timestamp + timeout_period < _time_delayed_us;
return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < _time_delayed_us);
}
bool isRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const
{
return sensor_timestamp + acceptance_interval > _time_delayed_us;
return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_delayed_us);
}
bool isNewestSampleRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const
{
return sensor_timestamp + acceptance_interval > _time_latest_us;
return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_latest_us);
}
void startAirspeedFusion();
src/modules/ekf2/EKF/ekf_helper.cpp
+25 -14
View File
@@ -116,8 +116,10 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
void Ekf::resetHorizontalPositionToLastKnown()
{
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
_information_events.flags.reset_pos_to_last_known = true;
ECL_INFO("reset position to last known position");
// Used when falling back to non-aiding mode of operation
resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
}
@@ -242,6 +244,8 @@ bool Ekf::resetMagHeading()
float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
float yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.e-2f));
ECL_INFO("reset mag heading %.3f -> %.3f rad", (double)getEulerYaw(_R_to_earth), (double)yaw_new);
// update quaternion states and corresponding covarainces
resetQuatStateYaw(yaw_new, yaw_new_variance);
@@ -854,8 +858,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
}
// report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present
bool deadreckon_time_exceeded = (_time_last_horizontal_aiding == 0)
|| isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max);
bool deadreckon_time_exceeded = isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max);
if (!_horizontal_deadreckon_time_exceeded && deadreckon_time_exceeded) {
// deadreckon time now exceeded
@@ -871,7 +874,7 @@ void Ekf::updateVerticalDeadReckoningStatus()
_time_last_v_pos_aiding = _time_last_hgt_fuse;
_vertical_position_deadreckon_time_exceeded = false;
} else if ((_time_last_v_pos_aiding == 0) || isTimedOut(_time_last_v_pos_aiding, (uint64_t)_params.valid_timeout_max)) {
} else if (isTimedOut(_time_last_v_pos_aiding, (uint64_t)_params.valid_timeout_max)) {
_vertical_position_deadreckon_time_exceeded = true;
}
@@ -879,8 +882,9 @@ void Ekf::updateVerticalDeadReckoningStatus()
_time_last_v_vel_aiding = _time_last_ver_vel_fuse;
_vertical_velocity_deadreckon_time_exceeded = false;
} else if (((_time_last_v_vel_aiding == 0) || isTimedOut(_time_last_v_vel_aiding, (uint64_t)_params.valid_timeout_max))
} else if (isTimedOut(_time_last_v_vel_aiding, (uint64_t)_params.valid_timeout_max)
&& _vertical_position_deadreckon_time_exceeded) {
_vertical_velocity_deadreckon_time_exceeded = true;
}
}
@@ -1180,20 +1184,27 @@ void Ekf::loadMagCovData()
void Ekf::startAirspeedFusion()
{
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// activate the wind states
_control_status.flags.wind = true;
// reset the wind speed states and corresponding covariances
resetWindUsingAirspeed();
}
if (!_control_status.flags.fuse_aspd) {
ECL_INFO("starting airspeed fusion");
_control_status.flags.fuse_aspd = true;
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// activate the wind states
_control_status.flags.wind = true;
// reset the wind speed states and corresponding covariances
resetWindUsingAirspeed();
}
_control_status.flags.fuse_aspd = true;
}
}
void Ekf::stopAirspeedFusion()
{
_control_status.flags.fuse_aspd = false;
if (_control_status.flags.fuse_aspd) {
ECL_INFO("stopping airspeed fusion");
_control_status.flags.fuse_aspd = false;
}
}
void Ekf::stopGpsFusion()
src/modules/ekf2/EKF/ev_vel_control.cpp
+1 -1
View File
@@ -205,7 +205,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
if (starting_conditions_passing) {
// activate fusion, only reset if necessary
if (!isHorizontalAidingActive() || yaw_alignment_changed) {
ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME, (double)measurement(0), (double)measurement(1), (double)measurement(2));
_information_events.flags.reset_vel_to_vision = true;
resetVelocityTo(measurement, measurement_var);
src/modules/ekf2/EKF/fake_height_control.cpp
+1 -1
View File
@@ -104,7 +104,7 @@ void Ekf::resetFakeHgtFusion()
void Ekf::resetHeightToLastKnown()
{
_information_events.flags.reset_pos_to_last_known = true;
ECL_INFO("reset height to last known");
ECL_INFO("reset height to last known (%.3f)", (double)_last_known_pos(2));
resetVerticalPositionTo(_last_known_pos(2), sq(_params.pos_noaid_noise));
}
src/modules/ekf2/EKF/optical_flow_control.cpp
+2 -2
View File
@@ -179,7 +179,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
} else {
_information_events.flags.reset_pos_to_last_known = true;
ECL_INFO("reset position to last known position");
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);
}
}
@@ -212,7 +212,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
// reset position, estimate is relative to initial position in this mode, so we start with zero error
ECL_INFO("reset position to last known");
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
_information_events.flags.reset_pos_to_last_known = true;
resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);