From 96c5c140411c5e6a2e2ae392b829c772fa66cc59 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Fri, 8 Mar 2019 14:59:28 +0100 Subject: [PATCH] EKF: Change the EKF initialization to 1) init tilt 2) init yaw 3) init tilt uncertainty 4) init yaw uncertainty Signed-off-by: CarlOlsson --- EKF/ekf.cpp | 17 +++++++++++++---- EKF/ekf.h | 4 ++-- EKF/ekf_helper.cpp | 36 ++++++++++++++++++++---------------- 3 files changed, 35 insertions(+), 22 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 81ad70852e..84183a7af5 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -233,9 +233,6 @@ bool Ekf::initialiseFilter() _state.mag_B.setZero(); _state.wind_vel.setZero(); - // initialise the state covariance matrix - initialiseCovariance(); - // get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static float pitch = 0.0f; float roll = 0.0f; @@ -260,7 +257,19 @@ bool Ekf::initialiseFilter() // calculate the initial magnetic field and yaw alignment // Get the magnetic declination calcMagDeclination(); - _control_status.flags.yaw_align = resetMagHeading(_mag_filt_state); + _control_status.flags.yaw_align = resetMagHeading(_mag_filt_state, false, false); + + // initialise the state covariance matrix + initialiseCovariance(); + + // update the yaw angle variance using the variance of the measurement + if (_control_status.flags.ev_yaw) { + // using error estimate from external vision data TODO: this is never true + increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f))); + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { + // using magnetic heading tuning parameter + increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f))); + } 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 diff --git a/EKF/ekf.h b/EKF/ekf.h index 3c14a96343..001b3ed6cd 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -544,9 +544,9 @@ private: // update the terrain vertical position estimate using a height above ground measurement from the range finder void fuseHagl(); - // reset the heading and magnetic field states using the declination and magnetometer measurements + // reset the heading and magnetic field states using the declination and magnetometer/external vision measurements // return true if successful - bool resetMagHeading(Vector3f &mag_init); + bool resetMagHeading(Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer=true); // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS. // It is used to align the yaw angle after launch or takeoff for fixed wing vehicle. diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index cb020b9cb3..5801dcedd4 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -558,7 +558,7 @@ bool Ekf::realignYawGPS() } // Reset heading and magnetic field states -bool Ekf::resetMagHeading(Vector3f &mag_init) +bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update_buffer) { // prevent a reset being performed more than once on the same frame if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) { @@ -737,24 +737,28 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) resetExtVisRotMat(); } - // update the yaw angle variance using the variance of the measurement - if (_control_status.flags.ev_yaw) { - // using error estimate from external vision data - increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f))); - } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { - // using magnetic heading tuning parameter - increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f))); + if (increase_yaw_var) { + // update the yaw angle variance using the variance of the measurement + if (_control_status.flags.ev_yaw) { + // using error estimate from external vision data + increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f))); + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { + // using magnetic heading tuning parameter + increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f))); + } } - // add the reset amount to the output observer buffered data - for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { - // Note q1 *= q2 is equivalent to q1 = q2 * q1 - _output_buffer[i].quat_nominal *= _state_reset_status.quat_change; - } + if (update_buffer) { + // add the reset amount to the output observer buffered data + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + // Note q1 *= q2 is equivalent to q1 = q2 * q1 + _output_buffer[i].quat_nominal *= _state_reset_status.quat_change; + } - // apply the change in attitude quaternion to our newest quaternion estimate - // which was already taken out from the output buffer - _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; + // apply the change in attitude quaternion to our newest quaternion estimate + // which was already taken out from the output buffer + _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; + } // capture the reset event _state_reset_status.quat_counter++;