diff --git a/EKF/common.h b/EKF/common.h index e110cdc495..861e6294a5 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -32,13 +32,14 @@ ****************************************************************************/ /** - * @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 { @@ -112,54 +113,100 @@ 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 diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 1e8606e277..fe7e617990 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -42,7 +42,6 @@ #include "ekf.h" Ekf::Ekf(): - _control_status{}, _filter_initialised(false), _earth_rate_initialised(false), _fuse_height(false), @@ -54,27 +53,48 @@ Ekf::Ekf(): _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{} + _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) { + _control_status = {}; + _control_status_prev = {}; + _state = {}; + _last_known_posNE.setZero(); _earth_rate_NED.setZero(); _R_prev = matrix::Dcm(); + 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(); _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 +116,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 +160,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(); @@ -223,7 +242,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; @@ -286,7 +305,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; @@ -309,10 +328,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); @@ -320,14 +337,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; @@ -344,13 +358,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; @@ -425,10 +439,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 fad67b5d7e..f657382916 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -87,10 +87,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); @@ -112,7 +112,7 @@ private: 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) @@ -133,7 +133,7 @@ private: 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 @@ -143,25 +143,25 @@ private: 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 + 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; @@ -245,15 +245,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 008d224d13..32dbc5c5e6 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -47,8 +47,30 @@ #include "mathlib.h" -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 = {}; + memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio)); + memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio)); } EstimatorInterface::~EstimatorInterface() diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 6d558d5212..07920aa7ea 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 @@ -80,10 +80,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; } @@ -104,6 +103,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); @@ -188,17 +188,17 @@ protected: 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 @@ -221,7 +221,6 @@ protected: 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 @@ -230,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) };