fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 21:47:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix bug in initialisation of height and magnetic field

Browse Source 
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.
This commit is contained in:
Paul Riseborough
2016-04-03 19:39:19 -07:00
committed by Roman
parent 03eac2f25e
commit eaf94935f0
1 changed files with 24 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+24 -13
View File
@@ -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);
}
}