From 05c3c46f839ef0738c2cb0c643dd40a2d6ef01d8 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 May 2017 18:50:19 +1000 Subject: [PATCH] ekf2: changes arising from code review --- EKF/control.cpp | 4 ++-- EKF/ekf.cpp | 3 ++- EKF/ekf.h | 10 +++++----- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index a4399c9486..1f2a7b1903 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -888,7 +888,7 @@ void Ekf::controlMagFusion() } // If we are on ground, store the local position and time to use as a reference - // Also reset the flight alignment falg so that the mag fields will be re-initialised next time we achieve flight altitude + // Also reset the flight alignment flag so that the mag fields will be re-initialised next time we achieve flight altitude if (!_control_status.flags.in_air) { _last_on_ground_posD = _state.pos(2); _flt_mag_align_complete = false; @@ -941,7 +941,7 @@ void Ekf::controlMagFusion() bool use_3D_fusion = _control_status.flags.tilt_align && // Use of 3D fusion requires valid tilt estimates _control_status.flags.in_air && // don't use when on the ground becasue of magnetic anomalies (_flt_mag_align_complete || height_achieved) && // once in-flight field alignment has been performed, ignore relative height - ((_imu_sample_delayed.time_us - _time_last_movement) < 2000000); // Using 3-axis fusion for a minimum period after to allow for false negatives + ((_imu_sample_delayed.time_us - _time_last_movement) < 2 * 1000 * 1000); // Using 3-axis fusion for a minimum period after to allow for false negatives // perform switch-over if (use_3D_fusion) { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 65a6fb1b30..e46be27fcb 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -494,7 +494,8 @@ void Ekf::calculateOutputStates() _R_to_earth_now(2,2) * delta_angle(2); _yaw_delta_ef += spin_del_ang_D; - // calculate filtered yaw rate to be used by the magnetomer fusion type selection logic + // Calculate filtered yaw rate to be used by the magnetomer fusion type selection logic + // Note fixed coefficients are used to save operations. The exact time constant is not important. _yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / _imu_sample_new.delta_ang_dt; // correct delta velocity for bias offsets diff --git a/EKF/ekf.h b/EKF/ekf.h index e98ad52fe2..017ae3a449 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -266,12 +266,13 @@ private: matrix::Dcm _R_to_earth; // transformation matrix from body frame to earth frame from last EKF predition - // used by magnetomer fusion mode selection + // used by magnetometer fusion mode selection Vector2f _accel_lpf_NE; // Low pass filtered horizontal earth frame acceleration (m/s**2) float _yaw_delta_ef{0.0f}; // Recent change in yaw angle measured about the earth frame D axis (rad) - float _yaw_rate_lpf_ef{0.0f}; // Filtered angular rate abut earth frame D axis (rad/sec) - bool _mag_bias_observable{false}; // true when there is enough rotation to make magnetomer bias errors observable + float _yaw_rate_lpf_ef{0.0f}; // Filtered angular rate about earth frame D axis (rad/sec) + bool _mag_bias_observable{false}; // true when there is enough rotation to make magnetometer bias errors observable bool _yaw_angle_observable{false}; // true when there is enough horizontal acceleration to make yaw observable + uint64_t _time_yaw_started{0}; // last system time in usec that a yaw rotation moaneouvre was detected float P[_k_num_states][_k_num_states] {}; // state covariance matrix @@ -337,9 +338,8 @@ private: // Variables used to control activation of post takeoff functionality float _last_on_ground_posD{0.0f}; // last vertical position when the in_air status was false (m) bool _flt_mag_align_complete{true}; // true when the in-flight mag field alignment has been completed - uint64_t _time_last_movement{0}; // last system time in usec that sufficient movement to use 3-axis magnetomer fusion was detected + uint64_t _time_last_movement{0}; // last system time in usec that sufficient movement to use 3-axis magnetometer fusion was detected float _saved_mag_variance[6] {}; // magnetic field state variances that have been saved for use at the next initialisation (Ga**2) - uint64_t _time_yaw_started{0}; // last system time in usec that a yaw rotation moaneouvre was detected gps_check_fail_status_u _gps_check_fail_status{};