dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: update EV height state machine (small piece of EV overhaul) 

- respect new EKF2_EV_CTRL parameter for VPOS usage
  - EV hgt rotate EV position before usage (there's often a small offset in frames)
  - EV hgt reset use proper EV velocity body frame
  - try to keep EV hgt and EV vel state machines consistent
  - small incremental piece of https://github.com/PX4/PX4-Autopilot/pull/19128
This commit is contained in:
Daniel Agar 2022-12-13 13:29:18 -05:00 committed by GitHub
parent 805ffa9d0b
commit 696eeb9a49
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 149 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/ekf2/EKF/bias_estimator.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -57,11 +57,11 @@ class BiasEstimator
{
public:
struct status {
float bias;
float bias_var;
float innov;
float innov_var;
float innov_test_ratio;
float bias{0.f};
float bias_var{0.f};
float innov{0.f};
float innov_var{0.f};
float innov_test_ratio{INFINITY};
};
BiasEstimator(float state_init, float state_var_init): _state{state_init}, _state_var{state_var_init} {};
@ -107,7 +107,7 @@ private:
float _time_since_last_negative_innov{0.f};
float _time_since_last_positive_innov{0.f};
status _status;
status _status{};
void constrainStateVar();
float computeKalmanGain(float innov_var) const;

1
src/modules/ekf2/EKF/common.h
View File

@ -380,6 +380,7 @@ struct parameters {
// vision position fusion
float ev_vel_noise{0.1f}; ///< minimum allowed observation noise for EV velocity fusion (m/sec)
float ev_pos_noise{0.1f}; ///< minimum allowed observation noise for EV position fusion (m)
float ev_att_noise{0.1f}; ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
int32_t ev_quality_minimum{0}; ///< vision minimum acceptable quality integer
float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD)

7
src/modules/ekf2/EKF/control.cpp
View File

@ -400,7 +400,7 @@ void Ekf::controlExternalVisionFusion()
// determine if we should use the horizontal position observations
bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (_ev_sample_delayed.quality >= _params.ev_quality_minimum);
bool starting_conditions_passing = quality_sufficient
const bool starting_conditions_passing = quality_sufficient
&& ((_ev_sample_delayed.time_us - _ev_sample_delayed_prev.time_us) < EV_MAX_INTERVAL)
&& ((_params.ev_quality_minimum <= 0) || (_ev_sample_delayed_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
&& ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
@ -409,13 +409,16 @@ void Ekf::controlExternalVisionFusion()
// EV velocity
controlEvVelFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
// EV height
controlEvHeightFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt);
// record observation and estimate for use next time
_ev_sample_delayed_prev = _ev_sample_delayed;
_hpos_pred_prev = _state.pos.xy();
_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw)
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw || _control_status.flags.ev_hgt)
&& !isRecent(_ev_sample_delayed.time_us, (uint64_t)_params.reset_timeout_max)) {
// Turn off EV fusion mode if no data has been received

4
src/modules/ekf2/EKF/ekf.h
View File

@ -851,7 +851,8 @@ private:
// control fusion of external vision observations
void controlExternalVisionFusion();
void controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset, bool quality_sufficient, estimator_aid_source3d_s& aid_src);
void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src);
void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s& aid_src);
// control fusion of optical flow observations
void controlOpticalFlowFusion();
@ -922,7 +923,6 @@ private:
void controlBaroHeightFusion();
void controlGnssHeightFusion(const gpsSample &gps_sample);
void controlRangeHeightFusion();
void controlEvHeightFusion(const extVisionSample &ev_sample);
bool isConditionalRangeAidSuitable();

1
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -1346,6 +1346,7 @@ void Ekf::stopEvFusion()
stopEvPosFusion();
stopEvVelFusion();
stopEvYawFusion();
stopEvHgtFusion();
}
void Ekf::stopEvPosFusion()

2
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -542,7 +542,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
}
if ((_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW)) || (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
if ((_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW)) || (_params.ev_ctrl > 0)) {
max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
}

217
src/modules/ekf2/EKF/ev_height_control.cpp
View File

