mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-01 23:54:07 +08:00
[WIP]: ekf2 only require baro or mag if enabled and always update all available height sources
This commit is contained in:
Daniel Agar
parent
6db9ed099d
commit
e4e9dc62ed
@ -78,6 +78,8 @@ public:
|
||||
return median();
|
||||
}
|
||||
|
||||
size_t window_size() const { return WINDOW; }
|
||||
|
||||
private:
|
||||
|
||||
static int cmp(const void *a, const void *b)
|
||||
|
||||
@ -153,6 +153,27 @@ public:
|
||||
return false;
|
||||
}
|
||||
|
||||
bool peek_first_older_than(const uint64_t ×tamp, data_type *sample) const
|
||||
{
|
||||
// start looking from newest observation data
|
||||
for (uint8_t i = 0; i < _size; i++) {
|
||||
int index = (_head - i);
|
||||
index = index < 0 ? _size + index : index;
|
||||
|
||||
if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)1e5) {
|
||||
*sample = _buffer[index];
|
||||
return true;
|
||||
}
|
||||
|
||||
if (index == _tail) {
|
||||
// we have reached the tail and haven't got a match
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; }
|
||||
|
||||
private:
|
||||
|
||||
@ -94,7 +94,16 @@ void Ekf::controlFusionModes()
|
||||
|
||||
// check for arrival of new sensor data at the fusion time horizon
|
||||
_time_prev_gps_us = _gps_sample_delayed.time_us;
|
||||
|
||||
const uint64_t delta_time_prev_gps_us = _gps_sample_delayed.time_us;
|
||||
_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
|
||||
|
||||
// if we have a new sample save the delta time between this sample and the last sample which is used for height offset calculations
|
||||
if (_gps_data_ready && (delta_time_prev_gps_us != 0)) {
|
||||
_delta_time_gps_us = _gps_sample_delayed.time_us - delta_time_prev_gps_us;
|
||||
}
|
||||
|
||||
|
||||
_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
|
||||
|
||||
if (_mag_data_ready) {
|
||||
@ -114,20 +123,28 @@ void Ekf::controlFusionModes()
|
||||
}
|
||||
}
|
||||
|
||||
_delta_time_baro_us = _baro_sample_delayed.time_us;
|
||||
|
||||
const uint64_t delta_time_prev_baro_us = _baro_sample_delayed.time_us;
|
||||
_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
|
||||
|
||||
// if we have a new baro sample save the delta time between this sample and the last sample which is
|
||||
// used below for baro offset calculations
|
||||
if (_baro_data_ready) {
|
||||
_delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us;
|
||||
if (_baro_data_ready && (delta_time_prev_baro_us != 0)) {
|
||||
_delta_time_baro_us = _baro_sample_delayed.time_us - delta_time_prev_baro_us;
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
// Get range data from buffer and check validity
|
||||
const uint64_t delta_time_prev_rng_us = _range_sensor.sample().time_us;
|
||||
const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
|
||||
_range_sensor.setDataReadiness(is_rng_data_ready);
|
||||
|
||||
// if we have a new sample save the delta time between this sample and the last sample which is used for height offset calculations
|
||||
if (is_rng_data_ready && (delta_time_prev_rng_us != 0)) {
|
||||
_delta_time_rng_us = _range_sensor.sample().time_us - delta_time_prev_rng_us;
|
||||
}
|
||||
|
||||
// update range sensor angle parameters in case they have changed
|
||||
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
|
||||
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
|
||||
@ -160,7 +177,16 @@ void Ekf::controlFusionModes()
|
||||
_flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps);
|
||||
}
|
||||
|
||||
|
||||
const uint64_t delta_time_prev_ev_us = _ev_sample_delayed.time_us;
|
||||
_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
|
||||
|
||||
// if we have a new EV sample save the delta time between this sample and the last sample which is used for height offset calculations
|
||||
if (_ev_data_ready && (delta_time_prev_ev_us != 0)) {
|
||||
_delta_time_ev_us = _ev_sample_delayed.time_us - delta_time_prev_ev_us;
|
||||
}
|
||||
|
||||
|
||||
_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
|
||||
|
||||
// check for height sensor timeouts and reset and change sensor if necessary
|
||||
@ -664,9 +690,7 @@ void Ekf::controlHeightSensorTimeouts()
|
||||
const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
|
||||
|
||||
// check the baro height source for consistency and freshness
|
||||
const baroSample &baro_init = _baro_buffer.get_newest();
|
||||
const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
|
||||
const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate);
|
||||
const bool baro_data_consistent = (_baro_hgt_test_ratio(1) < 1.f); // TODO: review
|
||||
|
||||
// if baro data is acceptable and GPS data is inaccurate, reset height to baro
|
||||
const bool reset_to_baro = !_baro_hgt_faulty &&
|
||||
@ -819,18 +843,11 @@ void Ekf::controlHeightFusion()
|
||||
// we have just switched to using range finder, calculate height sensor offset such that current
|
||||
// measurement matches our current height estimate
|
||||
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
|
||||
_hgt_sensor_offset = _terrain_vpos;
|
||||
_rng_hgt_offset = _terrain_vpos;
|
||||
}
|
||||
|
||||
} else if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) {
|
||||
startBaroHgtFusion();
|
||||
|
||||
} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
|
||||
// we have just switched to using gps height, calculate height sensor offset such that current
|
||||
// measurement matches our current height estimate
|
||||
if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
|
||||
_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
@ -844,13 +861,13 @@ void Ekf::controlHeightFusion()
|
||||
// measurement matches our current height estimate
|
||||
// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
|
||||
if (_control_status.flags.in_air && isTerrainEstimateValid()) {
|
||||
_hgt_sensor_offset = _terrain_vpos;
|
||||
_rng_hgt_offset = _terrain_vpos;
|
||||
|
||||
} else if (_control_status.flags.in_air) {
|
||||
_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
|
||||
_rng_hgt_offset = _range_sensor.getDistBottom() + _state.pos(2);
|
||||
|
||||
} else {
|
||||
_hgt_sensor_offset = _params.rng_gnd_clearance;
|
||||
_rng_hgt_offset = _params.rng_gnd_clearance;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -869,7 +886,7 @@ void Ekf::controlHeightFusion()
|
||||
|
||||
// we have just switched to using range finder, calculate height sensor offset such that current
|
||||
// measurement matches our current height estimate
|
||||
_hgt_sensor_offset = _terrain_vpos;
|
||||
_rng_hgt_offset = _terrain_vpos;
|
||||
}
|
||||
|
||||
} else {
|
||||
@ -907,43 +924,38 @@ void Ekf::controlHeightFusion()
|
||||
}
|
||||
|
||||
updateBaroHgtBias();
|
||||
updateBaroHgtOffset();
|
||||
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
// baro
|
||||
if (_baro_data_ready && !_baro_hgt_faulty) {
|
||||
fuseBaroHgt();
|
||||
}
|
||||
|
||||
if (_baro_data_ready && !_baro_hgt_faulty) {
|
||||
fuseBaroHgt();
|
||||
}
|
||||
// GPS
|
||||
if (_gps_data_ready) {
|
||||
fuseGpsHgt();
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.gps_hgt) {
|
||||
// range
|
||||
if (_range_sensor.isDataHealthy()) {
|
||||
fuseRngHgt();
|
||||
|
||||
if (_gps_data_ready) {
|
||||
fuseGpsHgt();
|
||||
}
|
||||
} else if (_control_status.flags.rng_hgt
|
||||
&&!_control_status.flags.in_air
|
||||
&& _range_sensor.isRegularlySendingData()
|
||||
&& isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL)) {
|
||||
|
||||
} else if (_control_status.flags.rng_hgt) {
|
||||
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
|
||||
// and are on the ground, then synthesise a measurement at the expected on ground value
|
||||
_range_sensor.setRange(_params.rng_gnd_clearance);
|
||||
_range_sensor.setDataReadiness(true);
|
||||
_range_sensor.setValidity(true); // bypass the checks
|
||||
|
||||
if (_range_sensor.isDataHealthy()) {
|
||||
fuseRngHgt();
|
||||
fuseRngHgt();
|
||||
}
|
||||
|
||||
} else if (!_control_status.flags.in_air
|
||||
&& _range_sensor.isRegularlySendingData()
|
||||
&& isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL)) {
|
||||
|
||||
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
|
||||
// and are on the ground, then synthesise a measurement at the expected on ground value
|
||||
_range_sensor.setRange(_params.rng_gnd_clearance);
|
||||
_range_sensor.setDataReadiness(true);
|
||||
_range_sensor.setValidity(true); // bypass the checks
|
||||
|
||||
fuseRngHgt();
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.ev_hgt) {
|
||||
|
||||
if (_control_status.flags.ev_hgt && _ev_data_ready) {
|
||||
fuseEvHgt();
|
||||
}
|
||||
// vision
|
||||
if (_control_status.flags.ev_hgt && _ev_data_ready) {
|
||||
fuseEvHgt();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -89,16 +89,17 @@ bool Ekf::update()
|
||||
{
|
||||
bool updated = false;
|
||||
|
||||
if (!_filter_initialised) {
|
||||
_filter_initialised = initialiseFilter();
|
||||
|
||||
if (!_filter_initialised) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Only run the filter if IMU data in the buffer has been updated
|
||||
if (_imu_updated) {
|
||||
|
||||
if (!_filter_initialised) {
|
||||
_filter_initialised = initialiseFilter();
|
||||
|
||||
if (!_filter_initialised) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// perform state and covariance prediction for the main filter
|
||||
predictState();
|
||||
predictCovariance();
|
||||
@ -115,59 +116,30 @@ bool Ekf::update()
|
||||
runYawEKFGSF();
|
||||
}
|
||||
|
||||
// the output observer always runs
|
||||
// Use full rate IMU data at the current time horizon
|
||||
calculateOutputStates(_newest_high_rate_imu_sample);
|
||||
if (_filter_initialised) {
|
||||
// the output observer always runs
|
||||
// Use full rate IMU data at the current time horizon
|
||||
calculateOutputStates(_newest_high_rate_imu_sample);
|
||||
}
|
||||
|
||||
return updated;
|
||||
}
|
||||
|
||||
bool Ekf::initialiseFilter()
|
||||
{
|
||||
// Filter accel for tilt initialization
|
||||
const imuSample &imu_init = _imu_buffer.get_newest();
|
||||
|
||||
// protect against zero data
|
||||
if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) {
|
||||
if (_imu_sample_delayed.delta_vel_dt < 1e-4f || _imu_sample_delayed.delta_ang_dt < 1e-4f) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_is_first_imu_sample) {
|
||||
_accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt);
|
||||
_gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt);
|
||||
_accel_lpf.reset(_imu_sample_delayed.delta_vel / _imu_sample_delayed.delta_vel_dt);
|
||||
_gyro_lpf.reset(_imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt);
|
||||
_is_first_imu_sample = false;
|
||||
|
||||
} else {
|
||||
_accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt);
|
||||
_gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt);
|
||||
}
|
||||
|
||||
// Sum the magnetometer measurements
|
||||
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
|
||||
if (_mag_sample_delayed.time_us != 0) {
|
||||
if (_mag_counter == 0) {
|
||||
_mag_lpf.reset(_mag_sample_delayed.mag);
|
||||
|
||||
} else {
|
||||
_mag_lpf.update(_mag_sample_delayed.mag);
|
||||
}
|
||||
|
||||
_mag_counter++;
|
||||
}
|
||||
}
|
||||
|
||||
// accumulate enough height measurements to be confident in the quality of the data
|
||||
if (_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;
|
||||
|
||||
} else {
|
||||
_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
|
||||
}
|
||||
|
||||
_baro_counter++;
|
||||
}
|
||||
_accel_lpf.update(_imu_sample_delayed.delta_vel / _imu_sample_delayed.delta_vel_dt);
|
||||
_gyro_lpf.update(_imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt);
|
||||
}
|
||||
|
||||
if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
|
||||
@ -177,13 +149,62 @@ bool Ekf::initialiseFilter()
|
||||
}
|
||||
}
|
||||
|
||||
if (_baro_counter < _obs_buffer_length) {
|
||||
// not enough baro samples accumulated
|
||||
return false;
|
||||
if (_baro_hgt_counter > 0) {
|
||||
_baro_hgt_offset = _baro_hgt_lpf.getState();
|
||||
}
|
||||
|
||||
// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
|
||||
setControlBaroHeight();
|
||||
if (_gps_hgt_counter > 0) {
|
||||
_gps_hgt_offset = _gps_hgt_lpf.getState();
|
||||
}
|
||||
|
||||
if (_rng_hgt_counter >= _rng_hgt_filter.window_size()) {
|
||||
_rng_hgt_offset = _rng_hgt_filter.median();
|
||||
}
|
||||
|
||||
if (_ev_hgt_counter > 0) {
|
||||
_ev_hgt_offset = _ev_hgt_lpf.getState();
|
||||
}
|
||||
|
||||
switch (_params.vdist_sensor_type) {
|
||||
default:
|
||||
|
||||
// FALLTHROUGH
|
||||
case VDIST_SENSOR_BARO:
|
||||
if (_baro_hgt_counter < _obs_buffer_length) {
|
||||
// not enough samples accumulated
|
||||
return false;
|
||||
}
|
||||
|
||||
setControlBaroHeight();
|
||||
break;
|
||||
|
||||
case VDIST_SENSOR_GPS:
|
||||
if (_gps_hgt_counter < _obs_buffer_length) {
|
||||
// not enough samples accumulated
|
||||
return false;
|
||||
}
|
||||
|
||||
setControlGPSHeight();
|
||||
break;
|
||||
|
||||
case VDIST_SENSOR_RANGE:
|
||||
if (_rng_hgt_counter < _obs_buffer_length) {
|
||||
// not enough samples accumulated
|
||||
return false;
|
||||
}
|
||||
|
||||
setControlRangeHeight();
|
||||
break;
|
||||
|
||||
case VDIST_SENSOR_EV:
|
||||
if (_ev_hgt_counter < _obs_buffer_length) {
|
||||
// not enough samples accumulated
|
||||
return false;
|
||||
}
|
||||
|
||||
setControlEVHeight();
|
||||
break;
|
||||
}
|
||||
|
||||
if (!initialiseTilt()) {
|
||||
return false;
|
||||
|
||||
@ -391,7 +391,11 @@ private:
|
||||
float _imu_collection_time_adj{0.0f}; ///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
|
||||
|
||||
uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec)
|
||||
|
||||
uint64_t _delta_time_baro_us{0}; ///< delta time between two consecutive delayed baro samples from the buffer (uSec)
|
||||
uint64_t _delta_time_gps_us{0};
|
||||
uint64_t _delta_time_rng_us{0};
|
||||
uint64_t _delta_time_ev_us{0};
|
||||
|
||||
Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s
|
||||
|
||||
@ -504,15 +508,17 @@ private:
|
||||
|
||||
// Variables used by the initial filter alignment
|
||||
bool _is_first_imu_sample{true};
|
||||
uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation
|
||||
uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation
|
||||
AlphaFilter<Vector3f> _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s)
|
||||
AlphaFilter<Vector3f> _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
|
||||
|
||||
// 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 _hgt_sensor_offset{0.0f}; ///< set as necessary if desired to maintain the same height after a height reset (m)
|
||||
float _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m)
|
||||
|
||||
float _baro_hgt_offset{NAN}; ///< baro height reading at the local NED origin (m)
|
||||
float _gps_hgt_offset{NAN};
|
||||
float _rng_hgt_offset{NAN};
|
||||
float _ev_hgt_offset{NAN};
|
||||
|
||||
float _baro_hgt_bias{0.0f};
|
||||
float _baro_hgt_bias_var{1.f};
|
||||
|
||||
@ -674,7 +680,7 @@ private:
|
||||
Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false);
|
||||
|
||||
bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
|
||||
Vector3f &innov_var, Vector2f &test_ratio);
|
||||
Vector3f &innov_var, Vector2f &test_ratio, bool fuse = true);
|
||||
|
||||
void fuseGpsVelPos();
|
||||
|
||||
@ -916,7 +922,6 @@ private:
|
||||
void startBaroHgtFusion();
|
||||
void startGpsHgtFusion();
|
||||
|
||||
void updateBaroHgtOffset();
|
||||
void updateBaroHgtBias();
|
||||
|
||||
// return an estimation of the GPS altitude variance
|
||||
|
||||
@ -255,9 +255,6 @@ void Ekf::resetVerticalPositionTo(const float &new_vert_pos)
|
||||
// Reset height state using the last height measurement
|
||||
void Ekf::resetHeight()
|
||||
{
|
||||
// Get the most recent GPS data
|
||||
const gpsSample &gps_newest = _gps_buffer.get_newest();
|
||||
|
||||
// reset the vertical position
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
|
||||
@ -265,33 +262,28 @@ void Ekf::resetHeight()
|
||||
if (!_control_status_prev.flags.rng_hgt) {
|
||||
|
||||
if (_control_status.flags.in_air && isTerrainEstimateValid()) {
|
||||
_hgt_sensor_offset = _terrain_vpos;
|
||||
_rng_hgt_offset = _terrain_vpos;
|
||||
|
||||
} else if (_control_status.flags.in_air) {
|
||||
_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
|
||||
_rng_hgt_offset = _range_sensor.getDistBottom() + _state.pos(2);
|
||||
|
||||
} else {
|
||||
_hgt_sensor_offset = _params.rng_gnd_clearance;
|
||||
_rng_hgt_offset = _params.rng_gnd_clearance;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// update the state and associated variance
|
||||
resetVerticalPositionTo(_hgt_sensor_offset - _range_sensor.getDistBottom());
|
||||
resetVerticalPositionTo(_rng_hgt_offset - _range_sensor.getDistBottom());
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
|
||||
|
||||
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
|
||||
const baroSample &baro_newest = _baro_buffer.get_newest();
|
||||
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
|
||||
|
||||
} else if (_control_status.flags.baro_hgt) {
|
||||
// initialize vertical position with newest baro measurement
|
||||
const baroSample &baro_newest = _baro_buffer.get_newest();
|
||||
|
||||
if (!_baro_hgt_faulty) {
|
||||
resetVerticalPositionTo(-baro_newest.hgt + _baro_hgt_offset);
|
||||
// initialize vertical position with newest baro measurement
|
||||
resetVerticalPositionTo(-_baro_hgt_lpf.getState() + _baro_hgt_offset);
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
|
||||
@ -303,39 +295,65 @@ void Ekf::resetHeight()
|
||||
} else if (_control_status.flags.gps_hgt) {
|
||||
// initialize vertical position and velocity with newest gps measurement
|
||||
if (!_gps_hgt_intermittent) {
|
||||
resetVerticalPositionTo(_hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref);
|
||||
gpsSample gps_sample;
|
||||
|
||||
if (!_gps_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &gps_sample)) {
|
||||
gps_sample = _gps_buffer.get_newest();
|
||||
}
|
||||
|
||||
resetVerticalPositionTo(_gps_hgt_offset - _gps_hgt_lpf.getState() + _gps_alt_ref);
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_newest.vacc));
|
||||
|
||||
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
|
||||
const baroSample &baro_newest = _baro_buffer.get_newest();
|
||||
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
|
||||
P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_sample.vacc));
|
||||
|
||||
} else {
|
||||
// TODO: reset to last known gps based estimate
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.ev_hgt) {
|
||||
// initialize vertical position with newest measurement
|
||||
const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
|
||||
extVisionSample ext_vision_sample;
|
||||
|
||||
// use the most recent data if it's time offset from the fusion time horizon is smaller
|
||||
if (ev_newest.time_us >= _ev_sample_delayed.time_us) {
|
||||
resetVerticalPositionTo(ev_newest.pos(2));
|
||||
|
||||
} else {
|
||||
resetVerticalPositionTo(_ev_sample_delayed.pos(2));
|
||||
if (!_ext_vision_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &ext_vision_sample)) {
|
||||
ext_vision_sample = _ext_vision_buffer.get_newest();
|
||||
}
|
||||
|
||||
resetVerticalPositionTo(_ev_hgt_lpf.getState() + _ev_hgt_offset);
|
||||
|
||||
P.uncorrelateCovarianceSetVariance<1>(9, sq(ext_vision_sample.posVar(2)));
|
||||
}
|
||||
|
||||
// reset the height sensor offsets which is subtracted from the readings if we need to use it as a backup
|
||||
_baro_hgt_offset = _state.pos(2) + _baro_hgt_lpf.getState();
|
||||
_gps_hgt_offset = _state.pos(2) + _gps_hgt_lpf.getState() - _gps_alt_ref;
|
||||
_rng_hgt_offset = _state.pos(2) + _range_sensor.getDistBottom();
|
||||
_ev_hgt_offset = _state.pos(2) + _ev_hgt_lpf.getState();
|
||||
|
||||
|
||||
// reset the vertical velocity state
|
||||
if (_control_status.flags.gps && !_gps_hgt_intermittent) {
|
||||
gpsSample gps_sample;
|
||||
|
||||
if (!_gps_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &gps_sample)) {
|
||||
gps_sample = _gps_buffer.get_newest();
|
||||
}
|
||||
|
||||
// If we are using GPS, then use it to reset the vertical velocity
|
||||
resetVerticalVelocityTo(gps_newest.vel(2));
|
||||
resetVerticalVelocityTo(gps_sample.vel(2));
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_newest.sacc));
|
||||
P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_sample.sacc));
|
||||
|
||||
} else if (_control_status.flags.ev_vel) {
|
||||
extVisionSample ext_vision_sample;
|
||||
|
||||
if (!_ext_vision_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &ext_vision_sample)) {
|
||||
ext_vision_sample = _ext_vision_buffer.get_newest();
|
||||
}
|
||||
|
||||
// use the most recent data if it's time offset from the fusion time horizon is smaller
|
||||
resetVerticalVelocityTo(ext_vision_sample.vel(2));
|
||||
|
||||
P.uncorrelateCovarianceSetVariance<1>(6, math::max(getVisionVelocityVarianceInEkfFrame()(2), sq(0.05f)));
|
||||
|
||||
} else {
|
||||
// we don't know what the vertical velocity is, so set it to zero
|
||||
@ -1281,10 +1299,6 @@ void Ekf::startBaroHgtFusion()
|
||||
{
|
||||
setControlBaroHeight();
|
||||
|
||||
// We don't need to set a height sensor offset
|
||||
// since we track a separate _baro_hgt_offset
|
||||
_hgt_sensor_offset = 0.0f;
|
||||
|
||||
// Turn off ground effect compensation if it times out
|
||||
if (_control_status.flags.gnd_effect) {
|
||||
if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
|
||||
@ -1297,27 +1311,6 @@ void Ekf::startBaroHgtFusion()
|
||||
void Ekf::startGpsHgtFusion()
|
||||
{
|
||||
setControlGPSHeight();
|
||||
|
||||
// we have just switched to using gps height, calculate height sensor offset such that current
|
||||
// measurement matches our current height estimate
|
||||
if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
|
||||
_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::updateBaroHgtOffset()
|
||||
{
|
||||
// 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) {
|
||||
const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
float Ekf::getGpsHeightVariance()
|
||||
|
||||
@ -144,6 +144,16 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
|
||||
_mag_data_sum.setZero();
|
||||
_mag_timestamp_sum = 0;
|
||||
}
|
||||
|
||||
|
||||
if (_mag_counter == 0) {
|
||||
_mag_lpf.reset(mag_sample.mag);
|
||||
|
||||
} else {
|
||||
_mag_lpf.update(mag_sample.mag);
|
||||
}
|
||||
|
||||
_mag_counter++;
|
||||
}
|
||||
|
||||
void EstimatorInterface::setGpsData(const gps_message &gps)
|
||||
@ -204,6 +214,20 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
|
||||
|
||||
_gps_buffer.push(gps_sample_new);
|
||||
}
|
||||
|
||||
if (gps.fix_type > 2) {
|
||||
// accumulate enough height measurements to be confident in the quality of the data
|
||||
const float hgt = gps.alt * 1e-3f;
|
||||
|
||||
if (_gps_hgt_counter == 0) {
|
||||
_gps_hgt_lpf.reset(hgt);
|
||||
|
||||
} else {
|
||||
_gps_hgt_lpf.update(hgt);
|
||||
}
|
||||
|
||||
_gps_hgt_counter++;
|
||||
}
|
||||
}
|
||||
|
||||
void EstimatorInterface::setBaroData(const baroSample &baro_sample)
|
||||
@ -249,6 +273,17 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
|
||||
_baro_alt_sum = 0.0f;
|
||||
_baro_timestamp_sum = 0;
|
||||
}
|
||||
|
||||
|
||||
// accumulate enough height measurements to be confident in the quality of the data
|
||||
if (_baro_hgt_counter == 0) {
|
||||
_baro_hgt_lpf.reset(baro_sample.hgt);
|
||||
|
||||
} else {
|
||||
_baro_hgt_lpf.update(baro_sample.hgt);
|
||||
}
|
||||
|
||||
_baro_hgt_counter++;
|
||||
}
|
||||
|
||||
void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
|
||||
@ -308,6 +343,8 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
|
||||
|
||||
_range_buffer.push(range_sample_new);
|
||||
}
|
||||
|
||||
_rng_hgt_filter.insert(range_sample.rng);
|
||||
}
|
||||
|
||||
void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
|
||||
@ -369,6 +406,17 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
|
||||
|
||||
_ext_vision_buffer.push(ev_sample_new);
|
||||
}
|
||||
|
||||
|
||||
// accumulate enough height measurements to be confident in the quality of the data
|
||||
if (_ev_hgt_counter == 0) {
|
||||
_ev_hgt_lpf.reset(evdata.pos(2));
|
||||
|
||||
} else {
|
||||
_ev_hgt_lpf.update(evdata.pos(2));
|
||||
}
|
||||
|
||||
_ev_hgt_counter++;
|
||||
}
|
||||
|
||||
void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
|
||||
|
||||
@ -63,6 +63,7 @@
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <mathlib/math/filter/AlphaFilter.hpp>
|
||||
#include <mathlib/math/filter/MedianFilter.hpp>
|
||||
|
||||
using namespace estimator;
|
||||
|
||||
@ -410,6 +411,21 @@ protected:
|
||||
warning_event_status_u _warning_events{};
|
||||
information_event_status_u _information_events{};
|
||||
|
||||
uint32_t _baro_hgt_counter{0};
|
||||
AlphaFilter<float> _baro_hgt_lpf{0.1f};
|
||||
|
||||
uint32_t _gps_hgt_counter{0};
|
||||
AlphaFilter<float> _gps_hgt_lpf{0.1f};
|
||||
|
||||
uint32_t _rng_hgt_counter{0};
|
||||
math::MedianFilter<float, 5> _rng_hgt_filter{};
|
||||
|
||||
uint32_t _ev_hgt_counter{0};
|
||||
AlphaFilter<float> _ev_hgt_lpf{0.1f};
|
||||
|
||||
uint32_t _mag_counter{0};
|
||||
AlphaFilter<Vector3f> _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss)
|
||||
|
||||
private:
|
||||
|
||||
inline void setDragData(const imuSample &imu);
|
||||
|
||||
@ -40,11 +40,30 @@
|
||||
|
||||
void Ekf::fuseBaroHgt()
|
||||
{
|
||||
const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
|
||||
|
||||
if (!PX4_ISFINITE(_baro_hgt_offset)) {
|
||||
_baro_hgt_offset = _state.pos(2) + unbiased_baro;
|
||||
}
|
||||
|
||||
if (!_control_status_prev.flags.baro_hgt && _control_status.flags.baro_hgt) {
|
||||
ECL_INFO("Baro height offset %.3f", (double)_baro_hgt_offset);
|
||||
}
|
||||
|
||||
// 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 && (_delta_time_baro_us != 0)) {
|
||||
const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
|
||||
|
||||
// apply a 10 second first order low pass filter to baro offset
|
||||
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);
|
||||
}
|
||||
|
||||
Vector2f baro_hgt_innov_gate;
|
||||
Vector3f baro_hgt_obs_var;
|
||||
|
||||
// vertical position innovation - baro measurement has opposite sign to earth z axis
|
||||
const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
|
||||
_baro_hgt_innov(2) = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
|
||||
// observation variance - user parameter defined
|
||||
baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
|
||||
@ -68,30 +87,72 @@ void Ekf::fuseBaroHgt()
|
||||
}
|
||||
|
||||
fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate,
|
||||
baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);
|
||||
baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio, _control_status.flags.baro_hgt);
|
||||
}
|
||||
|
||||
void Ekf::fuseGpsHgt()
|
||||
{
|
||||
if (!PX4_ISFINITE(_gps_hgt_offset)) {
|
||||
_gps_hgt_offset = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref;
|
||||
}
|
||||
|
||||
if (!_control_status_prev.flags.gps_hgt && _control_status.flags.gps_hgt) {
|
||||
ECL_INFO("GPS height offset %.3f, Altitude ref: %.3f", (double)_gps_hgt_offset, (double)_gps_alt_ref);
|
||||
}
|
||||
|
||||
if (!_control_status.flags.gps_hgt && (_delta_time_gps_us != 0)) {
|
||||
// calculate a filtered offset between the GPS origin and local NED origin if we are not using the GPS as a height reference
|
||||
const float local_time_step = math::constrain(1e-6f * _delta_time_gps_us, 0.f, 1.f);
|
||||
|
||||
// apply a 10 second first order low pass filter to height offset
|
||||
_gps_hgt_offset += local_time_step * math::constrain(0.1f * _gps_pos_innov(2), -0.1f, 0.1f);
|
||||
}
|
||||
|
||||
Vector2f gps_hgt_innov_gate;
|
||||
Vector3f gps_hgt_obs_var;
|
||||
// vertical position innovation - gps measurement has opposite sign to earth z axis
|
||||
_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
|
||||
_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _gps_hgt_offset;
|
||||
gps_hgt_obs_var(2) = getGpsHeightVariance();
|
||||
// innovation gate size
|
||||
gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
|
||||
|
||||
fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate,
|
||||
gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
|
||||
gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio, _control_status.flags.gps_hgt);
|
||||
}
|
||||
|
||||
void Ekf::fuseRngHgt()
|
||||
{
|
||||
if (!PX4_ISFINITE(_rng_hgt_offset)) {
|
||||
// calculate height sensor offset such that current measurement matches our current height estimate
|
||||
// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
|
||||
if (_control_status.flags.in_air && isTerrainEstimateValid()) {
|
||||
_rng_hgt_offset = _terrain_vpos;
|
||||
|
||||
} else if (_control_status.flags.in_air) {
|
||||
_rng_hgt_offset = _range_sensor.getDistBottom() + _state.pos(2);
|
||||
|
||||
} else {
|
||||
_rng_hgt_offset = _params.rng_gnd_clearance;
|
||||
}
|
||||
}
|
||||
|
||||
if (!_control_status_prev.flags.rng_hgt && _control_status.flags.rng_hgt) {
|
||||
ECL_INFO("RNG height offset %.3f", (double)_rng_hgt_offset);
|
||||
}
|
||||
|
||||
if (!_control_status.flags.rng_hgt && (_delta_time_rng_us != 0)) {
|
||||
// calculate a filtered offset between the RNG origin and local NED origin if we are not using the GPS as a height reference
|
||||
const float local_time_step = math::constrain(1e-6f * _delta_time_rng_us, 0.f, 1.f);
|
||||
|
||||
// apply a 10 second first order low pass filter to height offset
|
||||
_rng_hgt_offset += local_time_step * math::constrain(0.1f * _rng_hgt_innov(2), -0.1f, 0.1f);
|
||||
}
|
||||
|
||||
Vector2f rng_hgt_innov_gate;
|
||||
Vector3f rng_hgt_obs_var;
|
||||
// use range finder with tilt correction
|
||||
_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
|
||||
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
|
||||
_params.rng_gnd_clearance)) - _rng_hgt_offset;
|
||||
// observation variance - user parameter defined
|
||||
rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
|
||||
+ sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
|
||||
@ -99,20 +160,37 @@ void Ekf::fuseRngHgt()
|
||||
rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
|
||||
|
||||
fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate,
|
||||
rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
|
||||
rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio, _control_status.flags.rng_hgt);
|
||||
}
|
||||
|
||||
void Ekf::fuseEvHgt()
|
||||
{
|
||||
if (!PX4_ISFINITE(_ev_hgt_offset)) {
|
||||
_ev_hgt_offset = _state.pos(2) - _ev_sample_delayed.pos(2);
|
||||
}
|
||||
|
||||
if (!_control_status_prev.flags.ev_hgt && _control_status.flags.ev_hgt) {
|
||||
ECL_INFO("Vision height offset %.3f", (double)_ev_hgt_offset);
|
||||
}
|
||||
|
||||
if (!_control_status.flags.ev_hgt && (_delta_time_ev_us != 0)) {
|
||||
// calculate a filtered offset between the GPS origin and local NED origin if we are not using the GPS as a height reference
|
||||
const float local_time_step = math::constrain(1e-6f * _delta_time_ev_us, 0.f, 1.f);
|
||||
|
||||
// apply a 10 second first order low pass filter to height offset
|
||||
_ev_hgt_offset += local_time_step * math::constrain(0.1f * _gps_pos_innov(2), -0.1f, 0.1f);
|
||||
}
|
||||
|
||||
Vector2f ev_hgt_innov_gate;
|
||||
Vector3f ev_hgt_obs_var;
|
||||
// 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);
|
||||
_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2) - _ev_hgt_offset;
|
||||
|
||||
// observation variance - defined externally
|
||||
ev_hgt_obs_var(2) = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
|
||||
// innovation gate size
|
||||
ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
|
||||
|
||||
fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate,
|
||||
ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
|
||||
ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio, _control_status.flags.ev_hgt);
|
||||
}
|
||||
|
||||
@ -105,6 +105,8 @@ public:
|
||||
float getValidMinVal() const { return _rng_valid_min_val; }
|
||||
float getValidMaxVal() const { return _rng_valid_max_val; }
|
||||
|
||||
const rangeSample &sample() const { return _sample; }
|
||||
|
||||
private:
|
||||
void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
|
||||
|
||||
|
||||
@ -147,9 +147,8 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
|
||||
}
|
||||
|
||||
bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
|
||||
Vector3f &innov_var, Vector2f &test_ratio)
|
||||
Vector3f &innov_var, Vector2f &test_ratio, bool fuse)
|
||||
{
|
||||
|
||||
innov_var(2) = P(9, 9) + obs_var(2);
|
||||
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
|
||||
_vert_pos_innov_ratio = innov(2) / sqrtf(innov_var(2));
|
||||
@ -170,16 +169,21 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate
|
||||
}
|
||||
|
||||
if (innov_check_pass) {
|
||||
_time_last_hgt_fuse = _time_last_imu;
|
||||
_innov_check_fail_status.flags.reject_ver_pos = false;
|
||||
fuseVelPosHeight(innovation, innov_var(2), 5);
|
||||
if (fuse) {
|
||||
_time_last_hgt_fuse = _time_last_imu;
|
||||
_innov_check_fail_status.flags.reject_ver_pos = false;
|
||||
fuseVelPosHeight(innovation, innov_var(2), 5);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
} else {
|
||||
_innov_check_fail_status.flags.reject_ver_pos = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (fuse) {
|
||||
_innov_check_fail_status.flags.reject_ver_pos = true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// Helper function that fuses a single velocity or position measurement
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user