fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 07:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: don't initialise filter without EV data if we are relying on it

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-18 19:25:19 +10:00
parent 50ba26a434
commit 22fba0fc6e
2 changed files with 20 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+17 -2
View File
@@ -92,6 +92,7 @@ Ekf::Ekf():
_hgt_counter(0), _hgt_counter(0),
_rng_filt_state(0.0f), _rng_filt_state(0.0f),
_mag_counter(0), _mag_counter(0),
_ev_counter(0),
_time_last_mag(0), _time_last_mag(0),
_hgt_sensor_offset(0.0f), _hgt_sensor_offset(0.0f),
_terrain_vpos(0.0f), _terrain_vpos(0.0f),
@@ -387,7 +388,7 @@ bool Ekf::update()
bool Ekf::initialiseFilter(void) bool Ekf::initialiseFilter(void)
{ {
// Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter // Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
// Sum the IMU delta angle measurements // Sum the IMU delta angle measurements
imuSample imu_init = _imu_buffer.get_newest(); imuSample imu_init = _imu_buffer.get_newest();
@@ -406,6 +407,17 @@ bool Ekf::initialiseFilter(void)
} }
} }
// Count the number of external vision measurements received
if (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) {
if (_ev_counter == 0 && _ev_sample_delayed.time_us !=0) {
// initialise the counter
_ev_counter = 1;
} else if (_ev_counter != 0) {
// increment the sample count
_ev_counter ++;
}
}
// set the default height source from the adjustable parameter // set the default height source from the adjustable parameter
if (_hgt_counter == 0) { if (_hgt_counter == 0) {
if (_params.fusion_mode & MASK_USE_EVPOS) { if (_params.fusion_mode & MASK_USE_EVPOS) {
@@ -454,7 +466,10 @@ 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
if (_hgt_counter <= 10 || _mag_counter <= 10) { bool hgt_count_fail = _hgt_counter <= 10;
bool mag_count_fail = _mag_counter <= 10;
bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 10);
if (hgt_count_fail || mag_count_fail || ev_count_fail) {
return false; return false;
} else { } else {
EKF/ekf.h
+3 -2
View File
@@ -209,9 +209,10 @@ private:
float _gps_alt_ref; // WGS-84 height (m) float _gps_alt_ref; // WGS-84 height (m)
// Variables used to initialise the filter states // Variables used to initialise the filter states
uint32_t _hgt_counter; // number of height samples taken uint32_t _hgt_counter; // number of height samples read during initialisation
float _rng_filt_state; // filtered height measurement float _rng_filt_state; // filtered height measurement
uint32_t _mag_counter; // number of magnetometer samples taken uint32_t _mag_counter; // number of magnetometer samples read during initialisation
uint32_t _ev_counter; // number of exgernal vision samples read during initialisation
uint64_t _time_last_mag; // measurement time of last magnetomter sample uint64_t _time_last_mag; // measurement time of last magnetomter sample
Vector3f _mag_filt_state; // filtered magnetometer measurement Vector3f _mag_filt_state; // filtered magnetometer measurement
Vector3f _delVel_sum; // summed delta velocity Vector3f _delVel_sum; // summed delta velocity