@ -38,94 +38,79 @@
#include "ekf.h"
void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample)
void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src)
{
static constexpr const char *HGT_SRC_NAME = "EV";
static constexpr const char *AID_SRC_NAME = "EV height";
auto &aid_src = _aid_src_ev_hgt;
HeightBiasEstimator &bias_est = _ev_hgt_b_est;
bias_est.predict(_dt_ekf_avg);
if (_ev_data_ready) {
// correct position for offset relative to IMU
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
const float measurement = ev_sample.pos(2);
const float measurement_var = ev_sample.posVar(2);
// rotate measurement into correct earth frame if required
Vector3f pos{ev_sample.pos};
Matrix3f pos_cov{matrix::diag(ev_sample.posVar)};
const float innov_gate = math::max(_params.ev_pos_innov_gate, 1.f);
// rotate EV to the EKF reference frame unless we're operating entirely in vision frame
// TODO: only necessary if there's a roll/pitch offset between VIO and EKF
if (!(_control_status.flags.ev_yaw && _control_status.flags.ev_pos)) {
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
updateVerticalPositionAidSrcStatus(ev_sample.time_us,
measurement - bias_est.getBias(),
measurement_var + bias_est.getBiasVar(),
innov_gate,
aid_src);
if (q_error.isAllFinite()) {
const Dcmf R_ev_to_ekf(q_error);
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
if (measurement_valid) {
bias_est.setMaxStateNoise(sqrtf(measurement_var));
bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd);
bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
pos = R_ev_to_ekf * ev_sample.pos;
pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.posVar) * R_ev_to_ekf.transpose();
// increase minimum variance to include EV orientation variance
// TODO: do this properly
const float orientation_var_max = math::max(ev_sample.orientation_var(0), ev_sample.orientation_var(1));
pos_cov(2, 2) = math::max(pos_cov(2, 2), orientation_var_max);
}
}
const bool continuing_conditions_passing = ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS) || (_params.height_sensor_ref == HeightSensor::EV)) // TODO: (_params.ev_ctrl & EvCtrl::VPOS)
&& measurement_valid;
const float measurement = pos(2) - pos_offset_earth(2);
float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f));
const bool starting_conditions_passing = continuing_conditions_passing
&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL);
// increase minimum variance if GPS active
if (_control_status.flags.gps_hgt) {
measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise));
}
if (_control_status.flags.ev_hgt) {
aid_src.fusion_enabled = true;
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
if (continuing_conditions_passing) {
updateVerticalPositionAidSrcStatus(ev_sample.time_us,
measurement - bias_est.getBias(),
measurement_var + bias_est.getBiasVar(),
math::max(_params.ev_pos_innov_gate, 1.f),
aid_src);
fuseVerticalPosition(aid_src);
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
if (measurement_valid && quality_sufficient) {
bias_est.setMaxStateNoise(sqrtf(measurement_var));
bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd);
bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
}
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS)) && measurement_valid;
if (isHeightResetRequired()) {
// All height sources are failing
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing;
_information_events.flags.reset_hgt_to_ev = true;
resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
bias_est.setBias(-_state.pos(2) + measurement);
if (_control_status.flags.ev_hgt) {
aid_src.fusion_enabled = true;
// reset vertical velocity
if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
if (continuing_conditions_passing) {
// correct velocity for offset relative to IMU
const Vector3f pos_offset_body = _params.ev_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;
if (ev_reset) {
switch (ev_sample.vel_frame) {
case VelocityFrame::LOCAL_FRAME_NED:
case VelocityFrame::LOCAL_FRAME_FRD: {
const Vector3f reset_vel = ev_sample.vel - vel_offset_earth;
resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise)));
}
break;
case VelocityFrame::BODY_FRAME_FRD: {
const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise)));
}
break;
}
} else {
resetVerticalVelocityToZero();
}
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
} else if ((_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) && !aid_src.fused) {
// fusion failed and EV sample indicates reset
ECL_INFO("%s height reset", HGT_SRC_NAME);
if (quality_sufficient) {
ECL_INFO("reset to %s", AID_SRC_NAME);
if (_height_sensor_ref == HeightSensor::EV) {
_information_events.flags.reset_hgt_to_ev = true;
@ -138,43 +123,91 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample)
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
} else if (is_fusion_failing) {
// Some other height source is still working
ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME);
} else {
// EV has reset, but quality isn't sufficient
// we have no choice but to stop EV and try to resume once quality is acceptable
stopEvHgtFusion();
return;
}
} else if (quality_sufficient) {
fuseVerticalPosition(aid_src);
} else {
ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME);
aid_src.innovation_rejected = true;
}
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
if (isHeightResetRequired() && quality_sufficient) {
// All height sources are failing
ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME);
_information_events.flags.reset_hgt_to_ev = true;
resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
bias_est.setBias(-_state.pos(2) + measurement);
// reset vertical velocity
if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
// correct velocity for offset relative to IMU
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
switch (ev_sample.vel_frame) {
case VelocityFrame::LOCAL_FRAME_NED:
case VelocityFrame::LOCAL_FRAME_FRD: {
const Vector3f reset_vel = ev_sample.vel - vel_offset_earth;
resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise)));
}
break;
case VelocityFrame::BODY_FRAME_FRD: {
const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise)));
}
break;
}
} else {
resetVerticalVelocityToZero();
}
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
} else if (is_fusion_failing) {
// A reset did not fix the issue but all the starting checks are not passing
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
stopEvHgtFusion();
}
} else {
if (starting_conditions_passing) {
if (_params.height_sensor_ref == HeightSensor::EV) {
ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
_height_sensor_ref = HeightSensor::EV;
_information_events.flags.reset_hgt_to_ev = true;
resetVerticalPositionTo(measurement, measurement_var);
bias_est.reset();
} else {
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
bias_est.setBias(-_state.pos(2) + measurement);
}
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
bias_est.setFusionActive();
_control_status.flags.ev_hgt = true;
}
// Stop fusion but do not declare it faulty
ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
stopEvHgtFusion();
}
} else if (_control_status.flags.ev_hgt
&& !isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) {
// No data anymore. Stop until it comes back.
ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
stopEvHgtFusion();
} else {
if (starting_conditions_passing) {
// activate fusion, only reset if necessary
if (_params.height_sensor_ref == HeightSensor::EV) {
ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
_information_events.flags.reset_hgt_to_ev = true;
resetVerticalPositionTo(measurement, measurement_var);
_height_sensor_ref = HeightSensor::EV;
bias_est.reset();
} else {
ECL_INFO("starting %s fusion", AID_SRC_NAME);
bias_est.setBias(-_state.pos(2) + measurement);
}
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
bias_est.setFusionActive();
_control_status.flags.ev_hgt = true;
}
}
}

