mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
ekf2: enable multiple height sources fusion
Instead of having a single height source fused into the EKF and the other ones "waiting" for a failure or the primary sensor, fuse all sources in EKF2 at the same time. To prevent the sources from fighting against each other, the "primary" source is set as reference and the other ones are running a bias estimator in order to make all the secondary height sources converge to the primary one. If the reference isn't available, another one is automatically selected from a priority list. This secondary reference keeps its current bias estimate but stops updating it in order to be the new reference as close as possible to the primary one.
This commit is contained in:
bresch
Daniel Agar
parent
f5f31006a0
commit
8fd79688c0
@ -97,7 +97,7 @@ inline bool BiasEstimator::isTestRatioPassing(const float innov_test_ratio) cons
|
||||
|
||||
inline void BiasEstimator::updateState(const float K, const float innov)
|
||||
{
|
||||
_state = math::constrain(_state + K * innov, -_state_max, _state_max);
|
||||
_state = _state + K * innov;
|
||||
}
|
||||
|
||||
inline void BiasEstimator::updateStateCovariance(const float K)
|
||||
|
||||
@ -64,9 +64,18 @@ public:
|
||||
float innov_test_ratio;
|
||||
};
|
||||
|
||||
BiasEstimator() = default;
|
||||
BiasEstimator(float state_init, float state_var_init): _state{state_init}, _state_var{state_var_init} {};
|
||||
virtual ~BiasEstimator() = default;
|
||||
|
||||
void reset()
|
||||
{
|
||||
_state = 0.f;
|
||||
_state_var = 0.f;
|
||||
_signed_innov_test_ratio_lpf.reset(0.f);
|
||||
_time_since_last_negative_innov = 0.f;
|
||||
_time_since_last_positive_innov = 0.f;
|
||||
}
|
||||
|
||||
virtual void predict(float dt);
|
||||
virtual void fuseBias(float measurement, float measurement_var);
|
||||
|
||||
@ -86,7 +95,6 @@ public:
|
||||
|
||||
private:
|
||||
float _state{0.f};
|
||||
float _state_max{100.f};
|
||||
float _dt{0.01f};
|
||||
|
||||
float _gate_size{3.f}; ///< Used for innovation filtering (innovation test ratio)
|
||||
|
||||
@ -105,12 +105,12 @@ enum TerrainFusionMask : uint8_t {
|
||||
TerrainFuseOpticalFlow = (1 << 1)
|
||||
};
|
||||
|
||||
enum VerticalHeightSensor : uint8_t {
|
||||
// Integer definitions for vdist_sensor_type
|
||||
BARO = 0, ///< Use baro height
|
||||
GPS = 1, ///< Use GPS height
|
||||
RANGE = 2, ///< Use range finder height
|
||||
EV = 3 ///< Use external vision
|
||||
enum HeightSensor : uint8_t {
|
||||
BARO = 0,
|
||||
GNSS = 1,
|
||||
RANGE = 2,
|
||||
EV = 3,
|
||||
UNKNOWN = 4
|
||||
};
|
||||
|
||||
enum SensorFusionMask : uint16_t {
|
||||
@ -123,7 +123,11 @@ enum SensorFusionMask : uint16_t {
|
||||
USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind
|
||||
ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
|
||||
USE_GPS_YAW = (1<<7), ///< set to true to use GPS yaw data if available
|
||||
USE_EXT_VIS_VEL = (1<<8) ///< set to true to use external vision velocity data
|
||||
USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data
|
||||
USE_BARO_HGT = (1<<9), ///< set to true to use baro data
|
||||
USE_GPS_HGT = (1<<10), ///< set to true to use GPS height data
|
||||
USE_RNG_HGT = (1<<11), ///< set to true to use range height data
|
||||
USE_EXT_VIS_HGT = (1<<12) ///< set to true to use external vision height data
|
||||
};
|
||||
|
||||
struct gpsMessage {
|
||||
@ -247,8 +251,9 @@ struct parameters {
|
||||
int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
|
||||
|
||||
// measurement source control
|
||||
int32_t fusion_mode{SensorFusionMask::USE_GPS}; ///< bitmasked integer that selects which aiding sources will be used
|
||||
int32_t vdist_sensor_type{VerticalHeightSensor::BARO}; ///< selects the primary source for height data
|
||||
int32_t fusion_mode{SensorFusionMask::USE_GPS |
|
||||
SensorFusionMask::USE_BARO_HGT}; ///< bitmasked integer that selects which aiding sources will be used
|
||||
int32_t height_sensor_ref{HeightSensor::BARO};
|
||||
int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
|
||||
TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
|
||||
|
||||
|
||||
@ -172,9 +172,6 @@ void Ekf::controlFusionModes()
|
||||
// run EKF-GSF yaw estimator once per _imu_sample_delayed update after all main EKF data samples available
|
||||
runYawEKFGSF();
|
||||
|
||||
// check for height sensor timeouts and reset and change sensor if necessary
|
||||
controlHeightSensorTimeouts();
|
||||
|
||||
// control use of observations for aiding
|
||||
controlMagFusion();
|
||||
controlOpticalFlowFusion();
|
||||
@ -660,134 +657,6 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlHeightSensorTimeouts()
|
||||
{
|
||||
/*
|
||||
* Handle the case where we have not fused height measurements recently and
|
||||
* uncertainty exceeds the max allowable. Reset using the best available height
|
||||
* measurement source, continue using it after the reset and declare the current
|
||||
* source failed if we have switched.
|
||||
*/
|
||||
|
||||
checkVerticalAccelerationHealth();
|
||||
|
||||
// check if height is continuously failing because of accel errors
|
||||
const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us);
|
||||
|
||||
// check if height has been inertial deadreckoning for too long
|
||||
// in vision hgt mode check for vision data
|
||||
const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6);
|
||||
|
||||
if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
|
||||
|
||||
const char *failing_height_source = nullptr;
|
||||
const char *new_height_source = nullptr;
|
||||
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
bool reset_to_gps = false;
|
||||
|
||||
// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
|
||||
if (!_gps_intermittent) {
|
||||
reset_to_gps = (_gps_checks_passed && !_fault_status.flags.bad_acc_vertical) || _baro_hgt_faulty || _baro_hgt_intermittent;
|
||||
}
|
||||
|
||||
if (reset_to_gps) {
|
||||
// set height sensor health
|
||||
_baro_hgt_faulty = true;
|
||||
|
||||
startGpsHgtFusion();
|
||||
|
||||
failing_height_source = "baro";
|
||||
new_height_source = "gps";
|
||||
|
||||
} else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
|
||||
resetHeightToBaro();
|
||||
|
||||
failing_height_source = "baro";
|
||||
new_height_source = "baro";
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.gps_hgt) {
|
||||
bool reset_to_baro = false;
|
||||
|
||||
// if baro data is available and GPS data is inaccurate and the timeout cannot be blamed on IMU data, reset height to baro
|
||||
if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
|
||||
reset_to_baro = (!_fault_status.flags.bad_acc_vertical && !_gps_checks_passed) || _gps_intermittent;
|
||||
}
|
||||
|
||||
if (reset_to_baro) {
|
||||
startBaroHgtFusion();
|
||||
|
||||
failing_height_source = "gps";
|
||||
new_height_source = "baro";
|
||||
|
||||
} else if (!_gps_intermittent) {
|
||||
resetHeightToGps();
|
||||
|
||||
failing_height_source = "gps";
|
||||
new_height_source = "gps";
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.rng_hgt) {
|
||||
|
||||
if (_range_sensor.isHealthy()) {
|
||||
resetHeightToRng();
|
||||
|
||||
failing_height_source = "rng";
|
||||
new_height_source = "rng";
|
||||
|
||||
} else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
|
||||
startBaroHgtFusion();
|
||||
|
||||
failing_height_source = "rng";
|
||||
new_height_source = "baro";
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.ev_hgt) {
|
||||
// check if vision data is available
|
||||
bool ev_data_available = false;
|
||||
|
||||
if (_ext_vision_buffer) {
|
||||
const extVisionSample &ev_init = _ext_vision_buffer->get_newest();
|
||||
ev_data_available = isRecent(ev_init.time_us, 2 * EV_MAX_INTERVAL);
|
||||
}
|
||||
|
||||
if (ev_data_available) {
|
||||
resetHeightToEv();
|
||||
|
||||
failing_height_source = "ev";
|
||||
new_height_source = "ev";
|
||||
|
||||
} else if (_range_sensor.isHealthy()) {
|
||||
// Fallback to rangefinder data if available
|
||||
startRngHgtFusion();
|
||||
|
||||
failing_height_source = "ev";
|
||||
new_height_source = "rng";
|
||||
|
||||
} else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
|
||||
startBaroHgtFusion();
|
||||
|
||||
failing_height_source = "ev";
|
||||
new_height_source = "baro";
|
||||
}
|
||||
}
|
||||
|
||||
if (failing_height_source && new_height_source) {
|
||||
_warning_events.flags.height_sensor_timeout = true;
|
||||
ECL_WARN("%s hgt timeout - reset to %s", failing_height_source, new_height_source);
|
||||
}
|
||||
|
||||
// Also reset the vertical velocity
|
||||
if (_control_status.flags.gps && !_gps_intermittent && _gps_checks_passed) {
|
||||
resetVerticalVelocityToGps(_gps_sample_delayed);
|
||||
|
||||
} else {
|
||||
resetVerticalVelocityToZero();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::checkVerticalAccelerationHealth()
|
||||
{
|
||||
// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical
|
||||
@ -797,6 +666,7 @@ void Ekf::checkVerticalAccelerationHealth()
|
||||
// Don't use stale innovation data.
|
||||
bool is_inertial_nav_falling = false;
|
||||
bool are_vertical_pos_and_vel_independant = false;
|
||||
bool is_using_multiple_hgt_sources = false;
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
// GNSS velocity
|
||||
@ -827,6 +697,8 @@ void Ekf::checkVerticalAccelerationHealth()
|
||||
}
|
||||
|
||||
if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) {
|
||||
is_using_multiple_hgt_sources = _control_status.flags.gps_hgt && _control_status.flags.baro_hgt;
|
||||
|
||||
if (isRecent(_vert_vel_fuse_time_us, 1000000)) {
|
||||
// If vertical position and velocity come from independent sensors then we can
|
||||
// trust them more if they disagree with the IMU, but need to check that they agree
|
||||
@ -861,7 +733,7 @@ void Ekf::checkVerticalAccelerationHealth()
|
||||
|
||||
// if vertical velocity and position are independent and agree, then do not require evidence of clipping if
|
||||
// innovations are large
|
||||
const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) && is_inertial_nav_falling;
|
||||
const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_using_multiple_hgt_sources || is_clipping_frequently) && is_inertial_nav_falling;
|
||||
|
||||
if (bad_vert_accel) {
|
||||
_time_bad_vert_accel = _time_last_imu;
|
||||
@ -882,115 +754,75 @@ void Ekf::checkVerticalAccelerationHealth()
|
||||
|
||||
void Ekf::controlHeightFusion()
|
||||
{
|
||||
checkVerticalAccelerationHealth();
|
||||
checkRangeAidSuitability();
|
||||
const bool do_range_aid = (_params.range_aid == 1) && _is_range_aid_suitable;
|
||||
|
||||
switch (_params.vdist_sensor_type) {
|
||||
default:
|
||||
ECL_ERR("Invalid hgt mode: %" PRIi32, _params.vdist_sensor_type);
|
||||
|
||||
// FALLTHROUGH
|
||||
case VerticalHeightSensor::BARO:
|
||||
if (do_range_aid) {
|
||||
if (!_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
|
||||
startRngAidHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!_control_status.flags.baro_hgt) {
|
||||
if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
|
||||
startBaroHgtFusion();
|
||||
|
||||
} else if (!_control_status.flags.gps_hgt && !_gps_intermittent && _gps_checks_passed) {
|
||||
// Use GPS as a fallback
|
||||
startGpsHgtFusion();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case VerticalHeightSensor::RANGE:
|
||||
|
||||
// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
|
||||
// and are on the ground, then synthesise a measurement at the expected on ground value
|
||||
if (!_control_status.flags.in_air
|
||||
&& !_range_sensor.isDataHealthy()
|
||||
&& _range_sensor.isRegularlySendingData()
|
||||
&& _range_sensor.isDataReady()) {
|
||||
|
||||
_range_sensor.setRange(_params.rng_gnd_clearance);
|
||||
_range_sensor.setValidity(true); // bypass the checks
|
||||
}
|
||||
|
||||
if (!_control_status.flags.rng_hgt) {
|
||||
if (_range_sensor.isDataHealthy()) {
|
||||
startRngHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case VerticalHeightSensor::GPS:
|
||||
|
||||
// NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
|
||||
// to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
|
||||
// Do switching between GPS and rangefinder if using range finder as a height source when close
|
||||
// to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers.
|
||||
if (do_range_aid) {
|
||||
if (!_control_status_prev.flags.rng_hgt && _range_sensor.isDataHealthy()) {
|
||||
startRngAidHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!_control_status.flags.gps_hgt) {
|
||||
if (!_gps_intermittent && _gps_checks_passed) {
|
||||
// In fallback mode and GPS has recovered so start using it
|
||||
startGpsHgtFusion();
|
||||
|
||||
} else if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty && !_baro_hgt_intermittent) {
|
||||
// Use baro as a fallback
|
||||
startBaroHgtFusion();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case VerticalHeightSensor::EV:
|
||||
|
||||
// don't start using EV data unless data is arriving frequently
|
||||
if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
|
||||
startEvHgtFusion();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
updateBaroHgtBias();
|
||||
updateBaroHgtOffset();
|
||||
updateGroundEffect();
|
||||
|
||||
if (_baro_data_ready) {
|
||||
updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
|
||||
controlBaroHeightFusion();
|
||||
controlGnssHeightFusion();
|
||||
controlRangeHeightFusion();
|
||||
controlEvHeightFusion();
|
||||
|
||||
if (_control_status.flags.baro_hgt && !_baro_hgt_faulty) {
|
||||
fuseBaroHgt(_aid_src_baro_hgt);
|
||||
}
|
||||
checkHeightSensorRefFallback();
|
||||
}
|
||||
|
||||
void Ekf::checkHeightSensorRefFallback()
|
||||
{
|
||||
if (_height_sensor_ref != HeightSensor::UNKNOWN) {
|
||||
// The reference sensor is running, all good
|
||||
return;
|
||||
}
|
||||
|
||||
if (_rng_data_ready) {
|
||||
updateRngHgt(_aid_src_rng_hgt);
|
||||
HeightSensor fallback_list[4];
|
||||
|
||||
if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
|
||||
fuseRngHgt(_aid_src_rng_hgt);
|
||||
}
|
||||
switch (_params.height_sensor_ref) {
|
||||
default:
|
||||
/* FALLTHROUGH */
|
||||
case HeightSensor::UNKNOWN:
|
||||
fallback_list[0] = HeightSensor::GNSS;
|
||||
fallback_list[1] = HeightSensor::BARO;
|
||||
fallback_list[2] = HeightSensor::EV;
|
||||
fallback_list[3] = HeightSensor::RANGE;
|
||||
break;
|
||||
|
||||
case HeightSensor::BARO:
|
||||
fallback_list[0] = HeightSensor::BARO;
|
||||
fallback_list[1] = HeightSensor::GNSS;
|
||||
fallback_list[2] = HeightSensor::EV;
|
||||
fallback_list[3] = HeightSensor::RANGE;
|
||||
break;
|
||||
|
||||
case HeightSensor::GNSS:
|
||||
fallback_list[0] = HeightSensor::GNSS;
|
||||
fallback_list[1] = HeightSensor::BARO;
|
||||
fallback_list[2] = HeightSensor::EV;
|
||||
fallback_list[3] = HeightSensor::RANGE;
|
||||
break;
|
||||
|
||||
case HeightSensor::RANGE:
|
||||
fallback_list[0] = HeightSensor::RANGE;
|
||||
fallback_list[1] = HeightSensor::EV;
|
||||
fallback_list[2] = HeightSensor::BARO;
|
||||
fallback_list[3] = HeightSensor::GNSS;
|
||||
break;
|
||||
|
||||
case HeightSensor::EV:
|
||||
fallback_list[0] = HeightSensor::EV;
|
||||
fallback_list[1] = HeightSensor::RANGE;
|
||||
fallback_list[2] = HeightSensor::BARO;
|
||||
fallback_list[3] = HeightSensor::GNSS;
|
||||
break;
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
|
||||
if (_control_status.flags.ev_hgt && _ev_data_ready) {
|
||||
fuseEvHgt();
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
if (((fallback_list[i] == HeightSensor::BARO) && _control_status.flags.baro_hgt)
|
||||
|| ((fallback_list[i] == HeightSensor::GNSS) && _control_status.flags.gps_hgt)
|
||||
|| ((fallback_list[i] == HeightSensor::RANGE) && _control_status.flags.rng_hgt)
|
||||
|| ((fallback_list[i] == HeightSensor::EV) && _control_status.flags.ev_hgt)) {
|
||||
ECL_INFO("fallback to secondary height reference");
|
||||
_height_sensor_ref = fallback_list[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1024,6 +856,199 @@ void Ekf::checkRangeAidSuitability()
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlBaroHeightFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_BARO_HGT)) {
|
||||
stopBaroHgtFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
_baro_b_est.predict(_dt_ekf_avg);
|
||||
|
||||
if (_baro_data_ready) {
|
||||
_baro_lpf.update(_baro_sample_delayed.hgt);
|
||||
updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
|
||||
|
||||
const bool continuing_conditions_passing = !_baro_hgt_faulty && !_baro_hgt_intermittent;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing;
|
||||
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
if (continuing_conditions_passing) {
|
||||
fuseBaroHgt(_aid_src_baro_hgt);
|
||||
|
||||
const bool is_fusion_failing = isTimedOut(_aid_src_baro_hgt.time_last_fuse, (uint64_t)5e6);
|
||||
|
||||
if (isHeightResetRequired()) {
|
||||
// All height sources are failing
|
||||
resetHeightToBaro();
|
||||
resetVerticalVelocityToZero();
|
||||
|
||||
} else if (is_fusion_failing) {
|
||||
// Some other height source is still working
|
||||
stopBaroHgtFusion();
|
||||
_baro_hgt_faulty = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
stopBaroHgtFusion();
|
||||
}
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startBaroHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.baro_hgt && _baro_hgt_intermittent) {
|
||||
// No baro data anymore. Stop until it comes back.
|
||||
stopBaroHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlGnssHeightFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_GPS_HGT)) {
|
||||
stopGpsHgtFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
_gps_hgt_b_est.predict(_dt_ekf_avg);
|
||||
|
||||
if (_gps_data_ready) {
|
||||
const bool continuing_conditions_passing = !_gps_intermittent && _gps_checks_passed && _NED_origin_initialised;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing;
|
||||
|
||||
if (_control_status.flags.gps_hgt) {
|
||||
if (continuing_conditions_passing) {
|
||||
/* fuseGpsHgt(); */ // Done in fuseGpsPos
|
||||
|
||||
const bool is_fusion_failing = isTimedOut(_aid_src_gnss_pos.time_last_fuse[2], _params.reset_timeout_max);
|
||||
|
||||
if (isHeightResetRequired()) {
|
||||
// All height sources are failing
|
||||
resetHeightToGps();
|
||||
resetVerticalVelocityToGps(_gps_sample_delayed);
|
||||
|
||||
} else if (is_fusion_failing) {
|
||||
// Some other height source is still working
|
||||
stopGpsHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
stopGpsHgtFusion();
|
||||
}
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startGpsHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.gps_hgt && _gps_intermittent) {
|
||||
stopGpsHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlRangeHeightFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_RNG_HGT)) {
|
||||
stopRngHgtFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
_rng_hgt_b_est.predict(_dt_ekf_avg);
|
||||
|
||||
// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
|
||||
// and are on the ground, then synthesise a measurement at the expected on ground value
|
||||
if (!_control_status.flags.in_air
|
||||
&& !_range_sensor.isDataHealthy()
|
||||
&& _range_sensor.isRegularlySendingData()
|
||||
&& _range_sensor.isDataReady()) {
|
||||
|
||||
_range_sensor.setRange(_params.rng_gnd_clearance);
|
||||
_range_sensor.setValidity(true); // bypass the checks
|
||||
}
|
||||
|
||||
if (_rng_data_ready) {
|
||||
|
||||
// TODO: primary height source:
|
||||
// run a bias estimator for all other sources
|
||||
updateRngHgt(_aid_src_rng_hgt);
|
||||
|
||||
const bool continuing_conditions_passing = _range_sensor.isDataHealthy();
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& _range_sensor.isRegularlySendingData();
|
||||
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
if (continuing_conditions_passing) {
|
||||
fuseRngHgt(_aid_src_rng_hgt);
|
||||
|
||||
const bool is_fusion_failing = isTimedOut(_aid_src_rng_hgt.time_last_fuse, (uint64_t)5e6);
|
||||
|
||||
if (isHeightResetRequired()) {
|
||||
// All height sources are failing
|
||||
resetHeightToRng();
|
||||
resetVerticalVelocityToZero();
|
||||
|
||||
} else if (is_fusion_failing) {
|
||||
// Some other height source is still working
|
||||
stopRngHgtFusion();
|
||||
_control_status.flags.rng_fault = true;
|
||||
_range_sensor.setFaulty();
|
||||
}
|
||||
|
||||
} else {
|
||||
stopRngHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startRngHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.rng_hgt && isTimedOut(_time_last_range, (uint64_t)5e6)) {
|
||||
stopRngHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlEvHeightFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_HGT)) {
|
||||
stopEvHgtFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
_ev_hgt_b_est.predict(_dt_ekf_avg);
|
||||
|
||||
if (_ev_data_ready) {
|
||||
const bool continuing_conditions_passing = true;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL);
|
||||
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
if (continuing_conditions_passing) {
|
||||
fuseEvHgt();
|
||||
|
||||
if (isHeightResetRequired()) {
|
||||
// All height sources are failing
|
||||
resetHeightToRng();
|
||||
resetVerticalVelocityToZero();
|
||||
}
|
||||
|
||||
} else {
|
||||
stopEvHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startEvHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.ev_hgt && isTimedOut(_time_last_ext_vision, (uint64_t)5e6)) {
|
||||
stopEvHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlAirDataFusion()
|
||||
{
|
||||
// control activation and initialisation/reset of wind states required for airspeed fusion
|
||||
|
||||
@ -164,10 +164,12 @@ bool Ekf::initialiseFilter()
|
||||
if (_baro_buffer && _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
|
||||
if (_baro_sample_delayed.time_us != 0) {
|
||||
if (_baro_counter == 0) {
|
||||
_baro_hgt_offset = _baro_sample_delayed.hgt;
|
||||
_baro_lpf.reset(_baro_sample_delayed.hgt);
|
||||
|
||||
} else {
|
||||
_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
|
||||
_baro_lpf.update(_baro_sample_delayed.hgt);
|
||||
|
||||
_baro_b_est.setBias(_baro_lpf.getState());
|
||||
}
|
||||
|
||||
_baro_counter++;
|
||||
@ -187,7 +189,7 @@ bool Ekf::initialiseFilter()
|
||||
}
|
||||
|
||||
// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
|
||||
setControlBaroHeight();
|
||||
_control_status.flags.baro_hgt = false;
|
||||
|
||||
if (!initialiseTilt()) {
|
||||
return false;
|
||||
|
||||
@ -47,6 +47,7 @@
|
||||
|
||||
#include "EKFGSF_yaw.h"
|
||||
#include "bias_estimator.hpp"
|
||||
#include "height_bias_estimator.hpp"
|
||||
|
||||
#include <uORB/topics/estimator_aid_source_1d.h>
|
||||
#include <uORB/topics/estimator_aid_source_2d.h>
|
||||
@ -586,14 +587,7 @@ private:
|
||||
|
||||
// Variables used to perform in flight resets and switch between height sources
|
||||
AlphaFilter<Vector3f> _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss)
|
||||
|
||||
float _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m)
|
||||
float _gps_hgt_offset{0.0f}; ///< GPS height reading at the local NED origin (m)
|
||||
float _rng_hgt_offset{0.0f}; ///< Range height reading at the local NED origin (m)
|
||||
float _ev_hgt_offset{0.0f}; ///< EV height reading at the local NED origin (m)
|
||||
|
||||
float _baro_hgt_bias{0.0f};
|
||||
float _baro_hgt_bias_var{1.f};
|
||||
AlphaFilter<float> _baro_lpf{0.1f}; ///< filtered barometric height measurement (m)
|
||||
|
||||
// Variables used to control activation of post takeoff functionality
|
||||
float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m)
|
||||
@ -709,6 +703,8 @@ private:
|
||||
void resetHorizontalPositionToLastKnown();
|
||||
void resetHorizontalPositionTo(const Vector2f &new_horz_pos);
|
||||
|
||||
bool isHeightResetRequired() const;
|
||||
|
||||
void resetVerticalPositionTo(float new_vert_pos);
|
||||
|
||||
void resetHeightToBaro();
|
||||
@ -923,9 +919,6 @@ private:
|
||||
void runMagAndMagDeclFusions(const Vector3f &mag);
|
||||
void run3DMagAndDeclFusions(const Vector3f &mag);
|
||||
|
||||
// control fusion of range finder observations
|
||||
void controlRangeFinderFusion();
|
||||
|
||||
// control fusion of air data observations
|
||||
void controlAirDataFusion();
|
||||
|
||||
@ -935,9 +928,6 @@ private:
|
||||
// control fusion of multi-rotor drag specific force observations
|
||||
void controlDragFusion();
|
||||
|
||||
// control fusion of pressure altitude observations
|
||||
void controlBaroFusion();
|
||||
|
||||
// control fusion of fake position observations to constrain drift
|
||||
void controlFakePosFusion();
|
||||
|
||||
@ -948,29 +938,19 @@ private:
|
||||
// control fusion of auxiliary velocity observations
|
||||
void controlAuxVelFusion();
|
||||
|
||||
// control for height sensor timeouts, sensor changes and state resets
|
||||
void controlHeightSensorTimeouts();
|
||||
|
||||
void checkVerticalAccelerationHealth();
|
||||
|
||||
// control for combined height fusion mode (implemented for switching between baro and range height)
|
||||
void controlHeightFusion();
|
||||
void checkHeightSensorRefFallback();
|
||||
void controlBaroHeightFusion();
|
||||
void controlGnssHeightFusion();
|
||||
void controlRangeHeightFusion();
|
||||
void controlEvHeightFusion();
|
||||
|
||||
// determine if flight condition is suitable to use range finder instead of the primary height sensor
|
||||
void checkRangeAidSuitability();
|
||||
|
||||
// set control flags to use baro height
|
||||
void setControlBaroHeight();
|
||||
|
||||
// set control flags to use range height
|
||||
void setControlRangeHeight();
|
||||
|
||||
// set control flags to use GPS height
|
||||
void setControlGPSHeight();
|
||||
|
||||
// set control flags to use external vision height
|
||||
void setControlEVHeight();
|
||||
|
||||
void stopMagFusion();
|
||||
void stopMag3DFusion();
|
||||
void stopMagHdgFusion();
|
||||
@ -978,13 +958,16 @@ private:
|
||||
void startMag3DFusion();
|
||||
|
||||
void startBaroHgtFusion();
|
||||
void stopBaroHgtFusion();
|
||||
|
||||
void startGpsHgtFusion();
|
||||
void stopGpsHgtFusion();
|
||||
|
||||
void startRngHgtFusion();
|
||||
void stopRngHgtFusion();
|
||||
void startRngAidHgtFusion();
|
||||
void startEvHgtFusion();
|
||||
|
||||
void updateBaroHgtOffset();
|
||||
void updateBaroHgtBias();
|
||||
void stopEvHgtFusion();
|
||||
|
||||
void updateGroundEffect();
|
||||
|
||||
@ -1091,7 +1074,12 @@ private:
|
||||
// yaw estimator instance
|
||||
EKFGSF_yaw _yawEstimator{};
|
||||
|
||||
BiasEstimator _baro_b_est{};
|
||||
uint8_t _height_sensor_ref{HeightSensor::UNKNOWN};
|
||||
|
||||
HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
|
||||
HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
|
||||
HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
|
||||
HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
|
||||
|
||||
int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec)
|
||||
uint8_t _ekfgsf_yaw_reset_count{0}; // number of times the yaw has been reset to the EKF-GSF estimate
|
||||
|
||||
@ -213,6 +213,18 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos)
|
||||
_time_last_hor_pos_fuse = _time_last_imu;
|
||||
}
|
||||
|
||||
bool Ekf::isHeightResetRequired() const
|
||||
{
|
||||
// check if height is continuously failing because of accel errors
|
||||
const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us);
|
||||
|
||||
// check if height has been inertial deadreckoning for too long
|
||||
const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6);
|
||||
|
||||
return (continuous_bad_accel_hgt || hgt_fusion_timeout);
|
||||
}
|
||||
|
||||
|
||||
void Ekf::resetVerticalPositionTo(const float new_vert_pos)
|
||||
{
|
||||
const float old_vert_pos = _state.pos(2);
|
||||
@ -244,10 +256,14 @@ void Ekf::resetHeightToBaro()
|
||||
ECL_INFO("reset height to baro");
|
||||
_information_events.flags.reset_hgt_to_baro = true;
|
||||
|
||||
resetVerticalPositionTo(-(_baro_sample_delayed.hgt - _baro_hgt_offset));
|
||||
resetVerticalPositionTo(-(_baro_sample_delayed.hgt - _baro_b_est.getBias()));
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
|
||||
|
||||
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
|
||||
}
|
||||
|
||||
void Ekf::resetHeightToGps()
|
||||
@ -255,14 +271,16 @@ void Ekf::resetHeightToGps()
|
||||
ECL_INFO("reset height to GPS");
|
||||
_information_events.flags.reset_hgt_to_gps = true;
|
||||
|
||||
const float z_pos_before_reset = _state.pos(2);
|
||||
resetVerticalPositionTo(-_gps_sample_delayed.hgt + getEkfGlobalOriginAltitude());
|
||||
resetVerticalPositionTo(-(_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_b_est.getBias()));
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(9, getGpsHeightVariance());
|
||||
|
||||
// adjust the baro offset
|
||||
_baro_hgt_offset += _state.pos(2) - z_pos_before_reset;
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
|
||||
|
||||
_aid_src_gnss_pos.time_last_fuse[2] = _time_last_imu;
|
||||
}
|
||||
|
||||
void Ekf::resetHeightToRng()
|
||||
@ -281,14 +299,14 @@ void Ekf::resetHeightToRng()
|
||||
}
|
||||
|
||||
// update the state and associated variance
|
||||
const float z_pos_before_reset = _state.pos(2);
|
||||
resetVerticalPositionTo(-dist_bottom + _rng_hgt_offset);
|
||||
resetVerticalPositionTo(-(dist_bottom - _rng_hgt_b_est.getBias()));
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
|
||||
|
||||
// adjust the baro offset
|
||||
_baro_hgt_offset += _state.pos(2) - z_pos_before_reset;
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
|
||||
}
|
||||
|
||||
void Ekf::resetHeightToEv()
|
||||
@ -296,14 +314,14 @@ void Ekf::resetHeightToEv()
|
||||
ECL_INFO("reset height to EV");
|
||||
_information_events.flags.reset_hgt_to_ev = true;
|
||||
|
||||
const float z_pos_before_reset = _state.pos(2);
|
||||
resetVerticalPositionTo(_ev_sample_delayed.pos(2));
|
||||
resetVerticalPositionTo(_ev_sample_delayed.pos(2) - _ev_hgt_b_est.getBias());
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(9, fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f)));
|
||||
|
||||
// adjust the baro offset
|
||||
_baro_hgt_offset += _state.pos(2) - z_pos_before_reset;
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample_delayed)
|
||||
@ -733,10 +751,9 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
|
||||
|
||||
resetVerticalPositionTo(_gps_alt_ref - current_alt);
|
||||
|
||||
_baro_hgt_offset += _state_reset_status.posD_change;
|
||||
|
||||
_rng_hgt_offset += _state_reset_status.posD_change;
|
||||
_ev_hgt_offset -= _state_reset_status.posD_change;
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
|
||||
}
|
||||
|
||||
return true;
|
||||
@ -1232,42 +1249,6 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::setControlBaroHeight()
|
||||
{
|
||||
_control_status.flags.baro_hgt = true;
|
||||
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
}
|
||||
|
||||
void Ekf::setControlRangeHeight()
|
||||
{
|
||||
_control_status.flags.rng_hgt = true;
|
||||
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
}
|
||||
|
||||
void Ekf::setControlGPSHeight()
|
||||
{
|
||||
_control_status.flags.gps_hgt = true;
|
||||
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
}
|
||||
|
||||
void Ekf::setControlEVHeight()
|
||||
{
|
||||
_control_status.flags.ev_hgt = true;
|
||||
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
}
|
||||
|
||||
void Ekf::stopMagFusion()
|
||||
{
|
||||
stopMag3DFusion();
|
||||
@ -1325,96 +1306,130 @@ void Ekf::startMag3DFusion()
|
||||
void Ekf::startBaroHgtFusion()
|
||||
{
|
||||
if (!_control_status.flags.baro_hgt) {
|
||||
if (!_control_status.flags.rng_hgt) {
|
||||
if (_params.height_sensor_ref == HeightSensor::BARO) {
|
||||
_height_sensor_ref = HeightSensor::BARO;
|
||||
resetHeightToBaro();
|
||||
|
||||
} else {
|
||||
_baro_b_est.setBias(_state.pos(2) + _baro_lpf.getState());
|
||||
}
|
||||
|
||||
setControlBaroHeight();
|
||||
|
||||
// We don't need to set a height sensor offset
|
||||
// since we track a separate _baro_hgt_offset
|
||||
|
||||
_control_status.flags.baro_hgt = true;
|
||||
_baro_b_est.setFusionActive();
|
||||
ECL_INFO("starting baro height fusion");
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopBaroHgtFusion()
|
||||
{
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
if (_height_sensor_ref == HeightSensor::BARO) {
|
||||
_height_sensor_ref = HeightSensor::UNKNOWN;
|
||||
}
|
||||
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_baro_b_est.setFusionInactive();
|
||||
ECL_INFO("stopping baro height fusion");
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::startGpsHgtFusion()
|
||||
{
|
||||
if (!_control_status.flags.gps_hgt) {
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
// switch out of range aid
|
||||
// calculate height sensor offset such that current
|
||||
// measurement matches our current height estimate
|
||||
_gps_hgt_offset = _gps_sample_delayed.hgt - getEkfGlobalOriginAltitude() + _state.pos(2);
|
||||
|
||||
if (_params.height_sensor_ref == HeightSensor::GNSS) {
|
||||
_gps_hgt_b_est.reset();
|
||||
_height_sensor_ref = HeightSensor::GNSS;
|
||||
resetHeightToGps();
|
||||
|
||||
} else {
|
||||
_gps_hgt_offset = 0.f;
|
||||
resetHeightToGps();
|
||||
_gps_hgt_b_est.setBias(_state.pos(2) + (_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude()));
|
||||
|
||||
// Reset the timeout value here because the fusion isn't done at the same place and would immediately trigger a timeout
|
||||
_aid_src_gnss_pos.time_last_fuse[2] = _time_last_imu;
|
||||
}
|
||||
|
||||
setControlGPSHeight();
|
||||
|
||||
_control_status.flags.gps_hgt = true;
|
||||
_gps_hgt_b_est.setFusionActive();
|
||||
ECL_INFO("starting GPS height fusion");
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopGpsHgtFusion()
|
||||
{
|
||||
if (_control_status.flags.gps_hgt) {
|
||||
|
||||
if (_height_sensor_ref == HeightSensor::GNSS) {
|
||||
_height_sensor_ref = HeightSensor::UNKNOWN;
|
||||
}
|
||||
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_gps_hgt_b_est.setFusionInactive();
|
||||
ECL_INFO("stopping GPS height fusion");
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::startRngHgtFusion()
|
||||
{
|
||||
if (!_control_status.flags.rng_hgt) {
|
||||
setControlRangeHeight();
|
||||
|
||||
// Range finder is the primary height source, the ground is now the datum used
|
||||
// to compute the local vertical position
|
||||
_rng_hgt_offset = 0.f;
|
||||
|
||||
if (!_control_status_prev.flags.ev_hgt) {
|
||||
// EV and range finders are using the same height datum
|
||||
if (_params.height_sensor_ref == HeightSensor::RANGE) {
|
||||
// Range finder is the primary height source, the ground is now the datum used
|
||||
// to compute the local vertical position
|
||||
_rng_hgt_b_est.reset();
|
||||
_height_sensor_ref = HeightSensor::RANGE;
|
||||
resetHeightToRng();
|
||||
|
||||
} else {
|
||||
_rng_hgt_b_est.setBias(_state.pos(2) + _range_sensor.getDistBottom());
|
||||
}
|
||||
|
||||
_control_status.flags.rng_hgt = true;
|
||||
_rng_hgt_b_est.setFusionActive();
|
||||
ECL_INFO("starting RNG height fusion");
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::startRngAidHgtFusion()
|
||||
void Ekf::stopRngHgtFusion()
|
||||
{
|
||||
if (!_control_status.flags.rng_hgt) {
|
||||
setControlRangeHeight();
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
if (_height_sensor_ref == HeightSensor::RANGE) {
|
||||
_height_sensor_ref = HeightSensor::UNKNOWN;
|
||||
}
|
||||
|
||||
// calculate height sensor offset such that current
|
||||
// measurement matches our current height estimate
|
||||
_rng_hgt_offset = _terrain_vpos;
|
||||
|
||||
ECL_INFO("starting RNG aid height fusion");
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_rng_hgt_b_est.setFusionInactive();
|
||||
ECL_INFO("stopping range height fusion");
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::startEvHgtFusion()
|
||||
{
|
||||
if (!_control_status.flags.ev_hgt) {
|
||||
setControlEVHeight();
|
||||
|
||||
if (!_control_status_prev.flags.rng_hgt) {
|
||||
// EV and range finders are using the same height datum
|
||||
if (_params.height_sensor_ref == HeightSensor::EV) {
|
||||
_rng_hgt_b_est.reset();
|
||||
_height_sensor_ref = HeightSensor::EV;
|
||||
resetHeightToEv();
|
||||
|
||||
} else {
|
||||
_ev_hgt_b_est.setBias(-_state.pos(2) + _ev_sample_delayed.pos(2));
|
||||
}
|
||||
|
||||
_control_status.flags.ev_hgt = true;
|
||||
_ev_hgt_b_est.setFusionActive();
|
||||
ECL_INFO("starting EV height fusion");
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::updateBaroHgtOffset()
|
||||
void Ekf::stopEvHgtFusion()
|
||||
{
|
||||
// calculate a filtered offset between the baro origin and local NED origin if we are not
|
||||
// using the baro as a height reference
|
||||
if (!_control_status.flags.baro_hgt && _baro_data_ready && (_delta_time_baro_us != 0)) {
|
||||
const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
if (_height_sensor_ref == HeightSensor::EV) {
|
||||
_height_sensor_ref = HeightSensor::UNKNOWN;
|
||||
}
|
||||
|
||||
// apply a 10 second first order low pass filter to baro offset
|
||||
const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
|
||||
|
||||
const float offset_rate_correction = 0.1f * (unbiased_baro + _state.pos(2) - _baro_hgt_offset);
|
||||
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
|
||||
_control_status.flags.ev_hgt = false;
|
||||
_ev_hgt_b_est.setFusionInactive();
|
||||
ECL_INFO("stopping EV height fusion");
|
||||
}
|
||||
}
|
||||
|
||||
@ -1428,27 +1443,6 @@ float Ekf::getGpsHeightVariance()
|
||||
return gps_alt_var;
|
||||
}
|
||||
|
||||
void Ekf::updateBaroHgtBias()
|
||||
{
|
||||
// Baro bias estimation using GPS altitude
|
||||
if (_baro_data_ready && (_delta_time_baro_us != 0)) {
|
||||
const float dt = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
|
||||
_baro_b_est.setMaxStateNoise(_params.baro_noise);
|
||||
_baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate);
|
||||
_baro_b_est.predict(dt);
|
||||
}
|
||||
|
||||
if (_gps_data_ready && !_gps_intermittent
|
||||
&& _gps_checks_passed && _NED_origin_initialised
|
||||
&& !_baro_hgt_faulty) {
|
||||
// Use GPS altitude as a reference to compute the baro bias measurement
|
||||
const float baro_bias = (_baro_sample_delayed.hgt - _baro_hgt_offset)
|
||||
- (_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude());
|
||||
const float baro_bias_var = getGpsHeightVariance() + sq(_params.baro_noise);
|
||||
_baro_b_est.fuseBias(baro_bias, baro_bias_var);
|
||||
}
|
||||
}
|
||||
|
||||
float Ekf::getRngHeightVariance() const
|
||||
{
|
||||
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
|
||||
@ -1660,14 +1654,13 @@ void Ekf::stopGpsFusion()
|
||||
|
||||
void Ekf::stopGpsPosFusion()
|
||||
{
|
||||
ECL_INFO("stopping GPS position fusion");
|
||||
if (_control_status.flags.gps) {
|
||||
ECL_INFO("stopping GPS position fusion");
|
||||
_control_status.flags.gps = false;
|
||||
stopGpsHgtFusion();
|
||||
|
||||
if (_control_status.flags.gps_hgt) {
|
||||
ECL_INFO("stopping GPS height fusion");
|
||||
startBaroHgtFusion();
|
||||
resetEstimatorAidStatus(_aid_src_gnss_pos);
|
||||
}
|
||||
|
||||
resetEstimatorAidStatus(_aid_src_gnss_pos);
|
||||
}
|
||||
|
||||
void Ekf::stopGpsVelFusion()
|
||||
|
||||
@ -437,7 +437,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||
float max_time_delay_ms = math::max((float)_params.sensor_interval_max_ms, _params.auxvel_delay_ms);
|
||||
|
||||
// using baro
|
||||
if (_params.vdist_sensor_type == 0) {
|
||||
if (_params.fusion_mode & SensorFusionMask::USE_BARO_HGT) {
|
||||
max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
@ -452,7 +452,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||
}
|
||||
|
||||
// range aid or range height
|
||||
if (_params.range_aid || (_params.vdist_sensor_type == VerticalHeightSensor::RANGE)) {
|
||||
if (_params.range_aid || (_params.fusion_mode & SensorFusionMask::USE_RNG_HGT)) {
|
||||
max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
|
||||
@ -102,12 +102,23 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
|
||||
auto &gps_pos = _aid_src_gnss_pos;
|
||||
resetEstimatorAidStatus(gps_pos);
|
||||
|
||||
const float height_measurement = gps_sample.hgt - getEkfGlobalOriginAltitude();
|
||||
const float height_measurement_var = getGpsHeightVariance();
|
||||
|
||||
// save current bias and update bias estimator
|
||||
const float bias = _gps_hgt_b_est.getBias();
|
||||
const float bias_var = _gps_hgt_b_est.getBiasVar();
|
||||
|
||||
_gps_hgt_b_est.setMaxStateNoise(height_measurement_var);
|
||||
_gps_hgt_b_est.setProcessNoiseStdDev(height_measurement_var); //TODO: update this
|
||||
_gps_hgt_b_est.fuseBias(height_measurement - (-_state.pos(2)), height_measurement_var + P(9, 9));
|
||||
|
||||
Vector3f position;
|
||||
position(0) = gps_sample.pos(0);
|
||||
position(1) = gps_sample.pos(1);
|
||||
|
||||
// vertical position - gps measurement has opposite sign to earth z axis
|
||||
position(2) = -(gps_sample.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_offset);
|
||||
position(2) = -(height_measurement - bias);
|
||||
|
||||
const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
|
||||
|
||||
@ -125,7 +136,7 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
|
||||
obs_var(0) = obs_var(1) = sq(math::constrain(gps_sample.hacc, lower_limit, upper_limit));
|
||||
}
|
||||
|
||||
obs_var(2) = getGpsHeightVariance();
|
||||
obs_var(2) = height_measurement_var + bias_var;
|
||||
|
||||
// innovation gate size
|
||||
float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
|
||||
|
||||
@ -44,17 +44,18 @@ void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s
|
||||
resetEstimatorAidStatusFlags(baro_hgt);
|
||||
|
||||
// innovation gate size
|
||||
float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
|
||||
const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
|
||||
|
||||
// observation variance - user parameter defined
|
||||
float obs_var = sq(fmaxf(_params.baro_noise, 0.01f));
|
||||
// measurement variance - user parameter defined
|
||||
const float measurement_var = sq(fmaxf(_params.baro_noise, 0.01f));
|
||||
const float measurement = _baro_sample_delayed.hgt;
|
||||
|
||||
// vertical position innovation - baro measurement has opposite sign to earth z axis
|
||||
baro_hgt.observation = -(_baro_sample_delayed.hgt - _baro_b_est.getBias() - _baro_hgt_offset);
|
||||
baro_hgt.observation_variance = obs_var;
|
||||
baro_hgt.observation = -(measurement - _baro_b_est.getBias());
|
||||
baro_hgt.observation_variance = measurement_var + _baro_b_est.getBiasVar();
|
||||
|
||||
baro_hgt.innovation = _state.pos(2) - baro_hgt.observation;
|
||||
baro_hgt.innovation_variance = P(9, 9) + obs_var;
|
||||
baro_hgt.innovation_variance = P(9, 9) + baro_hgt.observation_variance;
|
||||
|
||||
// Compensate for positive static pressure transients (negative vertical position innovations)
|
||||
// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
|
||||
@ -86,6 +87,12 @@ void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s
|
||||
baro_hgt.fusion_enabled = _control_status.flags.baro_hgt;
|
||||
|
||||
baro_hgt.timestamp_sample = baro_sample.time_us;
|
||||
|
||||
// update the bias estimator before updating the main filter but after
|
||||
// using its current state to compute the vertical position innovation
|
||||
_baro_b_est.setMaxStateNoise(_params.baro_noise);
|
||||
_baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate);
|
||||
_baro_b_est.fuseBias(measurement - (-_state.pos(2)) , measurement_var + P(9, 9));
|
||||
}
|
||||
|
||||
void Ekf::fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt)
|
||||
@ -104,18 +111,19 @@ void Ekf::updateRngHgt(estimator_aid_source_1d_s &rng_hgt)
|
||||
// reset flags
|
||||
resetEstimatorAidStatusFlags(rng_hgt);
|
||||
|
||||
// observation variance - user parameter defined
|
||||
float obs_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
|
||||
// measurement variance - user parameter defined
|
||||
const float measurement_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
|
||||
const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
|
||||
|
||||
// innovation gate size
|
||||
float innov_gate = fmaxf(_params.range_innov_gate, 1.f);
|
||||
const float innov_gate = fmaxf(_params.range_innov_gate, 1.f);
|
||||
|
||||
// vertical position innovation, use range finder with tilt correction
|
||||
rng_hgt.observation = (-math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance)) + _rng_hgt_offset;
|
||||
rng_hgt.observation_variance = obs_var;
|
||||
rng_hgt.observation = -(measurement - _rng_hgt_b_est.getBias());
|
||||
rng_hgt.observation_variance = measurement_var + _rng_hgt_b_est.getBiasVar();
|
||||
|
||||
rng_hgt.innovation = _state.pos(2) - rng_hgt.observation;
|
||||
rng_hgt.innovation_variance = P(9, 9) + obs_var;
|
||||
rng_hgt.innovation_variance = P(9, 9) + rng_hgt.observation_variance;
|
||||
|
||||
setEstimatorAidStatusTestRatio(rng_hgt, innov_gate);
|
||||
|
||||
@ -130,6 +138,14 @@ void Ekf::updateRngHgt(estimator_aid_source_1d_s &rng_hgt)
|
||||
rng_hgt.fusion_enabled = _control_status.flags.rng_hgt;
|
||||
|
||||
rng_hgt.timestamp_sample = _range_sensor.getSampleAddress()->time_us;
|
||||
|
||||
// update the bias estimator before updating the main filter but after
|
||||
// using its current state to compute the vertical position innovation
|
||||
const float rng_noise = sqrtf(measurement_var);
|
||||
_rng_hgt_b_est.setMaxStateNoise(rng_noise);
|
||||
_rng_hgt_b_est.setProcessNoiseStdDev(rng_noise); // TODO: fix
|
||||
|
||||
_rng_hgt_b_est.fuseBias(measurement - (-_state.pos(2)) , measurement_var + P(9, 9));
|
||||
}
|
||||
|
||||
void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt)
|
||||
@ -145,15 +161,25 @@ void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt)
|
||||
|
||||
void Ekf::fuseEvHgt()
|
||||
{
|
||||
const float measurement = _ev_sample_delayed.pos(2);
|
||||
const float measurement_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
|
||||
|
||||
const float bias = _ev_hgt_b_est.getBias();
|
||||
const float bias_var = _ev_hgt_b_est.getBiasVar();
|
||||
|
||||
const float ev_noise = sqrtf(measurement_var);
|
||||
_ev_hgt_b_est.setMaxStateNoise(ev_noise);
|
||||
_ev_hgt_b_est.setProcessNoiseStdDev(ev_noise); // TODO: fix
|
||||
_ev_hgt_b_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
|
||||
|
||||
// calculate the innovation assuming the external vision observation is in local NED frame
|
||||
_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
|
||||
const float obs = measurement - bias;
|
||||
const float obs_var = measurement_var + bias_var;
|
||||
_ev_pos_innov(2) = _state.pos(2) - obs;
|
||||
|
||||
// innovation gate size
|
||||
float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f);
|
||||
|
||||
// observation variance - defined externally
|
||||
float obs_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
|
||||
|
||||
// _ev_pos_test_ratio(1) is the vertical test ratio
|
||||
fuseVerticalPosition(_ev_pos_innov(2), innov_gate, obs_var,
|
||||
_ev_pos_innov_var(2), _ev_pos_test_ratio(1));
|
||||
|
||||
@ -106,7 +106,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
||||
_param_ekf2_req_hdrift(_params->req_hdrift),
|
||||
_param_ekf2_req_vdrift(_params->req_vdrift),
|
||||
_param_ekf2_aid_mask(_params->fusion_mode),
|
||||
_param_ekf2_hgt_mode(_params->vdist_sensor_type),
|
||||
_param_ekf2_hgt_mode(_params->height_sensor_ref),
|
||||
_param_ekf2_terr_mask(_params->terrain_fusion_mode),
|
||||
_param_ekf2_noaid_tout(_params->valid_timeout_max),
|
||||
_param_ekf2_rng_noise(_params->range_noise),
|
||||
@ -292,7 +292,7 @@ void EKF2::Run()
|
||||
}
|
||||
|
||||
// if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output
|
||||
if (_params->vdist_sensor_type == 0) {
|
||||
if (_params->fusion_mode & SensorFusionMask::USE_BARO_HGT) {
|
||||
float sens_baro_rate = 0.f;
|
||||
|
||||
if (param_get(param_find("SENS_BARO_RATE"), &sens_baro_rate) == PX4_OK) {
|
||||
|
||||
@ -608,22 +608,31 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
|
||||
* 5 : Set to true to enable multi-rotor drag specific force fusion
|
||||
* 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used
|
||||
* 7 : Set to true to enable GPS yaw fusion. Cannot be used if bit position 4 is true.
|
||||
* 8 : Set to true to enable vision velocity fusion
|
||||
* 9 : Set to true to enable barometer height fusion.
|
||||
* 10 : Set to true to enable GPS height fusion.
|
||||
* 11 : Set to true to enable range finder height fusion.
|
||||
* 12 : Set to true to enable vision height fusion.
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 0
|
||||
* @max 511
|
||||
* @bit 0 use GPS
|
||||
* @bit 1 use optical flow
|
||||
* @max 8091
|
||||
* @bit 0 position: GPS
|
||||
* @bit 1 velocity: optical flow
|
||||
* @bit 2 inhibit IMU bias estimation
|
||||
* @bit 3 vision position fusion
|
||||
* @bit 4 vision yaw fusion
|
||||
* @bit 5 multi-rotor drag fusion
|
||||
* @bit 3 position: vision
|
||||
* @bit 4 yaw: vision
|
||||
* @bit 5 wind: multi-rotor drag fusion
|
||||
* @bit 6 rotate external vision
|
||||
* @bit 7 GPS yaw fusion
|
||||
* @bit 8 vision velocity fusion
|
||||
* @bit 7 yaw: GPS
|
||||
* @bit 8 velocity: vision
|
||||
* @bit 9 height: barometer
|
||||
* @bit 10 height: GPS
|
||||
* @bit 11 height: range finder
|
||||
* @bit 12 height: vision
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
|
||||
PARAM_DEFINE_INT32(EKF2_AID_MASK, 513);
|
||||
|
||||
/**
|
||||
* Determines the primary source of height data used by the EKF.
|
||||
|
||||
@ -10,9 +10,19 @@ EkfWrapper::~EkfWrapper()
|
||||
{
|
||||
}
|
||||
|
||||
void EkfWrapper::setBaroHeight()
|
||||
void EkfWrapper::setBaroHeightRef()
|
||||
{
|
||||
_ekf_params->vdist_sensor_type = VerticalHeightSensor::BARO;
|
||||
_ekf_params->height_sensor_ref |= HeightSensorRef::BARO;
|
||||
}
|
||||
|
||||
void EkfWrapper::enableBaroHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_BARO_HGT;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableBaroHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_BARO_HGT;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingBaroHeightFusion() const
|
||||
@ -20,9 +30,19 @@ bool EkfWrapper::isIntendingBaroHeightFusion() const
|
||||
return _ekf->control_status_flags().baro_hgt;
|
||||
}
|
||||
|
||||
void EkfWrapper::setGpsHeight()
|
||||
void EkfWrapper::setGpsHeightRef()
|
||||
{
|
||||
_ekf_params->vdist_sensor_type = VerticalHeightSensor::GPS;
|
||||
_ekf_params->height_sensor_ref |= HeightSensorRef::GPS;
|
||||
}
|
||||
|
||||
void EkfWrapper::enableGpsHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS_HGT;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableGpsHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS_HGT;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingGpsHeightFusion() const
|
||||
@ -30,9 +50,19 @@ bool EkfWrapper::isIntendingGpsHeightFusion() const
|
||||
return _ekf->control_status_flags().gps_hgt;
|
||||
}
|
||||
|
||||
void EkfWrapper::setRangeHeight()
|
||||
void EkfWrapper::setRangeHeightRef()
|
||||
{
|
||||
_ekf_params->vdist_sensor_type = VerticalHeightSensor::RANGE;
|
||||
_ekf_params->height_sensor_ref |= HeightSensorRef::RANGE;
|
||||
}
|
||||
|
||||
void EkfWrapper::enableRangeHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_RNG_HGT;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableRangeHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_RNG_HGT;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingRangeHeightFusion() const
|
||||
@ -40,9 +70,19 @@ bool EkfWrapper::isIntendingRangeHeightFusion() const
|
||||
return _ekf->control_status_flags().rng_hgt;
|
||||
}
|
||||
|
||||
void EkfWrapper::setVisionHeight()
|
||||
void EkfWrapper::setVisionHeightRef()
|
||||
{
|
||||
_ekf_params->vdist_sensor_type = VerticalHeightSensor::EV;
|
||||
_ekf_params->height_sensor_ref |= HeightSensorRef::EV;
|
||||
}
|
||||
|
||||
void EkfWrapper::enableVisionHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_HGT;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableVisionHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_HGT;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingVisionHeightFusion() const
|
||||
|
||||
@ -49,16 +49,24 @@ public:
|
||||
~EkfWrapper();
|
||||
|
||||
|
||||
void setBaroHeight();
|
||||
void setBaroHeightRef();
|
||||
void enableBaroHeightFusion();
|
||||
void disableBaroHeightFusion();
|
||||
bool isIntendingBaroHeightFusion() const;
|
||||
|
||||
void setGpsHeight();
|
||||
void setGpsHeightRef();
|
||||
void enableGpsHeightFusion();
|
||||
void disableGpsHeightFusion();
|
||||
bool isIntendingGpsHeightFusion() const;
|
||||
|
||||
void setRangeHeight();
|
||||
void setRangeHeightRef();
|
||||
void enableRangeHeightFusion();
|
||||
void disableRangeHeightFusion();
|
||||
bool isIntendingRangeHeightFusion() const;
|
||||
|
||||
void setVisionHeight();
|
||||
void setVisionHeightRef();
|
||||
void enableVisionHeightFusion();
|
||||
void disableVisionHeightFusion();
|
||||
bool isIntendingVisionHeightFusion() const;
|
||||
|
||||
void enableGpsFusion();
|
||||
|
||||
@ -358,6 +358,7 @@ TEST_F(EkfFusionLogicTest, doBaroHeightFusion)
|
||||
// GIVEN: EKF that receives baro and GPS data
|
||||
_sensor_simulator.startGps();
|
||||
_sensor_simulator.runSeconds(11);
|
||||
_ekf_wrapper.enableGpsHeightFusion();
|
||||
|
||||
// THEN: EKF should intend to fuse baro by default
|
||||
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
|
||||
@ -396,26 +397,28 @@ TEST_F(EkfFusionLogicTest, doBaroHeightFusionTimeout)
|
||||
|
||||
// BUT WHEN: GPS height data is also available
|
||||
_sensor_simulator.startGps();
|
||||
_ekf_wrapper.enableGpsHeightFusion();
|
||||
_sensor_simulator.runSeconds(11);
|
||||
reset_logging_checker.capturePostResetState();
|
||||
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(2));
|
||||
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
|
||||
|
||||
// AND: the baro data jumps by a lot
|
||||
_sensor_simulator._baro.setData(800.f);
|
||||
_sensor_simulator.runSeconds(20);
|
||||
reset_logging_checker.capturePostResetState();
|
||||
|
||||
// THEN: EKF should fallback to GPS height
|
||||
// THEN: EKF should fallback to GPS height (without reset)
|
||||
EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
|
||||
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
|
||||
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(3));
|
||||
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2));
|
||||
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(2));
|
||||
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
|
||||
}
|
||||
|
||||
TEST_F(EkfFusionLogicTest, doGpsHeightFusion)
|
||||
{
|
||||
// WHEN: commanding GPS height and sending GPS data
|
||||
_ekf_wrapper.setGpsHeight();
|
||||
_ekf_wrapper.enableGpsHeightFusion();
|
||||
_sensor_simulator.startGps();
|
||||
_sensor_simulator.runSeconds(11);
|
||||
|
||||
@ -434,7 +437,7 @@ TEST_F(EkfFusionLogicTest, doGpsHeightFusion)
|
||||
TEST_F(EkfFusionLogicTest, doRangeHeightFusion)
|
||||
{
|
||||
// WHEN: commanding range height and sending range data
|
||||
_ekf_wrapper.setRangeHeight();
|
||||
_ekf_wrapper.enableRangeHeightFusion();
|
||||
_sensor_simulator.startRangeFinder();
|
||||
_sensor_simulator.runSeconds(2.5f);
|
||||
// THEN: EKF should intend to fuse range height
|
||||
@ -462,7 +465,7 @@ TEST_F(EkfFusionLogicTest, doRangeHeightFusion)
|
||||
TEST_F(EkfFusionLogicTest, doVisionHeightFusion)
|
||||
{
|
||||
// WHEN: commanding vision height and sending vision data
|
||||
_ekf_wrapper.setVisionHeight();
|
||||
_ekf_wrapper.enableVisionHeightFusion();
|
||||
_sensor_simulator.startExternalVision();
|
||||
_sensor_simulator.runSeconds(2);
|
||||
|
||||
|
||||
@ -157,12 +157,12 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback)
|
||||
_ekf_wrapper.enableFlowFusion();
|
||||
_sensor_simulator.startFlow();
|
||||
|
||||
_ekf_wrapper.setGpsHeight();
|
||||
_ekf_wrapper.enableGpsHeightFusion();
|
||||
|
||||
_sensor_simulator.runSeconds(1);
|
||||
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
|
||||
EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion());
|
||||
EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
|
||||
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
|
||||
|
||||
// WHEN: stopping GPS fusion
|
||||
_sensor_simulator.stopGps();
|
||||
|
||||
@ -80,7 +80,6 @@ public:
|
||||
_ekf->set_vehicle_at_rest(true);
|
||||
|
||||
_ekf_wrapper.enableGpsFusion();
|
||||
_ekf_wrapper.setBaroHeight();
|
||||
_sensor_simulator.runSeconds(2); // Run to pass the GPS checks
|
||||
_sensor_simulator.runSeconds(3.5); // And a bit more to start the GPS fusion TODO: this shouldn't be necessary
|
||||
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user