From 22fba0fc6ecfe74abe8891925df3732a3b190d0a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 18 May 2016 19:18:07 +1000 Subject: [PATCH 1/6] EKF: don't initialise filter without EV data if we are relying on it --- EKF/ekf.cpp | 19 +++++++++++++++++-- EKF/ekf.h | 5 +++-- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index e15cb646a5..68d709483f 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -92,6 +92,7 @@ Ekf::Ekf(): _hgt_counter(0), _rng_filt_state(0.0f), _mag_counter(0), + _ev_counter(0), _time_last_mag(0), _hgt_sensor_offset(0.0f), _terrain_vpos(0.0f), @@ -387,7 +388,7 @@ bool Ekf::update() 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 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 if (_hgt_counter == 0) { 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 - 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; } else { diff --git a/EKF/ekf.h b/EKF/ekf.h index 66691d5b46..55ffa5c363 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -209,9 +209,10 @@ private: float _gps_alt_ref; // WGS-84 height (m) // 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 - 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 Vector3f _mag_filt_state; // filtered magnetometer measurement Vector3f _delVel_sum; // summed delta velocity From 94a63ec9d56675eb99b9d86368e3be83ebffcda3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 18 May 2016 19:19:38 +1000 Subject: [PATCH 2/6] EKF: don't interfere with quaternion covariances during tilt alignment --- EKF/ekf_helper.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index a6748de7f1..0dfa6c0fbd 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -407,8 +407,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // reset the quaternion variances because the yaw angle could have changed by a significant amount // by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise // will grow them again. - zeroRows(P, 0, 3); - zeroCols(P, 0, 3); + if (_control_status.flags.tilt_align) { + zeroRows(P, 0, 3); + zeroCols(P, 0, 3); + } // update transformation matrix from body to world frame using the current estimate _R_to_earth = quat_to_invrotmat(_state.quat_nominal); From b9a3712ccba9d64b2ee960b2d735779183c1b1fe Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 18 May 2016 19:23:44 +1000 Subject: [PATCH 3/6] EKF: record yaw alignment event during initialisation to allow heading fusion to start early This ensures bad yaw gyro biases are compensated for early, rather than waiting for the tilt alignment to fully converge before fusing heading. --- EKF/ekf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 68d709483f..e928f72240 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -511,7 +511,7 @@ bool Ekf::initialiseFilter(void) Vector3f mag_init = _mag_filt_state; // calculate the initial magnetic field and yaw alignment - resetMagHeading(mag_init); + _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 From e272d5f003d67dc1542323d7f62eac10bc109e61 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 18 May 2016 20:11:08 +1000 Subject: [PATCH 4/6] EKF: Use consistent position noise values during alignment --- EKF/vel_pos_fusion.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index c71e014b7d..6ad7a02e0a 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -88,11 +88,10 @@ void Ekf::fuseVelPosHeight() // 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) { + 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] = _params.gps_pos_noise; + 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); From aaac867da8cb6e91e9bbba13c343691997a03c93 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 18 May 2016 20:34:12 +1000 Subject: [PATCH 5/6] EKF: Adjust tilt alignment threshold --- EKF/control.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index ffa7f37ce4..19f8134583 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -54,14 +54,15 @@ void Ekf::controlFusionModes() // whilst we are aligning the tilt, monitor the variances Vector3f angle_err_var_vec = calcRotVecVariances(); - // Once the tilt variances have reduced sufficiently, initialise the yaw and magnetic field states - if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < 0.002f) { + // Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states + // and declare the tilt alignment complete + if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) { _control_status.flags.tilt_align = true; _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } } - // control use of various external souces for positon and velocity aiding + // control use of various external souces for position and velocity aiding controlExternalVisionAiding(); controlOpticalFlowAiding(); controlGpsAiding(); From c1b02eaa9194d76fed82983bdcfe0ab207ce5c1c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 18 May 2016 21:06:45 +1000 Subject: [PATCH 6/6] EKF: Don't use 3-axis magnetometer fusion until the tilt is aligned --- EKF/control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 19f8134583..ba3d880fe3 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -481,7 +481,7 @@ void Ekf::controlMagAiding() // or the more accurate 3-axis fusion if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) { - if (_control_status.flags.in_air) { + if (_control_status.flags.in_air && _control_status.flags.tilt_align) { // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states if (!_control_status.flags.mag_3D) { _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);