zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF: Fix variable names to match convention

master
Paul Riseborough 8 years ago
parent
afd6e54b2a
commit
5ad329b641
3 changed files with 32 additions and 32 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      EKF/ekf.cpp
  2. 42
      EKF/estimator_interface.cpp
  3. 4
      EKF/estimator_interface.h

18
EKF/ekf.cpp
Unescape View File

@ -259,10 +259,10 @@ bool Ekf::initialiseFilter(void)
// increment the sample count and apply a LPF to the measurement // increment the sample count and apply a LPF to the measurement
_mag_counter ++; _mag_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed // don't start using data until we can be certain all bad initial data has been flushed
if (_mag_counter == (uint8_t)(OBS_BUFFER_LENGTH + 1)) { if (_mag_counter == (uint8_t)(_obs_buffer_length + 1)) {
// initialise filter states // initialise filter states
_mag_filt_state = _mag_sample_delayed.mag; _mag_filt_state = _mag_sample_delayed.mag;
} else if (_mag_counter > (uint8_t)(OBS_BUFFER_LENGTH + 1)) { } else if (_mag_counter > (uint8_t)(_obs_buffer_length + 1)) {
// noise filter the data // noise filter the data
_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f; _mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
} }
@ -306,10 +306,10 @@ bool Ekf::initialiseFilter(void)
// increment the sample count and apply a LPF to the measurement // increment the sample count and apply a LPF to the measurement
_hgt_counter ++; _hgt_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed // don't start using data until we can be certain all bad initial data has been flushed
if (_hgt_counter == (uint8_t)(OBS_BUFFER_LENGTH + 1)) { if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
// initialise filter states // initialise filter states
_rng_filt_state = _range_sample_delayed.rng; _rng_filt_state = _range_sample_delayed.rng;
} else if (_hgt_counter > (uint8_t)(OBS_BUFFER_LENGTH + 1)) { } else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
// noise filter the data // noise filter the data
_rng_filt_state = 0.9f * _rng_filt_state + 0.1f * _range_sample_delayed.rng; _rng_filt_state = 0.9f * _rng_filt_state + 0.1f * _range_sample_delayed.rng;
} }
@ -330,10 +330,10 @@ bool Ekf::initialiseFilter(void)
// increment the sample count and apply a LPF to the measurement // increment the sample count and apply a LPF to the measurement
_hgt_counter ++; _hgt_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed // don't start using data until we can be certain all bad initial data has been flushed
if (_hgt_counter == (uint8_t)(OBS_BUFFER_LENGTH + 1)) { if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
// initialise filter states // initialise filter states
_baro_hgt_offset = _baro_sample_delayed.hgt; _baro_hgt_offset = _baro_sample_delayed.hgt;
} else if (_hgt_counter > (uint8_t)(OBS_BUFFER_LENGTH + 1)) { } else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
// noise filter the data // noise filter the data
_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt; _baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
} }
@ -347,9 +347,9 @@ bool Ekf::initialiseFilter(void)
} }
// check to see if we have enough measurements and return false if not // check to see if we have enough measurements and return false if not
bool hgt_count_fail = _hgt_counter <= 2*OBS_BUFFER_LENGTH; bool hgt_count_fail = _hgt_counter <= 2*_obs_buffer_length;
bool mag_count_fail = _mag_counter <= 2*OBS_BUFFER_LENGTH; bool mag_count_fail = _mag_counter <= 2*_obs_buffer_length;
bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2*OBS_BUFFER_LENGTH); bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2*_obs_buffer_length);
if (hgt_count_fail || mag_count_fail || ev_count_fail) { if (hgt_count_fail || mag_count_fail || ev_count_fail) {
return false; return false;

42
EKF/estimator_interface.cpp
Unescape View File

@ -48,8 +48,8 @@
EstimatorInterface::EstimatorInterface(): EstimatorInterface::EstimatorInterface():
OBS_BUFFER_LENGTH(10), _obs_buffer_length(10),
IMU_BUFFER_LENGTH(30), _imu_buffer_length(30),
_min_obs_interval_us(0), _min_obs_interval_us(0),
_dt_imu_avg(0.0f), _dt_imu_avg(0.0f),
_imu_ticks(0), _imu_ticks(0),
@ -140,7 +140,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
// calculate the minimum interval between observations required to guarantee no loss of data // calculate the minimum interval between observations required to guarantee no loss of data
// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon // this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
_min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us)/(OBS_BUFFER_LENGTH - 1); _min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us)/(_obs_buffer_length - 1);
} else { } else {
_imu_updated = false; _imu_updated = false;
@ -351,36 +351,36 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
math::max(_params.airspeed_delay_ms, _params.baro_delay_ms)))))); math::max(_params.airspeed_delay_ms, _params.baro_delay_ms))))));
// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
IMU_BUFFER_LENGTH = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1; _imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
// set the observaton buffer length to handle the minimum time of arrival between observations in combination // set the observaton buffer length to handle the minimum time of arrival between observations in combination
// with the worst case delay from current time to ekf fusion time // with the worst case delay from current time to ekf fusion time
// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceil((float)max_time_delay_ms * 0.5f)); uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceil((float)max_time_delay_ms * 0.5f));
OBS_BUFFER_LENGTH = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1; _obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1;
// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
OBS_BUFFER_LENGTH = math::min(OBS_BUFFER_LENGTH,IMU_BUFFER_LENGTH); _obs_buffer_length = math::min(_obs_buffer_length,_imu_buffer_length);
ECL_INFO("EKF IMU buffer length = %i",(int)IMU_BUFFER_LENGTH); ECL_INFO("EKF IMU buffer length = %i",(int)_imu_buffer_length);
ECL_INFO("EKF observation buffer length = %i",(int)OBS_BUFFER_LENGTH); ECL_INFO("EKF observation buffer length = %i",(int)_obs_buffer_length);
if (!(_imu_buffer.allocate(IMU_BUFFER_LENGTH) && if (!(_imu_buffer.allocate(_imu_buffer_length) &&
_gps_buffer.allocate(OBS_BUFFER_LENGTH) && _gps_buffer.allocate(_obs_buffer_length) &&
_mag_buffer.allocate(OBS_BUFFER_LENGTH) && _mag_buffer.allocate(_obs_buffer_length) &&
_baro_buffer.allocate(OBS_BUFFER_LENGTH) && _baro_buffer.allocate(_obs_buffer_length) &&
_range_buffer.allocate(OBS_BUFFER_LENGTH) && _range_buffer.allocate(_obs_buffer_length) &&
_airspeed_buffer.allocate(OBS_BUFFER_LENGTH) && _airspeed_buffer.allocate(_obs_buffer_length) &&
_flow_buffer.allocate(OBS_BUFFER_LENGTH) && _flow_buffer.allocate(_obs_buffer_length) &&
_ext_vision_buffer.allocate(OBS_BUFFER_LENGTH) && _ext_vision_buffer.allocate(_obs_buffer_length) &&
_output_buffer.allocate(IMU_BUFFER_LENGTH))) { _output_buffer.allocate(_imu_buffer_length))) {
ECL_ERR("EKF buffer allocation failed!"); ECL_ERR("EKF buffer allocation failed!");
unallocate_buffers(); unallocate_buffers();
return false; return false;
} }
// zero the data in the observation buffers // zero the data in the observation buffers
for (int index=0; index < OBS_BUFFER_LENGTH; index++) { for (int index=0; index < _obs_buffer_length; index++) {
gpsSample gps_sample_init = {}; gpsSample gps_sample_init = {};
_gps_buffer.push(gps_sample_init); _gps_buffer.push(gps_sample_init);
magSample mag_sample_init = {}; magSample mag_sample_init = {};
@ -398,7 +398,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
} }
// zero the data in the imu data and output observer state buffers // zero the data in the imu data and output observer state buffers
for (int index=0; index < IMU_BUFFER_LENGTH; index++) { for (int index=0; index < _imu_buffer_length; index++) {
imuSample imu_sample_init = {}; imuSample imu_sample_init = {};
_imu_buffer.push(imu_sample_init); _imu_buffer.push(imu_sample_init);
outputSample output_sample_init = {}; outputSample output_sample_init = {};

4
EKF/estimator_interface.h
Unescape View File

@ -280,14 +280,14 @@ protected:
max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS * 0.001) max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS * 0.001)
This can be adjusted to match the max sensor data rate plus some margin for jitter. This can be adjusted to match the max sensor data rate plus some margin for jitter.
*/ */
uint8_t OBS_BUFFER_LENGTH; uint8_t _obs_buffer_length;
/* /*
IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the
EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for. EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for.
max sensor time offet (msec) = IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS max sensor time offet (msec) = IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS
This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay. This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay.
*/ */
uint8_t IMU_BUFFER_LENGTH; uint8_t _imu_buffer_length;
static const unsigned FILTER_UPDATE_PERIOD_MS = 10; // ekf prediction period in milliseconds static const unsigned FILTER_UPDATE_PERIOD_MS = 10; // ekf prediction period in milliseconds
unsigned _min_obs_interval_us; // minimum time interval between observations that will guarantee data is not lost (usec) unsigned _min_obs_interval_us; // minimum time interval between observations that will guarantee data is not lost (usec)

Write Preview
Loading…
Cancel
Save