From 437f6ca5fbbd976347974573600c6403a1c82364 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Thu, 18 Feb 2016 03:21:04 -0700 Subject: [PATCH 01/11] Moved initialization to object constructors to allow C99 compiler compatibility. --- EKF/common.h | 203 ++++++++++++++++++++++-------------- EKF/ekf.cpp | 105 ++++++++++--------- EKF/ekf.h | 90 ++++++++-------- EKF/estimator_interface.cpp | 2 +- EKF/estimator_interface.h | 64 ++++++------ 5 files changed, 260 insertions(+), 204 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 49ff1318ed..955319c962 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -31,14 +31,17 @@ * ****************************************************************************/ +#include "Matrix.hpp" + /** - * @file estimator_base.h + * @file common.h * Definition of base class for attitude estimators * * @author Roman Bast * @author Siddharth Bharat Purohit * */ + namespace estimator { struct gps_message { @@ -64,45 +67,45 @@ typedef matrix::Quaternion Quaternion; typedef matrix::Matrix Matrix3f; struct outputSample { - Quaternion quat_nominal; // nominal quaternion describing vehicle attitude - Vector3f vel; // NED velocity estimate in earth frame in m/s - Vector3f pos; // NED position estimate in earth frame in m/s - uint64_t time_us; // timestamp in microseconds + Quaternion quat_nominal; // nominal quaternion describing vehicle attitude + Vector3f vel; // NED velocity estimate in earth frame in m/s + Vector3f pos; // NED position estimate in earth frame in m/s + uint64_t time_us; // timestamp in microseconds }; struct imuSample { - Vector3f delta_ang; // delta angle in body frame (integrated gyro measurements) - Vector3f delta_vel; // delta velocity in body frame (integrated accelerometer measurements) - float delta_ang_dt; // delta angle integration period in seconds - float delta_vel_dt; // delta velocity integration period in seconds - uint64_t time_us; // timestamp in microseconds + Vector3f delta_ang; // delta angle in body frame (integrated gyro measurements) + Vector3f delta_vel; // delta velocity in body frame (integrated accelerometer measurements) + float delta_ang_dt; // delta angle integration period in seconds + float delta_vel_dt; // delta velocity integration period in seconds + uint64_t time_us; // timestamp in microseconds }; struct gpsSample { - Vector2f pos; // NE earth frame gps horizontal position measurement in m - float hgt; // gps height measurement in m - Vector3f vel; // NED earth frame gps velocity measurement in m/s - uint64_t time_us; // timestamp in microseconds + Vector2f pos; // NE earth frame gps horizontal position measurement in m + float hgt; // gps height measurement in m + Vector3f vel; // NED earth frame gps velocity measurement in m/s + uint64_t time_us; // timestamp in microseconds }; struct magSample { - Vector3f mag; // NED magnetometer body frame measurements - uint64_t time_us; // timestamp in microseconds + Vector3f mag; // NED magnetometer body frame measurements + uint64_t time_us; // timestamp in microseconds }; struct baroSample { - float hgt; // barometer height above sea level measurement in m - uint64_t time_us; // timestamp in microseconds + float hgt; // barometer height above sea level measurement in m + uint64_t time_us; // timestamp in microseconds }; struct rangeSample { - float rng; // range (distance to ground) measurement in m - uint64_t time_us; // timestamp in microseconds + float rng; // range (distance to ground) measurement in m + uint64_t time_us; // timestamp in microseconds }; struct airspeedSample { - float airspeed; // airspeed measurement in m/s - uint64_t time_us; // timestamp in microseconds + float airspeed; // airspeed measurement in m/s + uint64_t time_us; // timestamp in microseconds }; struct flowSample { @@ -112,83 +115,129 @@ struct flowSample { }; struct parameters { - float mag_delay_ms = 0.0f; // magnetometer measurement delay relative to the IMU - float baro_delay_ms = 0.0f; // barometer height measurement delay relative to the IMU - float gps_delay_ms = 200.0f; // GPS measurement delay relative to the IMU - float airspeed_delay_ms = 200.0f; // airspeed measurement delay relative to the IMU + float mag_delay_ms; // magnetometer measurement delay relative to the IMU + float baro_delay_ms; // barometer height measurement delay relative to the IMU + float gps_delay_ms; // GPS measurement delay relative to the IMU + float airspeed_delay_ms; // airspeed measurement delay relative to the IMU // input noise - float gyro_noise = 1.0e-3f; // IMU angular rate noise used for covariance prediction - float accel_noise = 2.5e-1f; // IMU acceleration noise use for covariance prediction + float gyro_noise; // IMU angular rate noise used for covariance prediction + float accel_noise; // IMU acceleration noise use for covariance prediction // process noise - float gyro_bias_p_noise = 7.0e-5f; // process noise for IMU delta angle bias prediction - float accel_bias_p_noise = 1.0e-4f; // process noise for IMU delta velocity bias prediction - float gyro_scale_p_noise = 3.0e-3f; // process noise for gyro scale factor prediction - float mag_p_noise = 2.5e-2f; // process noise for magnetic field prediction - float wind_vel_p_noise = 1.0e-1f; // process noise for wind velocity prediction + float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction + float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction + float gyro_scale_p_noise; // process noise for gyro scale factor prediction + float mag_p_noise; // process noise for magnetic field prediction + float wind_vel_p_noise; // process noise for wind velocity prediction - float gps_vel_noise = 5.0e-1f; // observation noise for gps velocity fusion - float gps_pos_noise = 1.0f; // observation noise for gps position fusion - float pos_noaid_noise = 10.0f; // observation noise for non-aiding position fusion - float baro_noise = 3.0f; // observation noise for barometric height fusion - float baro_innov_gate = 3.0f; // barometric height innovation consistency gate size in standard deviations - float posNE_innov_gate = 3.0f; // GPS horizontal position innovation consistency gate size in standard deviations - float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations + float gps_vel_noise; // observation noise for gps velocity fusion + float gps_pos_noise; // observation noise for gps position fusion + float pos_noaid_noise; // observation noise for non-aiding position fusion + float baro_noise; // observation noise for barometric height fusion + float baro_innov_gate; // barometric height innovation consistency gate size in standard deviations + float posNE_innov_gate; // GPS horizontal position innovation consistency gate size in standard deviations + float vel_innov_gate; // GPS velocity innovation consistency gate size in standard deviations - float mag_heading_noise = 1.7e-1f; // measurement noise used for simple heading fusion - float mag_noise = 5.0e-2f; // measurement noise used for 3-axis magnetoemeter fusion - float mag_declination_deg = 0.0f; // magnetic declination in degrees - float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations - float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations - int mag_declination_source = 3; // bitmask used to control the handling of declination data - int mag_fusion_type = 0; // integer used to specify the type of magnetometer fusion used + float mag_heading_noise; // measurement noise used for simple heading fusion + float mag_noise; // measurement noise used for 3-axis magnetoemeter fusion + float mag_declination_deg; // magnetic declination in degrees + float heading_innov_gate; // heading fusion innovation consistency gate size in standard deviations + float mag_innov_gate; // magnetometer fusion innovation consistency gate size in standard deviations + int mag_declination_source; // bitmask used to control the handling of declination data + int mag_fusion_type; // integer used to specify the type of magnetometer fusion used // these parameters control the strictness of GPS quality checks used to determine uf the GPS is // good enough to set a local origin and commence aiding - int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used - float req_hacc = 5.0f; // maximum acceptable horizontal position error - float req_vacc = 8.0f; // maximum acceptable vertical position error - float req_sacc = 1.0f; // maximum acceptable speed error - int req_nsats = 6; // minimum acceptable satellite count - float req_gdop = 2.0f; // maximum acceptable geometric dilution of precision - float req_hdrift = 0.3f; // maximum acceptable horizontal drift speed - float req_vdrift = 0.5f; // maximum acceptable vertical drift speed + int gps_check_mask; // bitmask used to control which GPS quality checks are used + float req_hacc; // maximum acceptable horizontal position error + float req_vacc; // maximum acceptable vertical position error + float req_sacc; // maximum acceptable speed error + int req_nsats; // minimum acceptable satellite count + float req_gdop; // maximum acceptable geometric dilution of precision + float req_hdrift; // maximum acceptable horizontal drift speed + float req_vdrift; // maximum acceptable vertical drift speed + + // Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility. + parameters() + { + mag_delay_ms = 0.0f; + baro_delay_ms = 0.0f; + gps_delay_ms = 200.0f; + airspeed_delay_ms = 200.0f; + + // input noise + gyro_noise = 1.0e-3f; + accel_noise = 2.5e-1f; + + // process noise + gyro_bias_p_noise = 7.0e-5f; + accel_bias_p_noise = 1.0e-4f; + gyro_scale_p_noise = 3.0e-3f; + mag_p_noise = 2.5e-2f; + wind_vel_p_noise = 1.0e-1f; + + gps_vel_noise = 5.0e-1f; + gps_pos_noise = 1.0f; + pos_noaid_noise = 10.0f; + baro_noise = 3.0f; + baro_innov_gate = 3.0f; + posNE_innov_gate = 3.0f; + vel_innov_gate = 3.0f; + + mag_heading_noise = 1.7e-1f; + mag_noise = 5.0e-2f; + mag_declination_deg = 0.0f; + heading_innov_gate = 3.0f; + mag_innov_gate = 3.0f; + + mag_declination_source = 7; + mag_fusion_type = 0; + + gps_check_mask = 21; + req_hacc = 5.0f; + req_vacc = 8.0f; + req_sacc = 1.0f; + req_nsats = 6; + req_gdop = 2.0f; + req_hdrift = 0.3f; + req_vdrift = 0.5f; + } }; // Bit locations for mag_declination_source #define MASK_USE_GEO_DECL (1<<0) // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value #define MASK_SAVE_GEO_DECL (1<<1) // set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library -#define MASK_FUSE_DECL (1<<2) // set to true if the declination is always fused as an observation to contrain drift when 3-axis fusion is performed +#define MASK_FUSE_DECL (1<<2) // set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed // Integer definitions for mag_fusion_type #define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic -#define MAG_FUSE_TYPE_HEADING 1 // Magnetic heading fusion will alays be used. This is less accurate, but less affected by earth field distortions -#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions +#define MAG_FUSE_TYPE_HEADING 1 // Magnetic heading fusion will always be used. This is less accurate, but less affected by earth field distortions +#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localized earth field distortions struct stateSample { - Vector3f ang_error; // attitude axis angle error (error state formulation) - Vector3f vel; // NED velocity in earth frame in m/s - Vector3f pos; // NED position in earth frame in m - Vector3f gyro_bias; // gyro bias estimate in rad/s - Vector3f gyro_scale; // gyro scale estimate - float accel_z_bias; // accelerometer z axis bias estimate - Vector3f mag_I; // NED earth magnetic field in gauss - Vector3f mag_B; // magnetometer bias estimate in body frame in gauss - Vector2f wind_vel; // wind velocity in m/s - Quaternion quat_nominal; // nominal quaternion describing vehicle attitude + Vector3f ang_error; // attitude axis angle error (error state formulation) + Vector3f vel; // NED velocity in earth frame in m/s + Vector3f pos; // NED position in earth frame in m + Vector3f gyro_bias; // gyro bias estimate in rad/s + Vector3f gyro_scale; // gyro scale estimate + float accel_z_bias; // accelerometer z axis bias estimate + Vector3f mag_I; // NED earth magnetic field in gauss + Vector3f mag_B; // magnetometer bias estimate in body frame in gauss + Vector2f wind_vel; // wind velocity in m/s + Quaternion quat_nominal; // nominal quaternion describing vehicle attitude }; struct fault_status_t { - bool bad_mag_x: 1; // true if the fusion of the magnetometer X-axis has encountered a numerical error - bool bad_mag_y: 1; // true if the fusion of the magnetometer Y-axis has encountered a numerical error - bool bad_mag_z: 1; // true if the fusion of the magnetometer Z-axis has encountered a numerical error - bool bad_mag_hdg: 1; // true if the fusion of the magnetic heading has encountered a numerical error - bool bad_mag_decl: 1; // true if the fusion of the magnetic declination has encountered a numerical error - bool bad_airspeed: 1; // true if fusion of the airspeed has encountered a numerical error - bool bad_sideslip: 1; // true if fusion of the synthetic sideslip constraint has encountered a numerical error - bool bad_optflow_X: 1; // true if fusion of the optical flow X axis has encountered a numerical error - bool bad_optflow_Y: 1; // true if fusion of the optical flow Y axis has encountered a numerical error + bool bad_mag_x: 1; // true if the fusion of the magnetometer X-axis has encountered a numerical error + bool bad_mag_y: 1; // true if the fusion of the magnetometer Y-axis has encountered a numerical error + bool bad_mag_z: 1; // true if the fusion of the magnetometer Z-axis has encountered a numerical error + bool bad_mag_hdg: 1; // true if the fusion of the magnetic heading has encountered a numerical error + bool bad_mag_decl: 1; // true if the fusion of the magnetic declination has encountered a numerical error + bool bad_airspeed: 1; // true if fusion of the airspeed has encountered a numerical error + bool bad_sideslip: 1; // true if fusion of the synthetic sideslip constraint has encountered a numerical error + bool bad_optflow_X: 1; // true if fusion of the optical flow X axis has encountered a numerical error + bool bad_optflow_Y: 1; // true if fusion of the optical flow Y axis has encountered a numerical error }; // publish the status of various GPS quality checks diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index a1d9fc4b44..10add1e56e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -42,39 +42,58 @@ #include "ekf.h" Ekf::Ekf(): - _control_status{}, - _filter_initialised(false), - _earth_rate_initialised(false), - _fuse_height(false), - _fuse_pos(false), - _fuse_hor_vel(false), - _fuse_vert_vel(false), - _time_last_fake_gps(0), - _time_last_pos_fuse(0), - _time_last_vel_fuse(0), - _time_last_hgt_fuse(0), - _time_last_of_fuse(0), - _vel_pos_innov{}, - _mag_innov{}, - _heading_innov{}, - _vel_pos_innov_var{}, - _mag_innov_var{}, - _heading_innov_var{} + _filter_initialised(false), + _earth_rate_initialised(false), + _fuse_height(false), + _fuse_pos(false), + _fuse_hor_vel(false), + _fuse_vert_vel(false), + _time_last_fake_gps(0), + _time_last_pos_fuse(0), + _time_last_vel_fuse(0), + _time_last_hgt_fuse(0), + _time_last_of_fuse(0), + _last_disarmed_posD(0.0f), + _heading_innov(0.0f), + _heading_innov_var(0.0f), + _mag_declination(0.0f), + _gpsDriftVelN(0.0f), + _gpsDriftVelE(0.0f), + _gps_drift_velD(0.0f), + _gps_velD_diff_filt(0.0f), + _gps_velN_filt(0.0f), + _gps_velE_filt(0.0f), + _last_gps_fail_us(0), + _last_gps_origin_time_us(0), + _gps_alt_ref(0.0f), + _baro_counter(0), + _baro_sum(0.0f), + _mag_counter(0), + _baro_at_alignment(0.0f) { - _earth_rate_NED.setZero(); - _R_prev = matrix::Dcm(); + _control_status = {}; + _control_status_prev = {}; + _state = {}; + _last_known_posNE.setZero(); + _earth_rate_NED.setZero(); + _R_prev = matrix::Dcm(); + _vel_pos_innov = {}; + _mag_innov = {}; + _vel_pos_innov_var = {}; + _mag_innov_var = {}; _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); _vel_corr.setZero(); - _last_known_posNE.setZero(); + _imu_down_sampled = {}; + _q_down_sampled.setZero(); + _mag_sum = {}; + _delVel_sum = {}; } - Ekf::~Ekf() { - - } + bool Ekf::init(uint64_t timestamp) { bool ret = initialise_interface(timestamp); @@ -96,7 +115,6 @@ bool Ekf::init(uint64_t timestamp) _output_new.pos.setZero(); _output_new.quat_nominal = matrix::Quaternion(); - _imu_down_sampled.delta_ang.setZero(); _imu_down_sampled.delta_vel.setZero(); _imu_down_sampled.delta_ang_dt = 0.0f; @@ -141,7 +159,7 @@ bool Ekf::update() // measurement updates - // Fuse magnetometer data using the selected fuson method and only if angular alignment is complete + // Fuse magnetometer data using the selected fusion method and only if angular alignment is complete if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) { fuseMag(); @@ -220,7 +238,7 @@ bool Ekf::initialiseFilter(void) _baro_sum += baro_init.hgt; } - // check to see if we have enough measruements and return false if not + // check to see if we have enough measurements and return false if not if (_baro_counter < 10 || _mag_counter < 10) { return false; @@ -283,7 +301,7 @@ void Ekf::predictState() } } - // attitude error state prediciton + // attitude error state prediction matrix::Dcm R_to_earth(_state.quat_nominal); // transformation matrix from body to world frame Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; @@ -306,10 +324,8 @@ void Ekf::predictState() constrainStates(); } - bool Ekf::collect_imu(imuSample &imu) { - imu.delta_ang(0) = imu.delta_ang(0) * _state.gyro_scale(0); imu.delta_ang(1) = imu.delta_ang(1) * _state.gyro_scale(1); imu.delta_ang(2) = imu.delta_ang(2) * _state.gyro_scale(2); @@ -317,14 +333,11 @@ bool Ekf::collect_imu(imuSample &imu) imu.delta_ang -= _state.gyro_bias * imu.delta_ang_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f); imu.delta_vel(2) -= _state.accel_z_bias * imu.delta_vel_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);; - // store the new sample for the complementary filter prediciton - _imu_sample_new = { - .delta_ang = imu.delta_ang, - .delta_vel = imu.delta_vel, - .delta_ang_dt = imu.delta_ang_dt, - .delta_vel_dt = imu.delta_vel_dt, - .time_us = imu.time_us - }; + _imu_sample_new.delta_ang = imu.delta_ang; + _imu_sample_new.delta_vel = imu.delta_vel; + _imu_sample_new.delta_ang_dt = imu.delta_ang_dt; + _imu_sample_new.delta_vel_dt = imu.delta_vel_dt; + _imu_sample_new.time_us = imu.time_us; _imu_down_sampled.delta_ang_dt += imu.delta_ang_dt; _imu_down_sampled.delta_vel_dt += imu.delta_vel_dt; @@ -341,13 +354,13 @@ bool Ekf::collect_imu(imuSample &imu) if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000) || _dt_imu_avg * _imu_ticks >= 0.02f) { - imu = { - .delta_ang = _q_down_sampled.to_axis_angle(), - .delta_vel = _imu_down_sampled.delta_vel, - .delta_ang_dt = _imu_down_sampled.delta_ang_dt, - .delta_vel_dt = _imu_down_sampled.delta_vel_dt, - .time_us = imu.time_us - }; + + imu.delta_ang = _q_down_sampled.to_axis_angle(); + imu.delta_vel = _imu_down_sampled.delta_vel; + imu.delta_ang_dt = _imu_down_sampled.delta_ang_dt; + imu.delta_vel_dt = _imu_down_sampled.delta_vel_dt; + imu.time_us = imu.time_us; + _imu_down_sampled.delta_ang.setZero(); _imu_down_sampled.delta_vel.setZero(); _imu_down_sampled.delta_ang_dt = 0.0f; @@ -422,10 +435,8 @@ void Ekf::calculateOutputStates() _delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt; _vel_corr = (_state.pos - _output_sample_delayed.pos); - } - void Ekf::fuseAirspeed() { diff --git a/EKF/ekf.h b/EKF/ekf.h index a7a426463a..0559a6ddce 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -86,10 +86,10 @@ public: bool collect_imu(imuSample &imu); // this is the current status of the filter control modes - filter_control_status_u _control_status = {}; + filter_control_status_u _control_status; // this is the previous status of the filter control modes - used to detect mode transitions - filter_control_status_u _control_status_prev = {}; + filter_control_status_u _control_status_prev; // get the ekf WGS-84 origin position and height and the system time it was last set void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt); @@ -97,70 +97,70 @@ public: private: static const uint8_t _k_num_states = 24; - static constexpr float _k_earth_rate = 0.000072921f; + static const float _k_earth_rate = 0.000072921f; - stateSample _state; // state struct of the ekf running at the delayed time horizon + stateSample _state; // state struct of the ekf running at the delayed time horizon bool _filter_initialised; bool _earth_rate_initialised; - bool _fuse_height; // baro height data should be fused - bool _fuse_pos; // gps position data should be fused - bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused - bool _fuse_vert_vel; // gps vertical velocity measurement should be fused + bool _fuse_height; // baro height data should be fused + bool _fuse_pos; // gps position data should be fused + bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused + bool _fuse_vert_vel; // gps vertical velocity measurement should be fused - uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode + uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode - uint64_t _time_last_pos_fuse; // time the last fusion of horizotal position measurements was performed (usec) + uint64_t _time_last_pos_fuse; // time the last fusion of horizontal position measurements was performed (usec) uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec) uint64_t _time_last_hgt_fuse; // time the last fusion of height measurements was performed (usec) uint64_t _time_last_of_fuse; // time the last fusion of optical flow measurements were performed (usec) Vector2f _last_known_posNE; // last known local NE position vector (m) float _last_disarmed_posD; // vertical position recorded at arming (m) - Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s + Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s - matrix::Dcm _R_prev; // transformation matrix from earth frame to body frame of previous ekf step + matrix::Dcm _R_prev; // transformation matrix from earth frame to body frame of previous ekf step float P[_k_num_states][_k_num_states]; // state covariance matrix - float _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos - float _mag_innov[3]; // earth magnetic field innovations - float _heading_innov; // heading measurement innovation + float _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos + float _mag_innov[3]; // earth magnetic field innovations + float _heading_innov; // heading measurement innovation - float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos - float _mag_innov_var[3]; // earth magnetic field innovation variance - float _heading_innov_var; // heading measurement innovation variance + float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos + float _mag_innov_var[3]; // earth magnetic field innovation variance + float _heading_innov_var; // heading measurement innovation variance - float _mag_declination = 0.0f; // magnetic declination used by reset and fusion functions (rad) + float _mag_declination; // magnetic declination used by reset and fusion functions (rad) // complementary filter states - Vector3f _delta_angle_corr; // delta angle correction vector - Vector3f _delta_vel_corr; // delta velocity correction vector - Vector3f _vel_corr; // velocity correction vector - imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate) - Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps) + Vector3f _delta_angle_corr; // delta angle correction vector + Vector3f _delta_vel_corr; // delta velocity correction vector + Vector3f _vel_corr; // velocity correction vector + imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate) + Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps) // variables used for the GPS quality checks - float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s) - float _gpsDriftVelE = 0.0f; // GPS east position derivative (m/s) - float _gps_drift_velD = 0.0f; // GPS down position derivative (m/s) - float _gps_velD_diff_filt = 0.0f; // GPS filtered Down velocity (m/s) - float _gps_velN_filt = 0.0f; // GPS filtered North velocity (m/s) - float _gps_velE_filt = 0.0f; // GPS filtered East velocity (m/s) - uint64_t _last_gps_fail_us = 0; // last system time in usec that the GPS failed it's checks + float _gpsDriftVelN; // GPS north position derivative (m/s) + float _gpsDriftVelE; // GPS east position derivative (m/s) + float _gps_drift_velD; // GPS down position derivative (m/s) + float _gps_velD_diff_filt; // GPS filtered Down velocity (m/s) + float _gps_velN_filt; // GPS filtered North velocity (m/s) + float _gps_velE_filt; // GPS filtered East velocity (m/s) + uint64_t _last_gps_fail_us; // last system time in usec that the GPS failed it's checks // Variables used to publish the WGS-84 location of the EKF local NED origin - uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec) - float _gps_alt_ref = 0.0f; // WGS-84 height (m) + uint64_t _last_gps_origin_time_us; // time the origin was last set (uSec) + float _gps_alt_ref; // WGS-84 height (m) - // Variables used to initialise the filter states - uint8_t _baro_counter = 0; // number of baro samples averaged - float _baro_sum = 0.0f; // summed baro measurement - uint8_t _mag_counter = 0; // number of magnetometer samples averaged - Vector3f _mag_sum = {}; // summed magnetometer measurement - Vector3f _delVel_sum = {}; // summed delta velocity - float _baro_at_alignment; // baro offset relative to alignment position + // Variables used to initialize the filter states + uint8_t _baro_counter; // number of baro samples averaged + float _baro_sum; // summed baro measurement + uint8_t _mag_counter; // number of magnetometer samples averaged + Vector3f _mag_sum; // summed magnetometer measurement + Vector3f _delVel_sum; // summed delta velocity + float _baro_at_alignment; // baro offset relative to alignment position gps_check_fail_status_u _gps_check_fail_status; @@ -168,10 +168,10 @@ private: // and the correction step void calculateOutputStates(); - // initialise filter states of both the delayed ekf and the real time complementary filter + // initialize filter states of both the delayed ekf and the real time complementary filter bool initialiseFilter(void); - // initialise ekf covariance matrix + // initialize ekf covariance matrix void initialiseCovariance(); // predict ekf state @@ -241,15 +241,15 @@ private: // Determine if we are airborne or motors are armed void calculateVehicleStatus(); - // return the square of two foating point numbers - used in autocoded sections + // return the square of two floating point numbers - used in auto coded sections inline float sq(float var) { return var * var; } - // zero the specified range of rows in the state covariance matricx + // zero the specified range of rows in the state covariance matrix void zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); - // zero the specified range of columns in the state covariance matricx + // zero the specified range of columns in the state covariance matrix void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); }; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 38685e52ec..1592222b30 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -215,7 +215,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _airspeed_buffer.allocate(OBS_BUFFER_LENGTH) && _flow_buffer.allocate(OBS_BUFFER_LENGTH) && _output_buffer.allocate(IMU_BUFFER_LENGTH))) { - PX4_WARN("Estimator Buffer Allocation failed!"); +// PX4_WARN("Estimator Buffer Allocation failed!"); unallocate_buffers(); return false; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 5e240178c5..ace2b3d974 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file estimator_base.h + * @file estimator_interface.h * Definition of base class for attitude estimators * * @author Roman Bast @@ -41,7 +41,7 @@ #include #include -#include +#include #include "RingBuffer.h" #include "common.h" @@ -81,10 +81,9 @@ public: virtual void get_covariances(float *covariances) = 0; - // get the ekf WGS-84 origin positoin and height and the system time it was last set + // get the ekf WGS-84 origin position and height and the system time it was last set virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0; - // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; } @@ -105,6 +104,7 @@ public: // set magnetometer data void setMagData(uint64_t time_usec, float *data); + //void setMagData(uint64_t time_usec, struct magSample *mag); // set gps data void setGpsData(uint64_t time_usec, struct gps_message *gps); @@ -165,15 +165,15 @@ public: protected: - parameters _params; // filter parameters + parameters _params; // filter parameters - static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer - static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer - static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; // ekf prediction period in milliseconds + static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer + static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer + static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; // ekf prediction period in milliseconds - float _dt_imu_avg; // average imu update period in s + float _dt_imu_avg; // average imu update period in s - imuSample _imu_sample_delayed; // captures the imu sample on the delayed time horizon + imuSample _imu_sample_delayed; // captures the imu sample on the delayed time horizon // measurement samples capturing measurements on the delayed time horizon magSample _mag_sample_delayed; @@ -183,26 +183,25 @@ protected: airspeedSample _airspeed_sample_delayed; flowSample _flow_sample_delayed; - outputSample _output_sample_delayed; // filter output on the delayed time horizon - outputSample _output_new; // filter output on the non-delayed time horizon - imuSample _imu_sample_new; // imu sample capturing the newest imu data + outputSample _output_sample_delayed; // filter output on the delayed time horizon + outputSample _output_new; // filter output on the non-delayed time horizon + imuSample _imu_sample_new; // imu sample capturing the newest imu data - uint64_t _imu_ticks; // counter for imu updates + uint64_t _imu_ticks; // counter for imu updates - bool _imu_updated = false; // true if the ekf should update (completed downsampling process) - bool _initialised = false; // true if ekf interface instance (data buffering) is initialised - bool _vehicle_armed = false; // vehicle arm status used to turn off functionality used on the ground - bool _in_air = false; // we assume vehicle is in the air, set by the given landing detector + bool _imu_updated; // true if the ekf should update (completed downsampling process) + bool _initialised; // true if the ekf interface instance (data buffering) is initialized + bool _vehicle_armed; // vehicle arm status used to turn off functionality used on the ground + bool _in_air; // we assume vehicle is in the air, set by the given landing detector - bool _NED_origin_initialised = false; - bool _gps_speed_valid = false; - float _gps_speed_accuracy = 0.0f; // GPS receiver reported speed accuracy (m/s) - struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) + bool _NED_origin_initialised; + bool _gps_speed_valid; + float _gps_speed_accuracy; // GPS receiver reported speed accuracy (m/s) + struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians) - bool _mag_healthy = false; // computed by mag innovation test + bool _mag_healthy; // computed by mag innovation test float _yaw_test_ratio; // yaw innovation consistency check ratio float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios - float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios // data buffer instances @@ -215,14 +214,13 @@ protected: RingBuffer _flow_buffer; RingBuffer _output_buffer; - uint64_t _time_last_imu; // timestamp of last imu sample in microseconds - uint64_t _time_last_gps; // timestamp of last gps measurement in microseconds - uint64_t _time_last_mag; // timestamp of last magnetometer measurement in microseconds - uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds - uint64_t _time_last_range; // timestamp of last range measurement in microseconds + uint64_t _time_last_imu; // timestamp of last imu sample in microseconds + uint64_t _time_last_gps; // timestamp of last gps measurement in microseconds + uint64_t _time_last_mag; // timestamp of last magnetometer measurement in microseconds + uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds + uint64_t _time_last_range; // timestamp of last range measurement in microseconds uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds - fault_status_t _fault_status; // allocate data buffers and intialise interface variables @@ -231,8 +229,6 @@ protected: // free buffer memory void unallocate_buffers(); - float _mag_declination_gps = - 0.0f; // magnetic declination returned by the geo library using the last valid GPS position (rad) - - float _mag_declination_to_save_deg = 0.0f; // magnetic declination to save to EKF2_MAG_DECL (deg) + float _mag_declination_gps; // magnetic declination returned by the geo library using the last valid GPS position (rad) + float _mag_declination_to_save_deg; // magnetic declination to save to EKF2_MAG_DECL (deg) }; From 72243c4a84b7d23a9d8772f3c904931b7717d8a3 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Thu, 18 Feb 2016 03:28:40 -0700 Subject: [PATCH 02/11] Resolve tab/space differences with upstream master. --- EKF/common.h | 4 +-- EKF/ekf.cpp | 56 +++++++++++++++++++-------------------- EKF/estimator_interface.h | 2 +- 3 files changed, 31 insertions(+), 31 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 955319c962..3fbcaaa618 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -144,8 +144,8 @@ struct parameters { float mag_declination_deg; // magnetic declination in degrees float heading_innov_gate; // heading fusion innovation consistency gate size in standard deviations float mag_innov_gate; // magnetometer fusion innovation consistency gate size in standard deviations - int mag_declination_source; // bitmask used to control the handling of declination data - int mag_fusion_type; // integer used to specify the type of magnetometer fusion used + int mag_declination_source; // bitmask used to control the handling of declination data + int mag_fusion_type; // integer used to specify the type of magnetometer fusion used // these parameters control the strictness of GPS quality checks used to determine uf the GPS is // good enough to set a local origin and commence aiding diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 10add1e56e..a596dbde2d 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -42,21 +42,21 @@ #include "ekf.h" Ekf::Ekf(): - _filter_initialised(false), - _earth_rate_initialised(false), - _fuse_height(false), - _fuse_pos(false), - _fuse_hor_vel(false), - _fuse_vert_vel(false), - _time_last_fake_gps(0), - _time_last_pos_fuse(0), - _time_last_vel_fuse(0), - _time_last_hgt_fuse(0), - _time_last_of_fuse(0), - _last_disarmed_posD(0.0f), + _filter_initialised(false), + _earth_rate_initialised(false), + _fuse_height(false), + _fuse_pos(false), + _fuse_hor_vel(false), + _fuse_vert_vel(false), + _time_last_fake_gps(0), + _time_last_pos_fuse(0), + _time_last_vel_fuse(0), + _time_last_hgt_fuse(0), + _time_last_of_fuse(0), + _last_disarmed_posD(0.0f), _heading_innov(0.0f), _heading_innov_var(0.0f), - _mag_declination(0.0f), + _mag_declination(0.0f), _gpsDriftVelN(0.0f), _gpsDriftVelE(0.0f), _gps_drift_velD(0.0f), @@ -69,25 +69,25 @@ Ekf::Ekf(): _baro_counter(0), _baro_sum(0.0f), _mag_counter(0), - _baro_at_alignment(0.0f) + _baro_at_alignment(0.0f) { - _control_status = {}; - _control_status_prev = {}; - _state = {}; - _last_known_posNE.setZero(); - _earth_rate_NED.setZero(); - _R_prev = matrix::Dcm(); - _vel_pos_innov = {}; - _mag_innov = {}; - _vel_pos_innov_var = {}; - _mag_innov_var = {}; + _control_status = {}; + _control_status_prev = {}; + _state = {}; + _last_known_posNE.setZero(); + _earth_rate_NED.setZero(); + _R_prev = matrix::Dcm(); + _vel_pos_innov = {}; + _mag_innov = {}; + _vel_pos_innov_var = {}; + _mag_innov_var = {}; _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); _vel_corr.setZero(); - _imu_down_sampled = {}; - _q_down_sampled.setZero(); - _mag_sum = {}; - _delVel_sum = {}; + _imu_down_sampled = {}; + _q_down_sampled.setZero(); + _mag_sum = {}; + _delVel_sum = {}; } Ekf::~Ekf() diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index ace2b3d974..3ddf44890a 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -192,7 +192,7 @@ protected: bool _imu_updated; // true if the ekf should update (completed downsampling process) bool _initialised; // true if the ekf interface instance (data buffering) is initialized bool _vehicle_armed; // vehicle arm status used to turn off functionality used on the ground - bool _in_air; // we assume vehicle is in the air, set by the given landing detector + bool _in_air; // we assume vehicle is in the air, set by the given landing detector bool _NED_origin_initialised; bool _gps_speed_valid; From 4ce4724105637dceee45240f93c5b2c673ccaea5 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Fri, 19 Feb 2016 23:57:06 -0700 Subject: [PATCH 03/11] Added variable initializations back into EstimatorInterface() constructor to resolve a runtime error that occurs with uninitialized variables. --- EKF/estimator_interface.cpp | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 1592222b30..a648b9486a 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -47,8 +47,30 @@ #include -EstimatorInterface::EstimatorInterface() +EstimatorInterface::EstimatorInterface(): + _dt_imu_avg(0.0f), + _imu_ticks(0), + _imu_updated(false), + _initialised(false), + _vehicle_armed(false), + _in_air(false), + _NED_origin_initialised(false), + _gps_speed_valid(false), + _gps_speed_accuracy(0.0f), + _mag_healthy(false), + _yaw_test_ratio(0.0f), + _time_last_imu(0), + _time_last_gps(0), + _time_last_mag(0), + _time_last_baro(0), + _time_last_range(0), + _time_last_airspeed(0), + _mag_declination_gps(0.0f), + _mag_declination_to_save_deg(0.0f) { + _pos_ref = {}; + _mag_test_ratio = {}; + _vel_pos_test_ratio = {}; } EstimatorInterface::~EstimatorInterface() From 6c96f45f085feaa5e729cf1cdb921a99f0bd25cd Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 23 Feb 2016 19:49:27 -0700 Subject: [PATCH 04/11] Remove whitespace differences with upstream for pull request. --- EKF/common.h | 80 +++++++++++++++++++-------------------- EKF/ekf.cpp | 1 + EKF/ekf.h | 42 ++++++++++---------- EKF/estimator_interface.h | 31 +++++++-------- 4 files changed, 78 insertions(+), 76 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index dd3b96f68f..c8fed865b3 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -67,45 +67,45 @@ typedef matrix::Quaternion Quaternion; typedef matrix::Matrix Matrix3f; struct outputSample { - Quaternion quat_nominal; // nominal quaternion describing vehicle attitude - Vector3f vel; // NED velocity estimate in earth frame in m/s - Vector3f pos; // NED position estimate in earth frame in m/s - uint64_t time_us; // timestamp in microseconds + Quaternion quat_nominal; // nominal quaternion describing vehicle attitude + Vector3f vel; // NED velocity estimate in earth frame in m/s + Vector3f pos; // NED position estimate in earth frame in m/s + uint64_t time_us; // timestamp in microseconds }; struct imuSample { - Vector3f delta_ang; // delta angle in body frame (integrated gyro measurements) - Vector3f delta_vel; // delta velocity in body frame (integrated accelerometer measurements) - float delta_ang_dt; // delta angle integration period in seconds - float delta_vel_dt; // delta velocity integration period in seconds - uint64_t time_us; // timestamp in microseconds + Vector3f delta_ang; // delta angle in body frame (integrated gyro measurements) + Vector3f delta_vel; // delta velocity in body frame (integrated accelerometer measurements) + float delta_ang_dt; // delta angle integration period in seconds + float delta_vel_dt; // delta velocity integration period in seconds + uint64_t time_us; // timestamp in microseconds }; struct gpsSample { - Vector2f pos; // NE earth frame gps horizontal position measurement in m - float hgt; // gps height measurement in m - Vector3f vel; // NED earth frame gps velocity measurement in m/s - uint64_t time_us; // timestamp in microseconds + Vector2f pos; // NE earth frame gps horizontal position measurement in m + float hgt; // gps height measurement in m + Vector3f vel; // NED earth frame gps velocity measurement in m/s + uint64_t time_us; // timestamp in microseconds }; struct magSample { - Vector3f mag; // NED magnetometer body frame measurements - uint64_t time_us; // timestamp in microseconds + Vector3f mag; // NED magnetometer body frame measurements + uint64_t time_us; // timestamp in microseconds }; struct baroSample { - float hgt; // barometer height above sea level measurement in m - uint64_t time_us; // timestamp in microseconds + float hgt; // barometer height above sea level measurement in m + uint64_t time_us; // timestamp in microseconds }; struct rangeSample { - float rng; // range (distance to ground) measurement in m - uint64_t time_us; // timestamp in microseconds + float rng; // range (distance to ground) measurement in m + uint64_t time_us; // timestamp in microseconds }; struct airspeedSample { - float airspeed; // airspeed measurement in m/s - uint64_t time_us; // timestamp in microseconds + float airspeed; // airspeed measurement in m/s + uint64_t time_us; // timestamp in microseconds }; struct flowSample { @@ -217,28 +217,28 @@ struct parameters { #define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions. struct stateSample { - Vector3f ang_error; // attitude axis angle error (error state formulation) - Vector3f vel; // NED velocity in earth frame in m/s - Vector3f pos; // NED position in earth frame in m - Vector3f gyro_bias; // gyro bias estimate in rad/s - Vector3f gyro_scale; // gyro scale estimate - float accel_z_bias; // accelerometer z axis bias estimate - Vector3f mag_I; // NED earth magnetic field in gauss - Vector3f mag_B; // magnetometer bias estimate in body frame in gauss - Vector2f wind_vel; // wind velocity in m/s - Quaternion quat_nominal; // nominal quaternion describing vehicle attitude + Vector3f ang_error; // attitude axis angle error (error state formulation) + Vector3f vel; // NED velocity in earth frame in m/s + Vector3f pos; // NED position in earth frame in m + Vector3f gyro_bias; // gyro bias estimate in rad/s + Vector3f gyro_scale; // gyro scale estimate + float accel_z_bias; // accelerometer z axis bias estimate + Vector3f mag_I; // NED earth magnetic field in gauss + Vector3f mag_B; // magnetometer bias estimate in body frame in gauss + Vector2f wind_vel; // wind velocity in m/s + Quaternion quat_nominal; // nominal quaternion describing vehicle attitude }; struct fault_status_t { - bool bad_mag_x: 1; // true if the fusion of the magnetometer X-axis has encountered a numerical error - bool bad_mag_y: 1; // true if the fusion of the magnetometer Y-axis has encountered a numerical error - bool bad_mag_z: 1; // true if the fusion of the magnetometer Z-axis has encountered a numerical error - bool bad_mag_hdg: 1; // true if the fusion of the magnetic heading has encountered a numerical error - bool bad_mag_decl: 1; // true if the fusion of the magnetic declination has encountered a numerical error - bool bad_airspeed: 1; // true if fusion of the airspeed has encountered a numerical error - bool bad_sideslip: 1; // true if fusion of the synthetic sideslip constraint has encountered a numerical error - bool bad_optflow_X: 1; // true if fusion of the optical flow X axis has encountered a numerical error - bool bad_optflow_Y: 1; // true if fusion of the optical flow Y axis has encountered a numerical error + bool bad_mag_x: 1; // true if the fusion of the magnetometer X-axis has encountered a numerical error + bool bad_mag_y: 1; // true if the fusion of the magnetometer Y-axis has encountered a numerical error + bool bad_mag_z: 1; // true if the fusion of the magnetometer Z-axis has encountered a numerical error + bool bad_mag_hdg: 1; // true if the fusion of the magnetic heading has encountered a numerical error + bool bad_mag_decl: 1; // true if the fusion of the magnetic declination has encountered a numerical error + bool bad_airspeed: 1; // true if fusion of the airspeed has encountered a numerical error + bool bad_sideslip: 1; // true if fusion of the synthetic sideslip constraint has encountered a numerical error + bool bad_optflow_X: 1; // true if fusion of the optical flow X axis has encountered a numerical error + bool bad_optflow_Y: 1; // true if fusion of the optical flow Y axis has encountered a numerical error }; // publish the status of various GPS quality checks diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 503af3c837..d6f8241982 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -83,6 +83,7 @@ Ekf::Ekf(): _mag_innov_var = {}; _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); + _last_known_posNE.setZero(); _vel_corr.setZero(); _imu_down_sampled = {}; _q_down_sampled.setZero(); diff --git a/EKF/ekf.h b/EKF/ekf.h index cee50b7f06..c68e818a44 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -100,17 +100,17 @@ private: static const uint8_t _k_num_states = 24; static const float _k_earth_rate = 0.000072921f; - stateSample _state; // state struct of the ekf running at the delayed time horizon + stateSample _state; // state struct of the ekf running at the delayed time horizon bool _filter_initialised; bool _earth_rate_initialised; - bool _fuse_height; // baro height data should be fused - bool _fuse_pos; // gps position data should be fused - bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused - bool _fuse_vert_vel; // gps vertical velocity measurement should be fused + bool _fuse_height; // baro height data should be fused + bool _fuse_pos; // gps position data should be fused + bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused + bool _fuse_vert_vel; // gps vertical velocity measurement should be fused - uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode + uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode uint64_t _time_last_pos_fuse; // time the last fusion of horizontal position measurements was performed (usec) uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec) @@ -119,28 +119,28 @@ private: Vector2f _last_known_posNE; // last known local NE position vector (m) float _last_disarmed_posD; // vertical position recorded at arming (m) - Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s + Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s - matrix::Dcm _R_prev; // transformation matrix from earth frame to body frame of previous ekf step + matrix::Dcm _R_prev; // transformation matrix from earth frame to body frame of previous ekf step float P[_k_num_states][_k_num_states]; // state covariance matrix - float _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos - float _mag_innov[3]; // earth magnetic field innovations - float _heading_innov; // heading measurement innovation + float _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos + float _mag_innov[3]; // earth magnetic field innovations + float _heading_innov; // heading measurement innovation - float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos - float _mag_innov_var[3]; // earth magnetic field innovation variance - float _heading_innov_var; // heading measurement innovation variance + float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos + float _mag_innov_var[3]; // earth magnetic field innovation variance + float _heading_innov_var; // heading measurement innovation variance float _mag_declination; // magnetic declination used by reset and fusion functions (rad) // complementary filter states - Vector3f _delta_angle_corr; // delta angle correction vector - Vector3f _delta_vel_corr; // delta velocity correction vector - Vector3f _vel_corr; // velocity correction vector - imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate) - Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps) + Vector3f _delta_angle_corr; // delta angle correction vector + Vector3f _delta_vel_corr; // delta velocity correction vector + Vector3f _vel_corr; // velocity correction vector + imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate) + Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps) // variables used for the GPS quality checks float _gpsDriftVelN; // GPS north position derivative (m/s) @@ -169,10 +169,10 @@ private: // and the correction step void calculateOutputStates(); - // initialize filter states of both the delayed ekf and the real time complementary filter + // initialise filter states of both the delayed ekf and the real time complementary filter bool initialiseFilter(void); - // initialize ekf covariance matrix + // initialise ekf covariance matrix void initialiseCovariance(); // predict ekf state diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 2bc98222a6..07920aa7ea 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -164,15 +164,15 @@ public: protected: - parameters _params; // filter parameters + parameters _params; // filter parameters - static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer - static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer - static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; // ekf prediction period in milliseconds + static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer + static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer + static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; // ekf prediction period in milliseconds - float _dt_imu_avg; // average imu update period in s + float _dt_imu_avg; // average imu update period in s - imuSample _imu_sample_delayed; // captures the imu sample on the delayed time horizon + imuSample _imu_sample_delayed; // captures the imu sample on the delayed time horizon // measurement samples capturing measurements on the delayed time horizon magSample _mag_sample_delayed; @@ -182,11 +182,11 @@ protected: airspeedSample _airspeed_sample_delayed; flowSample _flow_sample_delayed; - outputSample _output_sample_delayed; // filter output on the delayed time horizon - outputSample _output_new; // filter output on the non-delayed time horizon - imuSample _imu_sample_new; // imu sample capturing the newest imu data + outputSample _output_sample_delayed; // filter output on the delayed time horizon + outputSample _output_new; // filter output on the non-delayed time horizon + imuSample _imu_sample_new; // imu sample capturing the newest imu data - uint64_t _imu_ticks; // counter for imu updates + uint64_t _imu_ticks; // counter for imu updates bool _imu_updated; // true if the ekf should update (completed downsampling process) bool _initialised; // true if the ekf interface instance (data buffering) is initialized @@ -201,6 +201,7 @@ protected: bool _mag_healthy; // computed by mag innovation test float _yaw_test_ratio; // yaw innovation consistency check ratio float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios + float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios // data buffer instances @@ -213,11 +214,11 @@ protected: RingBuffer _flow_buffer; RingBuffer _output_buffer; - uint64_t _time_last_imu; // timestamp of last imu sample in microseconds - uint64_t _time_last_gps; // timestamp of last gps measurement in microseconds - uint64_t _time_last_mag; // timestamp of last magnetometer measurement in microseconds - uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds - uint64_t _time_last_range; // timestamp of last range measurement in microseconds + uint64_t _time_last_imu; // timestamp of last imu sample in microseconds + uint64_t _time_last_gps; // timestamp of last gps measurement in microseconds + uint64_t _time_last_mag; // timestamp of last magnetometer measurement in microseconds + uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds + uint64_t _time_last_range; // timestamp of last range measurement in microseconds uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds fault_status_t _fault_status; From 79d07c831f2c70f51f034d0d714c804dbf8dfd9e Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 23 Feb 2016 19:53:55 -0700 Subject: [PATCH 05/11] Convert spaces to tabs to match upstream. --- EKF/common.h | 8 ++++---- EKF/ekf.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index c8fed865b3..c93d440b17 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -67,10 +67,10 @@ typedef matrix::Quaternion Quaternion; typedef matrix::Matrix Matrix3f; struct outputSample { - Quaternion quat_nominal; // nominal quaternion describing vehicle attitude - Vector3f vel; // NED velocity estimate in earth frame in m/s - Vector3f pos; // NED position estimate in earth frame in m/s - uint64_t time_us; // timestamp in microseconds + Quaternion quat_nominal; // nominal quaternion describing vehicle attitude + Vector3f vel; // NED velocity estimate in earth frame in m/s + Vector3f pos; // NED position estimate in earth frame in m/s + uint64_t time_us; // timestamp in microseconds }; struct imuSample { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index d6f8241982..c7d65ac6e0 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -83,7 +83,7 @@ Ekf::Ekf(): _mag_innov_var = {}; _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); - _last_known_posNE.setZero(); + _last_known_posNE.setZero(); _vel_corr.setZero(); _imu_down_sampled = {}; _q_down_sampled.setZero(); From a4cecb1704cc6b88ec111dcd8213664550b3bd18 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 23 Feb 2016 19:57:27 -0700 Subject: [PATCH 06/11] Match variable initialization order difference with upstream. --- EKF/ekf.cpp | 2 +- EKF/ekf.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index c7d65ac6e0..605326c124 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -83,8 +83,8 @@ Ekf::Ekf(): _mag_innov_var = {}; _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); + _vel_corr.setZero(); _last_known_posNE.setZero(); - _vel_corr.setZero(); _imu_down_sampled = {}; _q_down_sampled.setZero(); _mag_sum = {}; diff --git a/EKF/ekf.h b/EKF/ekf.h index c68e818a44..25fb0f0138 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -155,7 +155,7 @@ private: uint64_t _last_gps_origin_time_us; // time the origin was last set (uSec) float _gps_alt_ref; // WGS-84 height (m) - // Variables used to initialize the filter states + // Variables used to initialise the filter states uint8_t _baro_counter; // number of baro samples averaged float _baro_sum; // summed baro measurement uint8_t _mag_counter; // number of magnetometer samples averaged From ccb5736353edcaf9df1fd7422979021cdcce9898 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 23 Feb 2016 19:58:30 -0700 Subject: [PATCH 07/11] Spaces to tab. --- EKF/ekf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 605326c124..547b447545 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -83,7 +83,7 @@ Ekf::Ekf(): _mag_innov_var = {}; _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); - _vel_corr.setZero(); + _vel_corr.setZero(); _last_known_posNE.setZero(); _imu_down_sampled = {}; _q_down_sampled.setZero(); From f9f00fa52b5e2b5a71ca6f8d45078fd5e287d416 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 23 Feb 2016 14:00:39 -0700 Subject: [PATCH 08/11] Remove unnecessary include. --- EKF/common.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index c93d440b17..861e6294a5 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -31,8 +31,6 @@ * ****************************************************************************/ -#include "Matrix.hpp" - /** * @file common.h * Definition of base class for attitude estimators From 48e80e9e3ee8622e9605446778806ca69d266ae1 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 23 Feb 2016 15:16:40 -0700 Subject: [PATCH 09/11] Correct C style array initialization. --- EKF/ekf.cpp | 8 ++++---- EKF/estimator_interface.cpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 547b447545..beeca470b1 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -77,10 +77,10 @@ Ekf::Ekf(): _last_known_posNE.setZero(); _earth_rate_NED.setZero(); _R_prev = matrix::Dcm(); - _vel_pos_innov = {}; - _mag_innov = {}; - _vel_pos_innov_var = {}; - _mag_innov_var = {}; + _vel_pos_innov = {0}; + _mag_innov = {0}; + _vel_pos_innov_var = {0}; + _mag_innov_var = {0}; _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); _vel_corr.setZero(); diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 2dbff4ada7..3bfeb7b64d 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -69,8 +69,8 @@ EstimatorInterface::EstimatorInterface(): _mag_declination_to_save_deg(0.0f) { _pos_ref = {}; - _mag_test_ratio = {}; - _vel_pos_test_ratio = {}; + _mag_test_ratio = {0}; + _vel_pos_test_ratio = {0}; } EstimatorInterface::~EstimatorInterface() From 342010c113f617814d0fed9c03dfe109eff4180d Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 23 Feb 2016 15:52:02 -0700 Subject: [PATCH 10/11] Update c style array initialization to attempt to pass Travic CI build tests. --- EKF/ekf.cpp | 8 ++++---- EKF/estimator_interface.cpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index beeca470b1..fe7e617990 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -77,10 +77,10 @@ Ekf::Ekf(): _last_known_posNE.setZero(); _earth_rate_NED.setZero(); _R_prev = matrix::Dcm(); - _vel_pos_innov = {0}; - _mag_innov = {0}; - _vel_pos_innov_var = {0}; - _mag_innov_var = {0}; + memset(_vel_pos_innov, 0, sizeof(_vel_pos_innov)); + memset(_mag_innov, 0, sizeof(_mag_innov)); + memset(_vel_pos_innov_var, 0, sizeof(_vel_pos_innov_var)); + memset(_mag_innov_var, 0, sizeof(_mag_innov_var)); _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); _vel_corr.setZero(); diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 3bfeb7b64d..32dbc5c5e6 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -69,8 +69,8 @@ EstimatorInterface::EstimatorInterface(): _mag_declination_to_save_deg(0.0f) { _pos_ref = {}; - _mag_test_ratio = {0}; - _vel_pos_test_ratio = {0}; + memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio)); + memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio)); } EstimatorInterface::~EstimatorInterface() From 6613335937a8402ebd8799883a7ae183c07f82ee Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 23 Feb 2016 16:15:52 -0700 Subject: [PATCH 11/11] Added constexpr back from const var type. --- EKF/ekf.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 25fb0f0138..f657382916 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -98,7 +98,7 @@ public: private: static const uint8_t _k_num_states = 24; - static const float _k_earth_rate = 0.000072921f; + static constexpr float _k_earth_rate = 0.000072921f; stateSample _state; // state struct of the ekf running at the delayed time horizon