From e41524ac9d7cc6cc11ffc0f02561cec248985431 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 27 May 2016 11:21:04 +1000 Subject: [PATCH 1/7] EKF: remove unused variables --- EKF/common.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 6c972d85d6..0dfa16c029 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -231,9 +231,7 @@ struct parameters { float rng_gnd_clearance; // minimum valid value for range when on ground (m) // vision position fusion - float ev_noise; // observation noise for vision sensor estimates (m) float ev_innov_gate; // vision estimator fusion innovation consistency gate size (STD) - float ev_gnd_clearance; // minimum valid value for vision based height estimate when on ground (m) // optical flow fusion float flow_noise; // observation noise for optical flow LOS rate measurements (rad/sec) From 6f412a73b486fd7ab7f49953aa3a68770e179e35 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 27 May 2016 11:24:53 +1000 Subject: [PATCH 2/7] EKF: Use correct height gate when using external vision --- EKF/vel_pos_fusion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 69f0d5534a..27ceb1dbe4 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -159,7 +159,7 @@ void Ekf::fuseVelPosHeight() R[5] = fmaxf(_ev_sample_delayed.posErr, 0.01f); R[5] = R[5] * R[5]; // innovation gate size - gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); + gate_size[5] = fmaxf(_params.ev_innov_gate, 1.0f); } } From 00bada8f25b6eef001f7d4d14c6f12e828470b86 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 27 May 2016 11:29:10 +1000 Subject: [PATCH 3/7] EKF: Clean up logic for horizontal position fusion Remove reference to optical flow fusion mode Simplify logic flow for selection of observation noise Remove unnecessary conditional statement --- EKF/vel_pos_fusion.cpp | 44 +++++++++++++++++++++++++++--------------- 1 file changed, 28 insertions(+), 16 deletions(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 27ceb1dbe4..ab6429f16d 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -86,35 +86,49 @@ void Ekf::fuseVelPosHeight() // Calculate innovations and observation variance depending on type of observations // being used - if (!_control_status.flags.gps && !_control_status.flags.ev_pos) { - // No observations - use a static position to constrain drift - if (_control_status.flags.in_air && _control_status.flags.tilt_align) { - R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); - } else { - R[3] = 0.5f; - } - _vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0); - _vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1); - - } else if (_control_status.flags.gps) { + if (_control_status.flags.gps) { + // we are using GPS measurements float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); _vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0); _vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); + // innovation gate size + gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f); + + } else if (_control_status.flags.ev_pos) { + // we are using external vision measurements R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f); _vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0); _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); + // innovation gate size + gate_size[3] = fmaxf(_params.ev_innov_gate, 1.0f); + + } else { + // No observations - use a static position to constrain drift + if (_control_status.flags.in_air && _control_status.flags.tilt_align) { + R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); + } else { + R[3] = 0.5f; + } + _vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0); + _vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1); + + // glitch protection is not required so set gate to a large value + gate_size[3] = 100.0f; + } + // convert North position noise to variance R[3] = R[3] * R[3]; + + // copy North axis values to East axis R[4] = R[3]; - // innovation gate sizes - gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f); gate_size[4] = gate_size[3]; + } if (_fuse_height) { @@ -183,9 +197,7 @@ void Ekf::fuseVelPosHeight() bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) && (_vel_pos_test_ratio[2] <= 1.0f); innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass; - bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow; - bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) - || using_synthetic_measurements || !_control_status.flags.tilt_align; + bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) || !_control_status.flags.tilt_align; innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass; innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align; From 1b6c5bbafd97a6d2cc82fa304606f6874c050bea Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 27 May 2016 13:14:52 +1000 Subject: [PATCH 4/7] EKF: Enable height source to be selected independent of EV aiding --- EKF/common.h | 1 + EKF/control.cpp | 40 ++++++++++++++++++++++++++++++++-------- EKF/ekf.cpp | 8 ++------ EKF/ekf_helper.cpp | 2 +- EKF/vel_pos_fusion.cpp | 2 +- 5 files changed, 37 insertions(+), 16 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 0dfa16c029..cad93dc713 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -422,6 +422,7 @@ union filter_control_status_u { uint16_t gps_hgt : 1; // 11 - true when range finder height is being fused as a primary height reference uint16_t ev_pos : 1; // 12 - true when local position data from external vision is being fused uint16_t ev_yaw : 1; // 13 - true when yaw data from external vision measurements is being fused + uint16_t ev_hgt : 1; // 14 - true when height data from external vision measurements is being fused } flags; uint16_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 0f3afa78f8..ea7565c678 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -323,6 +323,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; // request a reset reset_height = true; printf("EKF baro hgt timeout - reset to GPS\n"); @@ -333,6 +334,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; // request a reset reset_height = true; printf("EKF baro hgt timeout - reset to baro\n"); @@ -372,6 +374,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; // request a reset reset_height = true; printf("EKF gps hgt timeout - reset to baro\n"); @@ -382,6 +385,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; // request a reset reset_height = true; printf("EKF gps hgt timeout - reset to GPS\n"); @@ -414,6 +418,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; // request a reset reset_height = true; printf("EKF rng hgt timeout - reset to baro\n"); @@ -424,6 +429,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = true; + _control_status.flags.ev_hgt = false; // request a reset reset_height = true; printf("EKF rng hgt timeout - reset to rng hgt\n"); @@ -448,25 +454,43 @@ void Ekf::controlHeightAiding() // check for height sensor timeouts and reset and change sensor if necessary controlHeightSensorTimeouts(); - // Control the soure of height measurements for the main filter - if (_control_status.flags.ev_pos) { - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - } else if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) { + // Control the source of height measurements for the main filter + // do not switch to a sensor if it is unhealthy or the data is stale + if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) && + !_baro_hgt_faulty && + (((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) || _control_status.flags.baro_hgt)) { + _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; + + } else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) && + !_gps_hgt_faulty && + (((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) || _control_status.flags.gps_hgt)) { - } else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; + + } else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && + !_rng_hgt_faulty && + (((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) < 2 * RNG_MAX_INTERVAL) || _control_status.flags.rng_hgt)) { - } else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE && !_rng_hgt_faulty) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = true; + _control_status.flags.ev_hgt = false; + + } else if ((_params.vdist_sensor_type == VDIST_SENSOR_EV) && + (((_imu_sample_delayed.time_us - _ev_sample_delayed.time_us) < 2 * EV_MAX_INTERVAL) || _control_status.flags.ev_hgt)) { + + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = true; + } } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index de6bf563ac..c09126cbb8 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -423,11 +423,7 @@ bool Ekf::initialiseFilter(void) // set the default height source from the adjustable parameter if (_hgt_counter == 0) { - if (_params.fusion_mode & MASK_USE_EVPOS) { - _primary_hgt_source = VDIST_SENSOR_EV; - } else { - _primary_hgt_source = _params.vdist_sensor_type; - } + _primary_hgt_source = _params.vdist_sensor_type; } if (_primary_hgt_source == VDIST_SENSOR_RANGE) { @@ -447,7 +443,7 @@ bool Ekf::initialiseFilter(void) } } else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS || _primary_hgt_source == VDIST_SENSOR_EV) { - // if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS + // if the user parameter specifies use of GPS or external vision (EV) for height we use baro height initially and switch to GPS or EV // later when it passes checks. if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { if (_hgt_counter == 0 && _baro_sample_delayed.time_us != 0) { diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 5e7c2720b1..bc33a37692 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -204,7 +204,7 @@ void Ekf::resetHeight() // TODO: reset to last known gps based estimate } - } else if (_control_status.flags.ev_pos) { + } else if (_control_status.flags.ev_hgt) { // initialize vertical position with newest measurement extVisionSample ev_newest = _ext_vision_buffer.get_newest(); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index ab6429f16d..8608e29afe 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -165,7 +165,7 @@ void Ekf::fuseVelPosHeight() R[5] = R[5] * R[5]; // innovation gate size gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f); - } else if (_control_status.flags.ev_pos) { + } else if (_control_status.flags.ev_hgt) { fuse_map[5] = true; // calculate the innovation assuming the external vision observaton is in local NED frame _vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2); From 9f81b8f09e6c25ab209da332a81f0f47e89f38ee Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 27 May 2016 13:15:53 +1000 Subject: [PATCH 5/7] EKF: provide reset protection for external vision height --- EKF/control.cpp | 42 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index ea7565c678..7a8ff9fff1 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -439,6 +439,48 @@ void Ekf::controlHeightSensorTimeouts() } } + // handle the case where we are using external vision data for height + if (_control_status.flags.ev_hgt) { + // check if vision data is available + extVisionSample ev_init = _ext_vision_buffer.get_newest(); + bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL); + // check if baro data is available + baroSample baro_init = _baro_buffer.get_newest(); + bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); + + // reset to baro if we have no vision data and baro data is available + bool reset_to_baro = !ev_data_available && baro_data_available; + + // reset to ev data if it is available + bool reset_to_ev = ev_data_available; + + if (reset_to_baro) { + // set height sensor health + _rng_hgt_faulty = true; + _baro_hgt_faulty = false; + // reset the height mode + _control_status.flags.baro_hgt = true; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; + // request a reset + reset_height = true; + printf("EKF ev hgt timeout - reset to baro\n"); + } else if (reset_to_ev) { + // reset the height mode + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = true; + // request a reset + reset_height = true; + printf("EKF ev hgt timeout - reset to ev hgt\n"); + } else { + // we have nothing to reset to + reset_height = false; + } + } + // Reset vertical position and velocity states to the last measurement if (reset_height) { resetHeight(); From 98c0b74a7154355796d0a129527f47553846b6fc Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 27 May 2016 13:59:14 +1000 Subject: [PATCH 6/7] EKF: Initialise height correctly when using external vision data If EV height selected ensure switch to correct height mode as soon as EV data is received The 0 height datum is not at the initialisation position, so the height state needs to be reset to the measurement on startup --- EKF/ekf.cpp | 31 ++++++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index c09126cbb8..a6b15ecf72 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -415,6 +415,13 @@ bool Ekf::initialiseFilter(void) if (_ev_counter == 0 && _ev_sample_delayed.time_us !=0) { // initialise the counter _ev_counter = 1; + // set the height fusion mode to use external vision data when we start getting valid data from the buffer + if (_primary_hgt_source == VDIST_SENSOR_EV) { + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = true; + } } else if (_ev_counter != 0) { // increment the sample count _ev_counter ++; @@ -426,6 +433,7 @@ bool Ekf::initialiseFilter(void) _primary_hgt_source = _params.vdist_sensor_type; } + // accumulate enough height measurements to be confident in the qulaity of the data 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 && _range_sample_delayed.time_us != 0) { @@ -433,6 +441,7 @@ bool Ekf::initialiseFilter(void) _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = true; + _control_status.flags.ev_hgt = false; _rng_filt_state = _range_sample_delayed.rng; _hgt_counter = 1; } else if (_hgt_counter != 0) { @@ -442,8 +451,8 @@ bool Ekf::initialiseFilter(void) } } - } else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS || _primary_hgt_source == VDIST_SENSOR_EV) { - // if the user parameter specifies use of GPS or external vision (EV) for height we use baro height initially and switch to GPS or EV + } 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 && _baro_sample_delayed.time_us != 0) { @@ -460,14 +469,16 @@ bool Ekf::initialiseFilter(void) } } + } else if (_primary_hgt_source == VDIST_SENSOR_EV) { + // do nothing becasue vision data is checked elsewhere } else { return false; } // check to see if we have enough measurements and return false if not - 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); + bool hgt_count_fail = _hgt_counter <= OBS_BUFFER_LENGTH; + bool mag_count_fail = _mag_counter <= OBS_BUFFER_LENGTH; + bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= OBS_BUFFER_LENGTH); if (hgt_count_fail || mag_count_fail || ev_count_fail) { return false; @@ -512,12 +523,18 @@ bool Ekf::initialiseFilter(void) // calculate the initial magnetic field and yaw alignment _control_status.flags.yaw_align = resetMagHeading(mag_init); - // if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup - // so it can be used as a backup ad set the initial height using the range finder if (_control_status.flags.rng_hgt) { + // if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup + // so it can be used as a backup ad set the initial height using the range finder baroSample baro_newest = _baro_buffer.get_newest(); _baro_hgt_offset = baro_newest.hgt; _state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance); + + } else if (_control_status.flags.ev_hgt) { + // if we are using external vision data for height, then the vertical position state needs to be reset + // because the initialisation position is not the zero datum + resetHeight(); + } // initialise the state covariance matrix From ad02818b3d61bef8e9c1144e2151a3eb09e1ff0b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 27 May 2016 14:00:50 +1000 Subject: [PATCH 7/7] EKF: Improve height reset for external vision reset to measurement closest in time to fusion time horizon --- EKF/ekf_helper.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index bc33a37692..bda1ed8da7 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -208,12 +208,15 @@ void Ekf::resetHeight() // initialize vertical position with newest measurement extVisionSample ev_newest = _ext_vision_buffer.get_newest(); - if (_time_last_imu - ev_newest.time_us < 2 * EV_MAX_INTERVAL) { + // use the most recent data if it's time offset from the fusion time horizon is smaller + int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us; + int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us; + if (abs(dt_newest) < abs(dt_delayed)) { _state.pos(2) = ev_newest.posNED(2); - } else { - // TODO: reset to last known baro based estimate + _state.pos(2) = _ev_sample_delayed.posNED(2); } + } // reset the vertical velocity covariance values