8
src/modules/ekf2/EKF/ev_vel_control.cpp
View File

@ -38,8 +38,8 @@
#include "ekf.h"
void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset,
bool quality_sufficient, estimator_aid_source3d_s &aid_src)
void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src)
{
static constexpr const char *AID_SRC_NAME = "EV velocity";
@ -134,8 +134,8 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, bool starting_con
continuing_conditions_passing = false;
}
starting_conditions_passing &= continuing_conditions_passing
&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing
&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
if (_control_status.flags.ev_vel) {
aid_src.fusion_enabled = true;

1
src/modules/ekf2/EKF/height_control.cpp
View File

@ -46,7 +46,6 @@ void Ekf::controlHeightFusion()
controlBaroHeightFusion();
controlGnssHeightFusion(_gps_sample_delayed);
controlRangeHeightFusion();
controlEvHeightFusion(_ev_sample_delayed);
checkHeightSensorRefFallback();
}

1
src/modules/ekf2/EKF2.cpp
View File

@ -125,6 +125,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_rng_k_gate(_params->range_kin_consistency_gate),
_param_ekf2_ev_ctrl(_params->ev_ctrl),
_param_ekf2_ev_qmin(_params->ev_quality_minimum),
_param_ekf2_evp_noise(_params->ev_pos_noise),
_param_ekf2_evv_noise(_params->ev_vel_noise),
_param_ekf2_eva_noise(_params->ev_att_noise),
_param_ekf2_evv_gate(_params->ev_vel_innov_gate),

2
src/modules/ekf2/EKF2.hpp
View File

@ -525,7 +525,7 @@ private:
(ParamInt<px4::params::EKF2_EV_NOISE_MD>)
_param_ekf2_ev_noise_md, ///< determine source of vision observation noise
(ParamExtInt<px4::params::EKF2_EV_QMIN>) _param_ekf2_ev_qmin,
(ParamFloat<px4::params::EKF2_EVP_NOISE>)
(ParamExtFloat<px4::params::EKF2_EVP_NOISE>)
_param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m)
(ParamExtFloat<px4::params::EKF2_EVV_NOISE>)
_param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s)

2
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
View File

@ -77,7 +77,7 @@ void EkfWrapper::setExternalVisionHeightRef()
void EkfWrapper::enableExternalVisionHeightFusion()
{
setExternalVisionHeightRef(); // TODO: replace by EV control parameter
_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::VPOS);
}
bool EkfWrapper::isIntendingExternalVisionHeightFusion() const