mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-08 08:50:35 +08:00
ekf2: refactor gps vel/pos fusion control logic
This commit is contained in:
bresch
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
9afc390552
commit
689ab12845
+131
-177
@@ -521,6 +521,11 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks()
|
||||
|
||||
void Ekf::controlGpsFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & MASK_USE_GPS)) {
|
||||
stopGpsFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
// Check for new GPS data that has fallen behind the fusion time horizon
|
||||
if (_gps_data_ready) {
|
||||
|
||||
@@ -531,8 +536,91 @@ void Ekf::controlGpsFusion()
|
||||
|
||||
// Determine if we should use GPS aiding for velocity and horizontal position
|
||||
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
|
||||
if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
|
||||
if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) {
|
||||
const bool mandatory_conditions_passing = _control_status.flags.tilt_align
|
||||
&& _control_status.flags.yaw_align
|
||||
&& _NED_origin_initialised;
|
||||
const bool continuing_conditions_passing = mandatory_conditions_passing
|
||||
&& !gps_checks_failing;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& gps_checks_passing;
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
if (mandatory_conditions_passing) {
|
||||
if (continuing_conditions_passing
|
||||
|| !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
|
||||
fuseGpsVelPos();
|
||||
|
||||
// We are relying on aiding to constrain drift so after a specified time
|
||||
// with no aiding we need to do something
|
||||
const bool do_vel_pos_reset = hasHorizontalAidingTimedOut()
|
||||
|| isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
|
||||
|
||||
/* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of
|
||||
navigation casued by a bad yaw estimate.
|
||||
|
||||
A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity
|
||||
innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff,
|
||||
different test criteria are used that take longer to trigger and reduce false positives. A reset is
|
||||
not performed if the fault condition was present before flight to prevent triggering due to GPS glitches
|
||||
or other sensor errors.
|
||||
|
||||
The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight.
|
||||
|
||||
The total number of resets allowed per boot cycle is limited.
|
||||
|
||||
The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
|
||||
to improve its estimate if the previous reset was not successful.
|
||||
|
||||
A reset is not performed when getting GPS back after a significant period of no data because the timeout
|
||||
could have been caused by bad GPS.
|
||||
*/
|
||||
const bool is_recent_takeoff_nav_failure = isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
|
||||
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)
|
||||
&& isRecentTakeoff();
|
||||
|
||||
|
||||
const bool is_inflight_nav_failure = _control_status.flags.in_air &&
|
||||
do_vel_pos_reset &&
|
||||
(_time_last_hor_vel_fuse > _time_last_on_ground_us) &&
|
||||
(_time_last_hor_pos_fuse > _time_last_on_ground_us);
|
||||
|
||||
if (is_recent_takeoff_nav_failure || is_inflight_nav_failure){
|
||||
const bool is_yaw_failure = !isVelStateAlignedWithObs();
|
||||
const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);
|
||||
|
||||
if (is_yaw_failure
|
||||
&& !was_gps_signal_lost
|
||||
&& _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) {
|
||||
|
||||
_do_ekfgsf_yaw_reset = true;
|
||||
_ekfgsf_yaw_reset_count++;
|
||||
|
||||
} else {
|
||||
|
||||
// use GPS velocity data to check and correct yaw angle if a FW vehicle
|
||||
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
|
||||
// if flying a fixed wing aircraft, do a complete reset that includes yaw
|
||||
_control_status.flags.mag_aligned_in_flight = realignYawGPS();
|
||||
}
|
||||
|
||||
_warning_events.flags.gps_fusion_timout = true;
|
||||
ECL_WARN("GPS fusion timeout - resetting");
|
||||
_velpos_reset_request = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
stopGpsFusion();
|
||||
_warning_events.flags.gps_quality_poor = true;
|
||||
ECL_WARN("GPS quality poor - stopping use");
|
||||
}
|
||||
|
||||
} else { // mandatory conditions are not passing
|
||||
stopGpsFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
// If the heading is not aligned, reset the yaw and magnetic field states
|
||||
// Do not use external vision for yaw if using GPS because yaw needs to be
|
||||
// defined relative to an NED reference frame
|
||||
@@ -546,198 +634,37 @@ void Ekf::controlGpsFusion()
|
||||
stopEvYawFusion();
|
||||
_inhibit_ev_yaw_use = true;
|
||||
|
||||
} else if (_control_status.flags.yaw_align) {
|
||||
// If the heading is valid start using gps aiding
|
||||
} else {
|
||||
startGpsFusion();
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.gps
|
||||
&& (!(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.yaw_align)) {
|
||||
|
||||
stopGpsFusion();
|
||||
}
|
||||
|
||||
// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
|
||||
if (_control_status.flags.gps && gps_checks_failing && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
|
||||
stopGpsFusion();
|
||||
|
||||
// Reset position state to external vision if we are going to use absolute values
|
||||
if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
|
||||
resetHorizontalPosition();
|
||||
}
|
||||
|
||||
_warning_events.flags.gps_quality_poor = true;
|
||||
ECL_WARN("GPS quality poor - stopping use");
|
||||
}
|
||||
|
||||
// handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before
|
||||
// we can start using GPS
|
||||
const bool align_yaw_using_gsf = !_control_status.flags.yaw_align
|
||||
&& (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE);
|
||||
_do_ekfgsf_yaw_reset = _do_ekfgsf_yaw_reset || align_yaw_using_gsf;
|
||||
|
||||
if ((align_yaw_using_gsf || _do_ekfgsf_yaw_reset)
|
||||
// Process yaw estimator reset request
|
||||
if (_do_ekfgsf_yaw_reset
|
||||
&& isTimedOut(_ekfgsf_yaw_reset_time, 5000000)){
|
||||
if (resetYawToEKFGSF()) {
|
||||
_ekfgsf_yaw_reset_time = _time_last_imu;
|
||||
_do_ekfgsf_yaw_reset = false;
|
||||
}
|
||||
}
|
||||
|
||||
// handle the case when we now have GPS, but have not been fusing it for an extended period
|
||||
if (_control_status.flags.gps) {
|
||||
// We are relying on aiding to constrain drift so after a specified time
|
||||
// with no aiding we need to do something
|
||||
bool do_vel_pos_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
|
||||
|
||||
// We haven't had an absolute position fix for a longer time so need to do something
|
||||
do_vel_pos_reset = do_vel_pos_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
|
||||
|
||||
/* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of
|
||||
navigation casued by a bad yaw estimate.
|
||||
|
||||
A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity
|
||||
innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff,
|
||||
different test criteria are used that take longer to trigger and reduce false positives. A reset is
|
||||
not performed if the fault condition was present before flight to prevent triggering due to GPS glitches
|
||||
or other sensor errors.
|
||||
|
||||
The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight.
|
||||
|
||||
The total number of resets allowed per boot cycle is limited.
|
||||
|
||||
The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
|
||||
to improve its estimate if the previous reset was not successful.
|
||||
|
||||
A reset is not performed when getting GPS back after a significant period of no data because the timeout
|
||||
could have been caused by bad GPS.
|
||||
*/
|
||||
|
||||
const bool recent_takeoff_nav_failure = _control_status.flags.in_air &&
|
||||
!isTimedOut(_time_last_on_ground_us, 30000000) &&
|
||||
isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) &&
|
||||
(_time_last_hor_vel_fuse > _time_last_on_ground_us);
|
||||
|
||||
const bool inflight_nav_failure = _control_status.flags.in_air &&
|
||||
do_vel_pos_reset &&
|
||||
(_time_last_hor_vel_fuse > _time_last_on_ground_us) &&
|
||||
(_time_last_hor_pos_fuse > _time_last_on_ground_us);
|
||||
|
||||
bool is_yaw_failure = false;
|
||||
|
||||
if ((recent_takeoff_nav_failure || inflight_nav_failure) && _time_last_hor_vel_fuse > 0) {
|
||||
// Do sanity check to see if the innovation failures is likely caused by a yaw angle error
|
||||
// by measuring the angle between the velocity estimate and the last velocity observation
|
||||
// Only use those vectors if their norm if they are larger than 4 times their noise standard deviation
|
||||
const float vel_obs_xy_norm_sq = _last_vel_obs.xy().norm_squared();
|
||||
const float vel_state_xy_norm_sq = _state.vel.xy().norm_squared();
|
||||
|
||||
const float vel_obs_threshold_sq = fmaxf(sq(4.f) * (_last_vel_obs_var(0) + _last_vel_obs_var(1)), sq(0.4f));
|
||||
const float vel_state_threshold_sq = fmaxf(sq(4.f) * (P(4, 4) + P(5, 5)), sq(0.4f));
|
||||
|
||||
if (vel_obs_xy_norm_sq > vel_obs_threshold_sq && vel_state_xy_norm_sq > vel_state_threshold_sq) {
|
||||
const float obs_dot_vel = Vector2f(_last_vel_obs).dot(_state.vel.xy());
|
||||
const float cos_sq = sq(obs_dot_vel) / (vel_state_xy_norm_sq * vel_obs_xy_norm_sq);
|
||||
|
||||
if (cos_sq < sq(cosf(math::radians(25.f))) || obs_dot_vel < 0.f) {
|
||||
// The angle between the observation and the velocity estimate is greater than 25 degrees
|
||||
is_yaw_failure = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Detect if coming back after significant time without GPS data
|
||||
const bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000);
|
||||
const bool do_yaw_vel_pos_reset = (_do_ekfgsf_yaw_reset || is_yaw_failure) &&
|
||||
_ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit &&
|
||||
isTimedOut(_ekfgsf_yaw_reset_time, 5000000) &&
|
||||
!gps_signal_was_lost;
|
||||
|
||||
if (do_yaw_vel_pos_reset) {
|
||||
if (resetYawToEKFGSF()) {
|
||||
_ekfgsf_yaw_reset_time = _time_last_imu;
|
||||
_do_ekfgsf_yaw_reset = false;
|
||||
_ekfgsf_yaw_reset_count++;
|
||||
|
||||
// Reset the timeout counters
|
||||
_time_last_hor_pos_fuse = _time_last_imu;
|
||||
_time_last_delpos_fuse = _time_last_imu;
|
||||
_time_last_hor_vel_fuse = _time_last_imu;
|
||||
_time_last_of_fuse = _time_last_imu;
|
||||
}
|
||||
|
||||
} else if (do_vel_pos_reset) {
|
||||
// use GPS velocity data to check and correct yaw angle if a FW vehicle
|
||||
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
|
||||
// if flying a fixed wing aircraft, do a complete reset that includes yaw
|
||||
_control_status.flags.mag_aligned_in_flight = realignYawGPS();
|
||||
}
|
||||
|
||||
resetVelocity();
|
||||
resetHorizontalPosition();
|
||||
_velpos_reset_request = false;
|
||||
_warning_events.flags.gps_fusion_timout = true;
|
||||
ECL_WARN("GPS fusion timeout - reset to GPS");
|
||||
|
||||
// Reset the timeout counters
|
||||
_time_last_hor_pos_fuse = _time_last_imu;
|
||||
_time_last_hor_vel_fuse = _time_last_imu;
|
||||
|
||||
_do_ekfgsf_yaw_reset = false;
|
||||
_velpos_reset_request = false; // included in yaw reset
|
||||
}
|
||||
}
|
||||
|
||||
// Only use GPS data for position and velocity aiding if enabled
|
||||
if (_control_status.flags.gps) {
|
||||
if (_velpos_reset_request) {
|
||||
resetVelocity();
|
||||
resetHorizontalPosition();
|
||||
_velpos_reset_request = false;
|
||||
|
||||
Vector2f gps_vel_innov_gates; // [horizontal vertical]
|
||||
Vector2f gps_pos_innov_gates; // [horizontal vertical]
|
||||
Vector3f gps_pos_obs_var;
|
||||
|
||||
// correct velocity for offset relative to IMU
|
||||
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
|
||||
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
|
||||
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
||||
_gps_sample_delayed.vel -= vel_offset_earth;
|
||||
|
||||
// correct position and height for offset relative to IMU
|
||||
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||
_gps_sample_delayed.pos -= pos_offset_earth.xy();
|
||||
_gps_sample_delayed.hgt += pos_offset_earth(2);
|
||||
|
||||
const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
|
||||
|
||||
if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
|
||||
// if we are using other sources of aiding, then relax the upper observation
|
||||
// noise limit which prevents bad GPS perturbing the position estimate
|
||||
gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(fmaxf(_gps_sample_delayed.hacc, lower_limit));
|
||||
|
||||
} else {
|
||||
// if we are not using another source of aiding, then we are reliant on the GPS
|
||||
// observations to constrain attitude errors and must limit the observation noise value.
|
||||
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
|
||||
gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit));
|
||||
}
|
||||
|
||||
_gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise);
|
||||
|
||||
_last_vel_obs_var.setAll(sq(_gps_sample_delayed.sacc));
|
||||
_last_vel_obs_var(2) *= sq(1.5f);
|
||||
|
||||
// calculate innovations
|
||||
_last_vel_obs = _gps_sample_delayed.vel;
|
||||
_gps_vel_innov = _state.vel - _last_vel_obs;
|
||||
_gps_pos_innov.xy() = Vector2f(_state.pos) - _gps_sample_delayed.pos;
|
||||
|
||||
// set innovation gate size
|
||||
gps_pos_innov_gates(0) = fmaxf(_params.gps_pos_innov_gate, 1.0f);
|
||||
gps_vel_innov_gates(0) = gps_vel_innov_gates(1) = fmaxf(_params.gps_vel_innov_gate, 1.0f);
|
||||
|
||||
// fuse GPS measurement
|
||||
fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates, _last_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
|
||||
fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, _last_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
|
||||
fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
|
||||
// Reset the timeout counters
|
||||
_time_last_hor_pos_fuse = _time_last_imu;
|
||||
_time_last_hor_vel_fuse = _time_last_imu;
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
|
||||
@@ -753,6 +680,33 @@ void Ekf::controlGpsFusion()
|
||||
_warning_events.flags.gps_data_stopped_using_alternate = true;
|
||||
ECL_WARN("GPS data stopped, using only EV, OF or air data");
|
||||
}
|
||||
//TODO: move that resetHorizontalPosition to EV
|
||||
/* // Handle the case where we are using GPS and another source of aiding and GPS is failing checks */
|
||||
/* if (_control_status.flags.gps && gps_checks_failing && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { */
|
||||
/* stopGpsFusion(); */
|
||||
|
||||
/* // Reset position state to external vision if we are going to use absolute values */
|
||||
/* if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { */
|
||||
/* resetHorizontalPosition(); */
|
||||
/* } */
|
||||
|
||||
/* _warning_events.flags.gps_quality_poor = true; */
|
||||
/* ECL_WARN("GPS quality poor - stopping use"); */
|
||||
/* } */
|
||||
}
|
||||
|
||||
bool Ekf::hasHorizontalAidingTimedOut() const
|
||||
{
|
||||
return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
|
||||
}
|
||||
|
||||
bool Ekf::isRecentTakeoff() const
|
||||
{
|
||||
return _control_status.flags.in_air
|
||||
&& !isTimedOut(_time_last_on_ground_us, 30000000);
|
||||
}
|
||||
|
||||
void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
|
||||
|
||||
Reference in New Issue
Block a user