From eaf94935f0a6214b3fa6fc1f4ca5e0a99daf87ca Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 3 Apr 2016 19:39:19 -0700 Subject: [PATCH] EKF: Fix bug in initialisation of height and magnetic field This prevents zero data being used to form the initial height and magnetic field. Do not start sampling initial values until non-zero time values are retrieved from the buffer. --- EKF/ekf.cpp | 37 ++++++++++++++++++++++++------------- 1 file changed, 24 insertions(+), 13 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 07ccb422c2..e676a9ffe2 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -339,12 +339,15 @@ bool Ekf::initialiseFilter(void) // Sum the magnetometer measurements if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { - if (_mag_counter == 0) { + if (_mag_counter == 0 && _mag_sample_delayed.time_us !=0) { + // initialise the filter states and counter when we start getting valid data from the buffer _mag_filt_state = _mag_sample_delayed.mag; + _mag_counter = 1; + } else if (_mag_counter != 0) { + // increment the sample count and apply a LPF to the measurement + _mag_counter ++; + _mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f; } - - _mag_counter ++; - _mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f; } // set the default height source from the adjustable parameter @@ -354,28 +357,36 @@ bool Ekf::initialiseFilter(void) if (_primary_hgt_source == VDIST_SENSOR_RANGE) { if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) { - if (_hgt_counter == 0) { + if (_hgt_counter == 0 && _range_sample_delayed.time_us != 0) { + // initialise the filter states and counter when we start getting valid data from the buffer _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = true; _hgt_filt_state = _range_sample_delayed.rng; + _hgt_counter = 1; + } else if (_hgt_counter != 0) { + // increment the sample count and apply a LPF to the measurement + _hgt_counter ++; + _hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _range_sample_delayed.rng; } - - _hgt_counter ++; - _hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _range_sample_delayed.rng; } } else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) { + // if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS + // later when it passes checks. if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { - if (_hgt_counter == 0) { + if (_hgt_counter == 0 && _baro_sample_delayed.time_us != 0) { + // initialise the filter states and counter when we start getting valid data from the buffer _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _hgt_filt_state = _baro_sample_delayed.hgt; + _hgt_counter = 1; + } else if (_hgt_counter != 0) { + // increment the sample count and apply a LPF to the measurement + _hgt_counter ++; + _hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _baro_sample_delayed.hgt; } - - _hgt_counter ++; - _hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _baro_sample_delayed.hgt; } } else { @@ -594,4 +605,4 @@ void Ekf::calculateOutputStates() _delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt; _vel_corr = (_state.pos - _output_sample_delayed.pos); -} \ No newline at end of file +}