ekf2 initialization fixes

EKF/common.h

@@ -108,8 +108,8 @@ struct magSample {
 };
 
 struct baroSample {
-	float       hgt;	// barometer height above sea level measurement in m
-	uint64_t    time_us;	// timestamp in microseconds
+	float       hgt{0.0f};	// barometer height above sea level measurement in m
+	uint64_t    time_us{0};	// timestamp in microseconds
 };
 
 struct rangeSample {
@@ -182,92 +182,92 @@ struct dragSample {
 
 struct parameters {
 	// measurement source control
-	int fusion_mode;		// bitmasked integer that selects which aiding sources will be used
-	int vdist_sensor_type;		// selects the primary source for height data
-	int sensor_interval_min_ms;	// minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers.
+	int fusion_mode{MASK_USE_GPS};		// bitmasked integer that selects which aiding sources will be used
+	int vdist_sensor_type{VDIST_SENSOR_BARO};		// selects the primary source for height data
+	int sensor_interval_min_ms{20};	// minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers.
 
 	// measurement time delays
-	float mag_delay_ms;		// magnetometer measurement delay relative to the IMU (msec)
-	float baro_delay_ms;		// barometer height measurement delay relative to the IMU (msec)
-	float gps_delay_ms;		// GPS measurement delay relative to the IMU (msec)
-	float airspeed_delay_ms;	// airspeed measurement delay relative to the IMU (msec)
-	float flow_delay_ms;		// optical flow measurement delay relative to the IMU (msec) - this is to the middle of the optical flow integration interval
-	float range_delay_ms;		// range finder measurement delay relative to the IMU (msec)
-	float ev_delay_ms;		// off-board vision measurement delay relative to the IMU (msec)
+	float mag_delay_ms{0.0f};		// magnetometer measurement delay relative to the IMU (msec)
+	float baro_delay_ms{0.0f};		// barometer height measurement delay relative to the IMU (msec)
+	float gps_delay_ms{200.0f};		// GPS measurement delay relative to the IMU (msec)
+	float airspeed_delay_ms{200.0f};	// airspeed measurement delay relative to the IMU (msec)
+	float flow_delay_ms{5.0f};		// optical flow measurement delay relative to the IMU (msec) - this is to the middle of the optical flow integration interval
+	float range_delay_ms{5.0f};		// range finder measurement delay relative to the IMU (msec)
+	float ev_delay_ms{100.0f};		// off-board vision measurement delay relative to the IMU (msec)
 
 	// input noise
-	float gyro_noise;		// IMU angular rate noise used for covariance prediction (rad/sec)
-	float accel_noise;		// IMU acceleration noise use for covariance prediction (m/sec/sec)
+	float gyro_noise{1.5e-2f};		// IMU angular rate noise used for covariance prediction (rad/sec)
+	float accel_noise{3.5e-1f};		// IMU acceleration noise use for covariance prediction (m/sec/sec)
 
 	// process noise
-	float gyro_bias_p_noise;	// process noise for IMU rate gyro bias prediction (rad/sec**2)
-	float accel_bias_p_noise;	// process noise for IMU accelerometer bias prediction (m/sec**3)
-	float mage_p_noise;		// process noise for earth magnetic field prediction (Guass/sec)
-	float magb_p_noise;		// process noise for body magnetic field prediction (Guass/sec)
-	float wind_vel_p_noise;		// process noise for wind velocity prediction (m/sec/sec)
-	float terrain_p_noise;		// process noise for terrain offset (m/sec)
-	float terrain_gradient;		// gradient of terrain used to estimate process noise due to changing position (m/m)
+	float gyro_bias_p_noise{1.0e-3f};	// process noise for IMU rate gyro bias prediction (rad/sec**2)
+	float accel_bias_p_noise{3.0e-3f};	// process noise for IMU accelerometer bias prediction (m/sec**3)
+	float mage_p_noise{1.0e-3f};		// process noise for earth magnetic field prediction (Guass/sec)
+	float magb_p_noise{1.0e-4};		// process noise for body magnetic field prediction (Guass/sec)
+	float wind_vel_p_noise{1.0e-1f};		// process noise for wind velocity prediction (m/sec/sec)
+	float terrain_p_noise{5.0f};		// process noise for terrain offset (m/sec)
+	float terrain_gradient{0.5f};		// gradient of terrain used to estimate process noise due to changing position (m/m)
 
-	// initialisation errors
-	float switch_on_gyro_bias;	// 1-sigma gyro bias uncertainty at switch on (rad/sec)
-	float switch_on_accel_bias;	// 1-sigma accelerometer bias uncertainty at switch on (m/s**2)
-	float initial_tilt_err;		// 1-sigma tilt error after initial alignment using gravity vector (rad)
+	// initialization errors
+	float switch_on_gyro_bias{0.1f};	// 1-sigma gyro bias uncertainty at switch on (rad/sec)
+	float switch_on_accel_bias{0.2f};	// 1-sigma accelerometer bias uncertainty at switch on (m/s**2)
+	float initial_tilt_err{0.1f};		// 1-sigma tilt error after initial alignment using gravity vector (rad)
 
 	// position and velocity fusion
-	float gps_vel_noise;		// observation noise for gps velocity fusion (m/sec)
-	float gps_pos_noise;		// observation noise for gps position fusion (m)
-	float pos_noaid_noise;		// observation noise for non-aiding position fusion (m)
-	float baro_noise;		// observation noise for barometric height fusion (m)
-	float baro_innov_gate;		// barometric height innovation consistency gate size (STD)
-	float posNE_innov_gate;		// GPS horizontal position innovation consistency gate size (STD)
-	float vel_innov_gate;		// GPS velocity innovation consistency gate size (STD)
-	float hgt_reset_lim;		// The maximum 1-sigma uncertainty in height that can be tolerated before the height state is reset (m)
+	float gps_vel_noise{5.0e-1f};		// observation noise for gps velocity fusion (m/sec)
+	float gps_pos_noise{0.5f};		// observation noise for gps position fusion (m)
+	float pos_noaid_noise{10.0f};		// observation noise for non-aiding position fusion (m)
+	float baro_noise{2.0f};		// observation noise for barometric height fusion (m)
+	float baro_innov_gate{5.0f};		// barometric height innovation consistency gate size (STD)
+	float posNE_innov_gate{5.0f};		// GPS horizontal position innovation consistency gate size (STD)
+	float vel_innov_gate{5.0f};		// GPS velocity innovation consistency gate size (STD)
+	float hgt_reset_lim{0.0f};		// The maximum 1-sigma uncertainty in height that can be tolerated before the height state is reset (m)
 
 	// magnetometer fusion
-	float mag_heading_noise;	// measurement noise used for simple heading fusion (rad)
-	float mag_noise;		// measurement noise used for 3-axis magnetoemeter fusion (Gauss)
-	float mag_declination_deg;	// magnetic declination (degrees)
-	float heading_innov_gate;	// heading fusion innovation consistency gate size (STD)
-	float mag_innov_gate;		// magnetometer fusion innovation consistency gate size (STD)
-	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
+	float mag_heading_noise{3.0e-1f};	// measurement noise used for simple heading fusion (rad)
+	float mag_noise{5.0e-2f};		// measurement noise used for 3-axis magnetoemeter fusion (Gauss)
+	float mag_declination_deg{0.0f};	// magnetic declination (degrees)
+	float heading_innov_gate{2.6f};	// heading fusion innovation consistency gate size (STD)
+	float mag_innov_gate{3.0f};		// magnetometer fusion innovation consistency gate size (STD)
+	int mag_declination_source{7};	// bitmask used to control the handling of declination data
+	int mag_fusion_type{0};		// integer used to specify the type of magnetometer fusion used
 
 	// airspeed fusion
-	float tas_innov_gate;		// True Airspeed Innovation consistency gate size in standard deciation
-  	float eas_noise;			// EAS measurement noise standard deviation used for airspeed fusion [m/s]
+	float tas_innov_gate{5.0f};		// True Airspeed Innovation consistency gate size in standard deciation
+	float eas_noise{1.4f};			// EAS measurement noise standard deviation used for airspeed fusion [m/s]
 
-  	// synthetic sideslip fusion
- 	float beta_innov_gate;		// synthetic sideslip innovation consistency gate size in standard deviation (STD)
- 	float beta_noise;			// synthetic sideslip noise (rad)
- 	float beta_avg_ft_us;		// The average time between synthetic sideslip measurements (usec)
+	// synthetic sideslip fusion
+	float beta_innov_gate{5.0f};		// synthetic sideslip innovation consistency gate size in standard deviation (STD)
+	float beta_noise{0.3f};			// synthetic sideslip noise (rad)
+	float beta_avg_ft_us{1000000.0f};		// The average time between synthetic sideslip measurements (usec)
 
 	// range finder fusion
-	float range_noise;		// observation noise for range finder measurements (m)
-	float range_innov_gate;		// range finder fusion innovation consistency gate size (STD)
-	float rng_gnd_clearance;	// minimum valid value for range when on ground (m)
-	float rng_sens_pitch;		// Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
-	float range_noise_scaler;	// scaling from range measurement to noise (m/m)
+	float range_noise{0.1f};		// observation noise for range finder measurements (m)
+	float range_innov_gate{5.0f};		// range finder fusion innovation consistency gate size (STD)
+	float rng_gnd_clearance{0.1f};	// minimum valid value for range when on ground (m)
+	float rng_sens_pitch{0.0f};		// Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
+	float range_noise_scaler{0.0f};	// scaling from range measurement to noise (m/m)
 
 	// vision position fusion
-	float ev_innov_gate;		// vision estimator fusion innovation consistency gate size (STD)
+	float ev_innov_gate{5.0f};		// vision estimator fusion innovation consistency gate size (STD)
 
 	// optical flow fusion
-	float flow_noise;		// observation noise for optical flow LOS rate measurements (rad/sec)
-	float flow_noise_qual_min;	// observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
-	int flow_qual_min;		// minimum acceptable quality integer from  the flow sensor
-	float flow_innov_gate;		// optical flow fusion innovation consistency gate size (STD)
-	float flow_rate_max;		// maximum valid optical flow rate (rad/sec)
+	float flow_noise{0.15f};		// observation noise for optical flow LOS rate measurements (rad/sec)
+	float flow_noise_qual_min{0.5f};	// observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
+	int flow_qual_min{1};		// minimum acceptable quality integer from  the flow sensor
+	float flow_innov_gate{3.0f};		// optical flow fusion innovation consistency gate size (STD)
+	float flow_rate_max{2.5f};		// maximum valid optical flow rate (rad/sec)
 
 	// 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;     // 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
+	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
 
 	// XYZ offset of sensors in body axes (m)
 	Vector3f imu_pos_body;	// xyz position of IMU in body frame (m)
@@ -277,142 +277,24 @@ struct parameters {
 	Vector3f ev_pos_body;	// xyz position of VI-sensor focal point in body frame (m)
 
 	// output complementary filter tuning
-	float vel_Tau;	// velocity state correction time constant (1/sec)
-	float pos_Tau;	// postion state correction time constant (1/sec)
+	float vel_Tau{0.25f};	// velocity state correction time constant (1/sec)
+	float pos_Tau{0.25f};	// postion state correction time constant (1/sec)
 
 	// accel bias learning control
-	float acc_bias_lim;		// maximum accel bias magnitude (m/s/s)
-	float acc_bias_learn_acc_lim;	// learning is disabled if the magnitude of the IMU acceleration vector is greeater than this (m/sec**2)
-	float acc_bias_learn_gyr_lim;	// learning is disabled if the magnitude of the IMU angular rate vector is greeater than this (rad/sec)
-	float acc_bias_learn_tc;	// time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
+	float acc_bias_lim{0.4f};		// maximum accel bias magnitude (m/s/s)
+	float acc_bias_learn_acc_lim{25.0f};	// learning is disabled if the magnitude of the IMU acceleration vector is greeater than this (m/sec**2)
+	float acc_bias_learn_gyr_lim{3.0f};	// learning is disabled if the magnitude of the IMU angular rate vector is greeater than this (rad/sec)
+	float acc_bias_learn_tc{0.5f};	// time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
 
-	unsigned no_gps_timeout_max;	// maximum time we allow dead reckoning while both gps position and velocity measurements are being
-					// rejected before attempting to reset the states to the GPS measurement (usec)
-	unsigned no_aid_timeout_max;	// maximum lapsed time from last fusion of measurements that constrain drift before
-					// the EKF will report that it is dead-reckoning (usec)
+	unsigned no_gps_timeout_max{7000000};	// maximum time we allow dead reckoning while both gps position and velocity measurements are being
+						// rejected before attempting to reset the states to the GPS measurement (usec)
+	unsigned no_aid_timeout_max{1000000};	// maximum lapsed time from last fusion of measurements that constrain drift before
+						// the EKF will report that it is dead-reckoning (usec)
 
 	// multi-rotor drag specific force fusion
-	float drag_noise;		// observation noise for drag specific force measurements (m/sec**2)
-	float bcoef_x;			// ballistic coefficient along the X-axis (kg/m**2)
-	float bcoef_y;			// ballistic coefficient along the Y-axis (kg/m**2)
-
-	// Initialize parameter values.  Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
-	parameters()
-	{
-		// measurement source control
-		fusion_mode = MASK_USE_GPS;
-		vdist_sensor_type = VDIST_SENSOR_BARO;
-		sensor_interval_min_ms = 20;
-
-		// measurement time delays
-		mag_delay_ms = 0.0f;
-		baro_delay_ms = 0.0f;
-		gps_delay_ms = 200.0f;
-		airspeed_delay_ms = 200.0f;
-		flow_delay_ms = 5.0f;
-		range_delay_ms = 5.0f;
-		ev_delay_ms = 100.0f;
-
-		// input noise
-		gyro_noise = 1.5e-2f;
-		accel_noise = 3.5e-1f;
-
-		// process noise
-		gyro_bias_p_noise = 1.0e-3f;
-		accel_bias_p_noise = 3.0e-3f;
-		mage_p_noise = 1.0e-3f;
-		magb_p_noise = 1.0e-4f;
-		wind_vel_p_noise = 1.0e-1f;
-		terrain_p_noise = 5.0f;
-		terrain_gradient = 0.5f;
-
-		// initialisation errors
-		switch_on_gyro_bias = 0.1f;
-		switch_on_accel_bias = 0.2f;
-		initial_tilt_err = 0.1f;
-
-		// position and velocity fusion
-		gps_vel_noise = 5.0e-1f;
-		gps_pos_noise = 0.5f;
-		pos_noaid_noise = 10.0f;
-		baro_noise = 2.0f;
-		baro_innov_gate = 5.0f;
-		posNE_innov_gate = 5.0f;
-		vel_innov_gate = 5.0f;
-		hgt_reset_lim = 0.0f;
-
-		// magnetometer fusion
-		mag_heading_noise = 3.0e-1f;
-		mag_noise = 5.0e-2f;
-		mag_declination_deg = 0.0f;
-		heading_innov_gate = 2.6f;
-		mag_innov_gate = 3.0f;
-		mag_declination_source = 7;
-		mag_fusion_type = 0;
-
-		// airspeed fusion
-		tas_innov_gate = 5.0f;
-  		eas_noise = 1.4f;
-
-		// synthetic sideslip fusion
- 		beta_innov_gate = 5.0f;
-		beta_noise = 0.3f;
- 		beta_avg_ft_us = 1000000.0f; //1 Hz
-
-		// range finder fusion
-		range_noise = 0.1f;
-		range_innov_gate = 5.0f;
-		rng_gnd_clearance = 0.1f;
-		rng_sens_pitch = 0.0f;
-		range_noise_scaler = 0.0f;
-
-		// vision position fusion
-		ev_innov_gate = 5.0f;
-
-		// optical flow fusion
-		flow_noise = 0.15f;
-		flow_noise_qual_min = 0.5f;
-		flow_qual_min = 1;
-		flow_innov_gate = 3.0f;
-		flow_rate_max = 2.5f;
-
-		// GPS quality checks
-		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;
-
-		// XYZ offset of sensors in body axes (m)
-		imu_pos_body = {};
-		gps_pos_body = {};
-		rng_pos_body = {};
-		flow_pos_body = {};
-		ev_pos_body = {};
-
-		// output complementary filter tuning time constants
-		vel_Tau = 0.25f;
-		pos_Tau = 0.25f;
-
-		// accel bias state limiting
-		acc_bias_lim = 0.4f;
-		acc_bias_learn_acc_lim = 25.0f;
-		acc_bias_learn_gyr_lim = 3.0f;
-		acc_bias_learn_tc = 0.5f;
-
-		// dead reckoning timers
-		no_gps_timeout_max = 7e6;
-		no_aid_timeout_max = 1e6;
-
-		// multi-rotor drag specific force fusion
-		drag_noise =  2.5f;
-		bcoef_x = 25.0f;
-		bcoef_y = 25.0f;
-
-	}
+	float drag_noise{2.5f};		// observation noise for drag specific force measurements (m/sec**2)
+	float bcoef_x{25.0f};			// ballistic coefficient along the X-axis (kg/m**2)
+	float bcoef_y{25.0f};			// ballistic coefficient along the Y-axis (kg/m**2)
 };
 
 struct stateSample {
@@ -463,7 +345,7 @@ union innovation_fault_status_u {
 		bool reject_sideslip: 1;	// 8 - true if the synthetic sideslip observation has been rejected
 		bool reject_hagl: 1;		// 9 - true if the height above ground observation has been rejected
 		bool reject_optflow_X: 1;	// 10 - true if the X optical flow observation has been rejected
-		bool reject_optflow_Y: 1;	// 11 - true if the Y optical flow observation has been rejected		
+		bool reject_optflow_Y: 1;	// 11 - true if the Y optical flow observation has been rejected
 	} flags;
 	uint16_t value;
 
@@ -510,21 +392,21 @@ union filter_control_status_u {
 };
 
 union ekf_solution_status {
-    struct {
-	uint16_t attitude           : 1; // 0 - True if the attitude estimate is good
-	uint16_t velocity_horiz     : 1; // 1 - True if the horizontal velocity estimate is good
-	uint16_t velocity_vert      : 1; // 2 - True if the vertical velocity estimate is good
-	uint16_t pos_horiz_rel      : 1; // 3 - True if the horizontal position (relative) estimate is good
-	uint16_t pos_horiz_abs      : 1; // 4 - True if the horizontal position (absolute) estimate is good
-	uint16_t pos_vert_abs       : 1; // 5 - True if the vertical position (absolute) estimate is good
-	uint16_t pos_vert_agl       : 1; // 6 - True if the vertical position (above ground) estimate is good
-	uint16_t const_pos_mode     : 1; // 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
-	uint16_t pred_pos_horiz_rel : 1; // 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
-	uint16_t pred_pos_horiz_abs : 1; // 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
-	uint16_t gps_glitch         : 1; // 10 - True if the EKF has detected a GPS glitch
-	uint16_t accel_error        : 1; // 11 - True if the EKF has detected bad accelerometer data
-    } flags;
-    uint16_t value;
+	struct {
+		uint16_t attitude           : 1; // 0 - True if the attitude estimate is good
+		uint16_t velocity_horiz     : 1; // 1 - True if the horizontal velocity estimate is good
+		uint16_t velocity_vert      : 1; // 2 - True if the vertical velocity estimate is good
+		uint16_t pos_horiz_rel      : 1; // 3 - True if the horizontal position (relative) estimate is good
+		uint16_t pos_horiz_abs      : 1; // 4 - True if the horizontal position (absolute) estimate is good
+		uint16_t pos_vert_abs       : 1; // 5 - True if the vertical position (absolute) estimate is good
+		uint16_t pos_vert_agl       : 1; // 6 - True if the vertical position (above ground) estimate is good
+		uint16_t const_pos_mode     : 1; // 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
+		uint16_t pred_pos_horiz_rel : 1; // 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
+		uint16_t pred_pos_horiz_abs : 1; // 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
+		uint16_t gps_glitch         : 1; // 10 - True if the EKF has detected a GPS glitch
+		uint16_t accel_error        : 1; // 11 - True if the EKF has detected bad accelerometer data
+	} flags;
+	uint16_t value;
 };
 
 }

EKF/ekf.cpp

@@ -57,103 +57,6 @@
 #define ISFINITE(x) __builtin_isfinite(x)
 #endif
 
-
-const float Ekf::_k_earth_rate = 0.000072921f;
-const float Ekf::_gravity_mss = 9.80665f;
-
-Ekf::Ekf():
-	_dt_update(0.01f),
-	_filter_initialised(false),
-	_earth_rate_initialised(false),
-	_fuse_height(false),
-	_fuse_pos(false),
-	_fuse_hor_vel(false),
-	_fuse_vert_vel(false),
-	_gps_data_ready(false),
-	_mag_data_ready(false),
-	_baro_data_ready(false),
-	_range_data_ready(false),
-	_flow_data_ready(false),
-	_ev_data_ready(false),
-	_tas_data_ready(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),
-	_time_last_arsp_fuse(0),
-	_time_last_beta_fuse(0),
-	_last_disarmed_posD(0.0f),
-	_imu_collection_time_adj(0.0f),
-	_time_acc_bias_check(0.0f),
-	_airspeed_innov(0.0f),
-	_airspeed_innov_var(0.0f),
-	_beta_innov(0.0f),
- 	_beta_innov_var(0.0f),
-	_heading_innov(0.0f),
-	_heading_innov_var(0.0f),
-	_delta_time_of(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),
-	_hgt_counter(0),
-	_rng_filt_state(0.0f),
-	_mag_counter(0),
-	_ev_counter(0),
-	_time_last_mag(0),
-	_hgt_sensor_offset(0.0f),
-	_baro_hgt_offset(0.0f),
-	_last_on_ground_posD(0.0f),
-	_terrain_vpos(0.0f),
-	_terrain_var(1.e4f),
-	_hagl_innov(0.0f),
-	_hagl_innov_var(0.0f),
-	_time_last_hagl_fuse(0),
-	_terrain_initialised(false),
-	_range_data_continuous(false),
-	_dt_last_range_update_filt_us(0.0f),
-	_baro_hgt_faulty(false),
-	_gps_hgt_faulty(false),
-	_rng_hgt_faulty(false),
-	_primary_hgt_source(VDIST_SENSOR_BARO),
-	_time_bad_vert_accel(0),
-	_time_good_vert_accel(0),
-	_bad_vert_accel_detected(false)
-{
-	_state = {};
-	_last_known_posNE.setZero();
-	_earth_rate_NED.setZero();
-	_R_to_earth = matrix::Dcm<float>();
-	memset(_vel_pos_innov, 0, sizeof(_vel_pos_innov));
-	memset(_mag_innov, 0, sizeof(_mag_innov));
-	memset(_flow_innov, 0, sizeof(_flow_innov));
-	memset(_vel_pos_innov_var, 0, sizeof(_vel_pos_innov_var));
-	memset(_mag_innov_var, 0, sizeof(_mag_innov_var));
-	memset(_flow_innov_var, 0, sizeof(_flow_innov_var));
-	_delta_angle_corr.setZero();
-	_last_known_posNE.setZero();
-	_imu_down_sampled = {};
-	_q_down_sampled.setZero();
-	_vel_err_integ.setZero();
-	_pos_err_integ.setZero();
-	_mag_filt_state = {};
-	_delVel_sum = {};
-	_flow_gyro_bias = {};
-	_imu_del_ang_of = {};
-	_gps_check_fail_status.value = 0;
-	_state_reset_status = {};
-	_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS);
-	memset(_drag_innov, 0, sizeof(_drag_innov));
-	memset(_drag_innov_var, 0, sizeof(_drag_innov_var));
-}
-
 bool Ekf::init(uint64_t timestamp)
 {
 	bool ret = initialise_interface(timestamp);
@@ -169,7 +72,7 @@ bool Ekf::init(uint64_t timestamp)
 
 	_output_new.vel.setZero();
 	_output_new.pos.setZero();
-	_output_new.quat_nominal = matrix::Quaternion<float>();
+	_output_new.quat_nominal.setZero();
 
 	_delta_angle_corr.setZero();
 	_imu_down_sampled.delta_ang.setZero();
@@ -209,7 +112,6 @@ bool Ekf::init(uint64_t timestamp)
 
 bool Ekf::update()
 {
-
 	if (!_filter_initialised) {
 		_filter_initialised = initialiseFilter();
 
@@ -258,13 +160,16 @@ bool Ekf::initialiseFilter()
 		if ((_mag_counter == 0) && (_mag_sample_delayed.time_us != 0)) {
 			// initialise the counter when we start getting data from the buffer
 			_mag_counter = 1;
+
 		} else if ((_mag_counter != 0) && (_mag_sample_delayed.time_us != 0)) {
 			// increment the sample count and apply a LPF to the measurement
 			_mag_counter ++;
+
 			// don't start using data until we can be certain all bad initial data has been flushed
 			if (_mag_counter == (uint8_t)(_obs_buffer_length + 1)) {
 				// initialise filter states
 				_mag_filt_state = _mag_sample_delayed.mag;
+
 			} else if (_mag_counter > (uint8_t)(_obs_buffer_length + 1)) {
 				// noise filter the data
 				_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
@@ -277,6 +182,7 @@ bool Ekf::initialiseFilter()
 		if ((_ev_counter == 0) && (_ev_sample_delayed.time_us != 0)) {
 			// initialise the counter
 			_ev_counter = 1;
+
 			// set the height fusion mode to use external vision data when we start getting valid data from the buffer
 			if (_primary_hgt_source == VDIST_SENSOR_EV) {
 				_control_status.flags.baro_hgt = false;
@@ -284,6 +190,7 @@ bool Ekf::initialiseFilter()
 				_control_status.flags.rng_hgt = false;
 				_control_status.flags.ev_hgt = true;
 			}
+
 		} else if ((_ev_counter != 0) && (_ev_sample_delayed.time_us != 0)) {
 			// increment the sample count
 			_ev_counter ++;
@@ -305,13 +212,16 @@ bool Ekf::initialiseFilter()
 				_control_status.flags.rng_hgt = true;
 				_control_status.flags.ev_hgt = false;
 				_hgt_counter = 1;
+
 			} else if ((_hgt_counter != 0) && (_range_sample_delayed.time_us != 0)) {
 				// increment the sample count and apply a LPF to the measurement
 				_hgt_counter ++;
+
 				// don't start using data until we can be certain all bad initial data has been flushed
 				if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
 					// initialise filter states
 					_rng_filt_state = _range_sample_delayed.rng;
+
 				} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
 					// noise filter the data
 					_rng_filt_state = 0.9f * _rng_filt_state + 0.1f * _range_sample_delayed.rng;
@@ -329,13 +239,16 @@ bool Ekf::initialiseFilter()
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = false;
 				_hgt_counter = 1;
+
 			} else if ((_hgt_counter != 0) && (_baro_sample_delayed.time_us != 0)) {
 				// increment the sample count and apply a LPF to the measurement
 				_hgt_counter ++;
+
 				// don't start using data until we can be certain all bad initial data has been flushed
 				if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
 					// initialise filter states
 					_baro_hgt_offset = _baro_sample_delayed.hgt;
+
 				} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
 					// noise filter the data
 					_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
@@ -345,14 +258,16 @@ bool Ekf::initialiseFilter()
 
 	} else if (_primary_hgt_source == VDIST_SENSOR_EV) {
 		_hgt_counter = _ev_counter;
+
 	} else {
 		return false;
 	}
 
 	// check to see if we have enough measurements and return false if not
-	bool hgt_count_fail = _hgt_counter <= 2*_obs_buffer_length;
-	bool mag_count_fail = _mag_counter <= 2*_obs_buffer_length;
-	bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2*_obs_buffer_length);
+	bool hgt_count_fail = _hgt_counter <= 2 * _obs_buffer_length;
+	bool mag_count_fail = _mag_counter <= 2 * _obs_buffer_length;
+	bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2 * _obs_buffer_length);
+
 	if (hgt_count_fail || mag_count_fail || ev_count_fail) {
 		return false;
 
@@ -402,8 +317,9 @@ bool Ekf::initialiseFilter()
 			// so it can be used as a backup ad set the initial height using the range finder
 			baroSample baro_newest = _baro_buffer.get_newest();
 			_baro_hgt_offset = baro_newest.hgt;
-			_state.pos(2) = -math::max(_rng_filt_state * _R_rng_to_earth_2_2,_params.rng_gnd_clearance);
+			_state.pos(2) = -math::max(_rng_filt_state * _R_rng_to_earth_2_2, _params.rng_gnd_clearance);
 			ECL_INFO("EKF using range finder height - commencing alignment");
+
 		} else if (_control_status.flags.ev_hgt) {
 			// if we are using external vision data for height, then the vertical position state needs to be reset
 			// because the initialisation position is not the zero datum
@@ -475,10 +391,10 @@ void Ekf::predictState()
 	constrainStates();
 
 	// calculate an average filter update time
-	float input = 0.5f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
+	float input = 0.5f * (_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
 
 	// filter and limit input between -50% and +100% of nominal value
-	input = math::constrain(input,0.0005f * (float)(FILTER_UPDATE_PERIOD_MS),0.002f * (float)(FILTER_UPDATE_PERIOD_MS));
+	input = math::constrain(input, 0.0005f * (float)(FILTER_UPDATE_PERIOD_MS), 0.002f * (float)(FILTER_UPDATE_PERIOD_MS));
 	_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
 }
 
@@ -515,6 +431,7 @@ bool Ekf::collect_imu(imuSample &imu)
 	// if the target time delta between filter prediction steps has been exceeded
 	// write the accumulated IMU data to the ring buffer
 	float target_dt = (float)(FILTER_UPDATE_PERIOD_MS) / 1000;
+
 	if (_imu_down_sampled.delta_ang_dt >= target_dt - _imu_collection_time_adj) {
 
 		// accumulate the amount of time to advance the IMU collection time so that we meet the
@@ -533,6 +450,7 @@ bool Ekf::collect_imu(imuSample &imu)
 		_imu_down_sampled.delta_vel_dt = 0.0f;
 		_q_down_sampled(0) = 1.0f;
 		_q_down_sampled(1) = _q_down_sampled(2) = _q_down_sampled(3) = 0.0f;
+
 		return true;
 	}
 
@@ -556,13 +474,13 @@ void Ekf::calculateOutputStates()
 
 	// correct delta angles for bias offsets and scale factors
 	Vector3f delta_angle;
-	float dt_scale_correction = _dt_imu_avg/_dt_ekf_avg;
-	delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0)*dt_scale_correction;
-	delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1)*dt_scale_correction;
-	delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2)*dt_scale_correction;
+	float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg;
+	delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0) * dt_scale_correction;
+	delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1) * dt_scale_correction;
+	delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2) * dt_scale_correction;
 
 	// correct delta velocity for bias offsets
-	Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias*dt_scale_correction;
+	Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias * dt_scale_correction;
 
 	// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
 	delta_angle += _delta_angle_corr;
@@ -613,12 +531,14 @@ void Ekf::calculateOutputStates()
 		// convert the quaternion delta to a delta angle
 		Vector3f delta_ang_error;
 		float scalar;
+
 		if (q_error(0) >= 0.0f) {
 			scalar = -2.0f;
 
 		} else {
 			scalar = 2.0f;
 		}
+
 		delta_ang_error(0) = scalar * q_error(1);
 		delta_ang_error(1) = scalar * q_error(2);
 		delta_ang_error(2) = scalar * q_error(3);
@@ -656,9 +576,10 @@ void Ekf::calculateOutputStates()
 		// this method is too expensive to use for the attitude states due to the quaternion operations required
 		// but does not introduce a time delay in the 'correction loop' and allows smaller tracking time constants
 		// to be used
-		outputSample output_states;
+		outputSample output_states = {};
 		unsigned max_index = _output_buffer.get_length() - 1;
-		for (unsigned index=0; index <= max_index; index++) {
+
+		for (unsigned index = 0; index <= max_index; index++) {
 			output_states = _output_buffer.get_from_index(index);
 
 			// a constant  velocity correction is applied
@@ -668,12 +589,10 @@ void Ekf::calculateOutputStates()
 			output_states.pos += pos_correction;
 
 			// push the updated data to the buffer
-			_output_buffer.push_to_index(index,output_states);
-
+			_output_buffer.push_to_index(index, output_states);
 		}
 
 		// update output state to corrected values
 		_output_new = _output_buffer.get_newest();
-
 	}
 }

EKF/ekf.h

@@ -47,7 +47,7 @@ class Ekf : public EstimatorInterface
 {
 public:
 
-	Ekf();
+	Ekf() = default;
 	~Ekf() = default;
 
 	// initialise variables to sane values (also interface class)
@@ -80,10 +80,10 @@ public:
 	void get_airspeed_innov_var(float *airspeed_innov_var);
 
 	// gets the innovations of synthetic sideslip measurement
- 	void get_beta_innov(float *beta_innov);
+	void get_beta_innov(float *beta_innov);
 
- 	// gets the innovation variance of the synthetic sideslip measurement
- 	void get_beta_innov_var(float *beta_innov_var);
+	// gets the innovation variance of the synthetic sideslip measurement
+	void get_beta_innov_var(float *beta_innov_var);
 
 	// gets the innovation variance of the heading measurement
 	void get_heading_innov_var(float *heading_innov_var);
@@ -165,7 +165,7 @@ public:
 	void get_gyro_bias(float bias[3]);
 
 	// get GPS check status
-	void get_gps_check_status(uint16_t *_gps_check_fail_status);
+	void get_gps_check_status(uint16_t *val);
 
 	// return the amount the local vertical position changed in the last reset and the number of reset events
 	void get_posD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter;}
@@ -174,13 +174,15 @@ public:
 	void get_velD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.velD_change; *counter = _state_reset_status.velD_counter;}
 
 	// return the amount the local horizontal position changed in the last reset and the number of reset events
-	void get_posNE_reset(float delta[2], uint8_t *counter){
+	void get_posNE_reset(float delta[2], uint8_t *counter)
+	{
 		memcpy(delta, &_state_reset_status.posNE_change._data[0], sizeof(_state_reset_status.posNE_change._data));
 		*counter = _state_reset_status.posNE_counter;
 	}
 
 	// return the amount the local horizontal velocity changed in the last reset and the number of reset events
-	void get_velNE_reset(float delta[2], uint8_t *counter) {
+	void get_velNE_reset(float delta[2], uint8_t *counter)
+	{
 		memcpy(delta, &_state_reset_status.velNE_change._data[0], sizeof(_state_reset_status.velNE_change._data));
 		*counter = _state_reset_status.velNE_counter;
 	}
@@ -204,9 +206,9 @@ public:
 
 private:
 
-	static const uint8_t _k_num_states = 24;
-	static const float _k_earth_rate;
-	static const float _gravity_mss;
+	static constexpr uint8_t _k_num_states{24};
+	static constexpr float _k_earth_rate{0.000072921f};
+	static constexpr float _gravity_mss{9.80665f};
 
 	// reset event monitoring
 	// structure containing velocity, position, height and yaw reset information
@@ -221,145 +223,145 @@ private:
 		Vector2f posNE_change;	// North, East position change due to last reset (m)
 		float posD_change;	// Down position change due to last reset (m)
 		Quaternion quat_change;	// quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
-	} _state_reset_status;
+	} _state_reset_status{};
 
-	float _dt_ekf_avg;		// average update rate of the ekf
-	float _dt_update;		// delta time since last ekf update. This time can be used for filters
-					// which run at the same rate as the Ekf::update() function
+	float _dt_ekf_avg{0.001f * FILTER_UPDATE_PERIOD_MS};		// average update rate of the ekf
+	float _dt_update{0.01f};		// delta time since last ekf update. This time can be used for filters
+						// which run at the same rate as the Ekf::update() function
 
-	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;	// true when the EKF sttes and covariances been initialised
-	bool _earth_rate_initialised;	// true when we know the earth rotatin rate (requires GPS)
+	bool _filter_initialised{false};	// true when the EKF sttes and covariances been initialised
+	bool _earth_rate_initialised{false};	// true when we know the earth rotatin rate (requires GPS)
 
-	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{false};		// baro height data should be fused
+	bool _fuse_pos{false};			// gps position data should be fused
+	bool _fuse_hor_vel{false};		// gps horizontal velocity measurement should be fused
+	bool _fuse_vert_vel{false};		// gps vertical velocity measurement should be fused
 
 	// booleans true when fresh sensor data is available at the fusion time horizon
-	bool _gps_data_ready;
-	bool _mag_data_ready;
-	bool _baro_data_ready;
-	bool _range_data_ready;
-	bool _flow_data_ready;
-	bool _ev_data_ready;
-	bool _tas_data_ready;
+	bool _gps_data_ready{false};
+	bool _mag_data_ready{false};
+	bool _baro_data_ready{false};
+	bool _range_data_ready{false};
+	bool _flow_data_ready{false};
+	bool _ev_data_ready{false};
+	bool _tas_data_ready{false};
 
-	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{0};	// 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)
-	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)
-	uint64_t _time_last_arsp_fuse;	// time the last fusion of airspeed measurements were performed (usec)
-	uint64_t _time_last_beta_fuse;	// time the last fusion of synthetic sideslip measurements were performed (usec)
+	uint64_t _time_last_pos_fuse{0};   // time the last fusion of horizontal position measurements was performed (usec)
+	uint64_t _time_last_vel_fuse{0};   // time the last fusion of velocity measurements was performed (usec)
+	uint64_t _time_last_hgt_fuse{0};   // time the last fusion of height measurements was performed (usec)
+	uint64_t _time_last_of_fuse{0};    // time the last fusion of optical flow measurements were performed (usec)
+	uint64_t _time_last_arsp_fuse{0};	// time the last fusion of airspeed measurements were performed (usec)
+	uint64_t _time_last_beta_fuse{0};	// time the last fusion of synthetic sideslip 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)
-	float _imu_collection_time_adj;	// the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
+	float _last_disarmed_posD{0.0f};      // vertical position recorded at arming (m)
+	float _imu_collection_time_adj{0.0f};	// the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
 
-	uint64_t _time_acc_bias_check;	// last time the  accel bias check passed (usec)
+	uint64_t _time_acc_bias_check{0};	// last time the  accel bias check passed (usec)
 
 	Vector3f _earth_rate_NED;	// earth rotation vector (NED) in rad/s
 
 	matrix::Dcm<float> _R_to_earth;	// transformation matrix from body frame to earth frame from last EKF predition
 
-	float P[_k_num_states][_k_num_states]{};	// state covariance matrix
+	float P[_k_num_states][_k_num_states] {};	// state covariance matrix
 
-	float _vel_pos_innov[6]{};	// innovations: 0-2 vel,  3-5 pos
-	float _vel_pos_innov_var[6]{};	// innovation variances: 0-2 vel, 3-5 pos
+	float _vel_pos_innov[6] {};	// innovations: 0-2 vel,  3-5 pos
+	float _vel_pos_innov_var[6] {};	// innovation variances: 0-2 vel, 3-5 pos
 
-	float _mag_innov[3]{};		// earth magnetic field innovations
-	float _mag_innov_var[3]{};	// earth magnetic field innovation variance
+	float _mag_innov[3] {};		// earth magnetic field innovations
+	float _mag_innov_var[3] {};	// earth magnetic field innovation variance
 
-	float _airspeed_innov;		// airspeed measurement innovation
-	float _airspeed_innov_var;	// airspeed measurement innovation variance
+	float _airspeed_innov{0.0f};		// airspeed measurement innovation
+	float _airspeed_innov_var{0.0f};	// airspeed measurement innovation variance
 
-	float _beta_innov;		// synthetic sideslip measurement innovation
- 	float _beta_innov_var;	// synthetic sideslip measurement innovation variance
+	float _beta_innov{0.0f};		// synthetic sideslip measurement innovation
+	float _beta_innov_var{0.0f};	// synthetic sideslip measurement innovation variance
 
-	float _drag_innov[2];		// multirotor drag measurement innovation
-	float _drag_innov_var[2];	// multirotor drag measurement innovation variance
+	float _drag_innov[2] {};		// multirotor drag measurement innovation
+	float _drag_innov_var[2] {};	// multirotor drag measurement innovation variance
 
-	float _heading_innov;		// heading measurement innovation
-	float _heading_innov_var;	// heading measurement innovation variance
+	float _heading_innov{0.0f};		// heading measurement innovation
+	float _heading_innov_var{0.0f};	// heading measurement innovation variance
 
 	// optical flow processing
-	float _flow_innov[2]{};		// flow measurement innovation
-	float _flow_innov_var[2]{};	// flow innovation variance
+	float _flow_innov[2] {};		// flow measurement innovation
+	float _flow_innov_var[2] {};	// flow innovation variance
 	Vector3f _flow_gyro_bias;	// bias errors in optical flow sensor rate gyro outputs
 	Vector3f _imu_del_ang_of;	// bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates
-	float _delta_time_of;		// time in sec that _imu_del_ang_of was accumulated over
+	float _delta_time_of{0.0f};		// time in sec that _imu_del_ang_of was accumulated over
 
-	float _mag_declination;		// magnetic declination used by reset and fusion functions (rad)
+	float _mag_declination{0.0f};		// magnetic declination used by reset and fusion functions (rad)
 
 	// output predictor states
 	Vector3f _delta_angle_corr;	// delta angle correction vector
-	imuSample _imu_down_sampled;	// down sampled imu data (sensor rate -> filter update rate)
+	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 _vel_err_integ;	// integral of velocity tracking error
 	Vector3f _pos_err_integ;	// integral of position tracking error
-	float _output_tracking_error[3]{};// contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
+	float _output_tracking_error[3] {}; // contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
 
 	// variables used for the GPS quality 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
+	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
 
 	// Variables used to publish the WGS-84 location of the EKF local NED origin
-	uint64_t _last_gps_origin_time_us;  // time the origin was last set (uSec)
-	float _gps_alt_ref;		// WGS-84 height (m)
+	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)
 
 	// Variables used to initialise the filter states
-	uint32_t _hgt_counter;		// number of height samples read during initialisation
-	float _rng_filt_state;		// filtered height measurement
-	uint32_t _mag_counter;		// number of magnetometer samples read during initialisation
-	uint32_t _ev_counter;		// number of exgernal vision samples read during initialisation
-	uint64_t _time_last_mag;	// measurement time of last magnetomter sample
+	uint32_t _hgt_counter{0};		// number of height samples read during initialisation
+	float _rng_filt_state{0.0f};		// filtered height measurement
+	uint32_t _mag_counter{0};		// number of magnetometer samples read during initialisation
+	uint32_t _ev_counter{0};		// number of exgernal vision samples read during initialisation
+	uint64_t _time_last_mag{0};	// measurement time of last magnetomter sample
 	Vector3f _mag_filt_state;	// filtered magnetometer measurement
 	Vector3f _delVel_sum;		// summed delta velocity
-	float _hgt_sensor_offset;	// set as necessary if desired to maintain the same height after a height reset (m)
-	float _baro_hgt_offset;		// baro height reading at the local NED origin (m)
+	float _hgt_sensor_offset{0.0f};	// set as necessary if desired to maintain the same height after a height reset (m)
+	float _baro_hgt_offset{0.0f};		// baro height reading at the local NED origin (m)
 
 	// Variables used to control activation of post takeoff functionality
-	float _last_on_ground_posD; // last vertical position when the in_air status was false (m)
+	float _last_on_ground_posD{0.0f}; // last vertical position when the in_air status was false (m)
 
 	gps_check_fail_status_u _gps_check_fail_status{};
 
 	// variables used to inhibit accel bias learning
-	bool _accel_bias_inhibit;	// true when the accel bias learning is being inhibited
-	float _accel_mag_filt;		// acceleration magnitude after application of a decaying envelope filter (m/sec**2)
-	float _ang_rate_mag_filt;	// angular rate magnitude after application of a decaying envelope filter (rad/sec)
+	bool _accel_bias_inhibit{false};	// true when the accel bias learning is being inhibited
+	float _accel_mag_filt{0.0f};		// acceleration magnitude after application of a decaying envelope filter (m/sec**2)
+	float _ang_rate_mag_filt{0.0f};	// angular rate magnitude after application of a decaying envelope filter (rad/sec)
 	Vector3f _prev_dvel_bias_var;	// saved delta velocity XYZ bias variances (m/sec)**2
 
 	// Terrain height state estimation
-	float _terrain_vpos;		// estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
-	float _terrain_var;		// variance of terrain position estimate (m^2)
-	float _hagl_innov;		// innovation of the last height above terrain measurement (m)
-	float _hagl_innov_var;		// innovation variance for the last height above terrain measurement (m^2)
+	float _terrain_vpos{0.0f};		// estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
+	float _terrain_var{1e4f};		// variance of terrain position estimate (m^2)
+	float _hagl_innov{0.0f};		// innovation of the last height above terrain measurement (m)
+	float _hagl_innov_var{0.0f};		// innovation variance for the last height above terrain measurement (m^2)
 	uint64_t _time_last_hagl_fuse;	// last system time in usec that the hagl measurement failed it's checks
-	bool _terrain_initialised;	// true when the terrain estimator has been intialised
-	float _sin_tilt_rng;		// sine of the range finder tilt rotation about the Y body axis
-	float _cos_tilt_rng;		// cosine of the range finder tilt rotation about the Y body axis
-	float _R_rng_to_earth_2_2;	// 2,2 element of the rotation matrix from sensor frame to earth frame
-	bool _range_data_continuous;	// true when we are receiving range finder data faster than a 2Hz average
-	float _dt_last_range_update_filt_us;	// filtered value of the delta time elapsed since the last range measurement came into
-						// the filter (microseconds)
+	bool _terrain_initialised{false};	// true when the terrain estimator has been intialised
+	float _sin_tilt_rng{0.0f};		// sine of the range finder tilt rotation about the Y body axis
+	float _cos_tilt_rng{0.0f};		// cosine of the range finder tilt rotation about the Y body axis
+	float _R_rng_to_earth_2_2{0.0f};	// 2,2 element of the rotation matrix from sensor frame to earth frame
+	bool _range_data_continuous{false};	// true when we are receiving range finder data faster than a 2Hz average
+	float _dt_last_range_update_filt_us{0.0f};	// filtered value of the delta time elapsed since the last range measurement came into
+							// the filter (microseconds)
 
 	// height sensor fault status
-	bool _baro_hgt_faulty;		// true if valid baro data is unavailable for use
-	bool _gps_hgt_faulty;		// true if valid gps height data is unavailable for use
-	bool _rng_hgt_faulty;		// true if valid rnage finder height data is unavailable for use
-	int _primary_hgt_source;	// priary source of height data set at initialisation
+	bool _baro_hgt_faulty{false};		// true if valid baro data is unavailable for use
+	bool _gps_hgt_faulty{false};		// true if valid gps height data is unavailable for use
+	bool _rng_hgt_faulty{false};		// true if valid rnage finder height data is unavailable for use
+	int _primary_hgt_source{VDIST_SENSOR_BARO};	// priary source of height data set at initialisation
 
 	// imu fault status
-	uint64_t _time_bad_vert_accel;	// last time a bad vertical accel was detected (usec)
-	uint64_t _time_good_vert_accel;	// last time a good vertical accel was detected (usec)
-	bool _bad_vert_accel_detected;	// true when bad vertical accelerometer data has been detected
+	uint64_t _time_bad_vert_accel{0};	// last time a bad vertical accel was detected (usec)
+	uint64_t _time_good_vert_accel{0};	// last time a good vertical accel was detected (usec)
+	bool _bad_vert_accel_detected{false};	// true when bad vertical accelerometer data has been detected
 
 	// update the real time complementary filter states. This includes the prediction
 	// and the correction step
@@ -390,7 +392,7 @@ private:
 	void fuseAirspeed();
 
 	// fuse synthetic zero sideslip measurement
- 	void fuseSideslip();
+	void fuseSideslip();
 
 	// fuse body frame drag specific forces for multi-rotor wind estimation
 	void fuseDrag();

EKF/ekf_helper.cpp

@@ -60,17 +60,17 @@ bool Ekf::resetVelocity()
 
 	} else {
 		return false;
-
 	}
 
 	// calculate the change in velocity and apply to the output predictor state history
 	Vector3f velocity_change = _state.vel - vel_before_reset;
-	outputSample output_states;
+	outputSample output_states = {};
 	unsigned max_index = _output_buffer.get_length() - 1;
-	for (unsigned index=0; index <= max_index; index++) {
+
+	for (unsigned index = 0; index <= max_index; index++) {
 		output_states = _output_buffer.get_from_index(index);
 		output_states.vel += velocity_change;
-		_output_buffer.push_to_index(index,output_states);
+		_output_buffer.push_to_index(index, output_states);
 
 	}
 
@@ -119,13 +119,14 @@ bool Ekf::resetPosition()
 	Vector2f posNE_change;
 	posNE_change(0) = _state.pos(0) - posNE_before_reset(0);
 	posNE_change(1) = _state.pos(1) - posNE_before_reset(1);
-	outputSample output_states;
+	outputSample output_states = {};
 	unsigned max_index = _output_buffer.get_length() - 1;
-	for (unsigned index=0; index <= max_index; index++) {
+
+	for (unsigned index = 0; index <= max_index; index++) {
 		output_states = _output_buffer.get_from_index(index);
 		output_states.pos(0) += posNE_change(0);
 		output_states.pos(1) += posNE_change(1);
-		_output_buffer.push_to_index(index,output_states);
+		_output_buffer.push_to_index(index, output_states);
 	}
 
 	// apply the change in position to our newest position estimate
@@ -230,8 +231,10 @@ void Ekf::resetHeight()
 		// use the most recent data if it's time offset from the fusion time horizon is smaller
 		int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
 		int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
+
 		if (std::abs(dt_newest) < std::abs(dt_delayed)) {
 			_state.pos(2) = ev_newest.posNED(2);
+
 		} else {
 			_state.pos(2) = _ev_sample_delayed.posNED(2);
 		}
@@ -259,6 +262,7 @@ void Ekf::resetHeight()
 		P[6][6] = 10.0f;
 
 	}
+
 	vert_vel_reset = true;
 
 	// store the reset amount and time to be published
@@ -283,9 +287,10 @@ void Ekf::resetHeight()
 	}
 
 	// add the reset amount to the output observer buffered data
-	outputSample output_states;
+	outputSample output_states = {};
 	unsigned output_length = _output_buffer.get_length();
-	for (unsigned i=0; i < output_length; i++) {
+
+	for (unsigned i = 0; i < output_length; i++) {
 		output_states = _output_buffer.get_from_index(i);
 
 		if (vert_pos_reset) {
@@ -296,7 +301,7 @@ void Ekf::resetHeight()
 			output_states.vel(2) += _state_reset_status.velD_change;
 		}
 
-		_output_buffer.push_to_index(i,output_states);
+		_output_buffer.push_to_index(i, output_states);
 
 	}
 }
@@ -314,15 +319,16 @@ void Ekf::alignOutputFilter()
 	Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
 
 	// loop through the output filter state history and add the deltas
-	outputSample output_states;
+	outputSample output_states = {};
 	unsigned output_length = _output_buffer.get_length();
-	for (unsigned i=0; i < output_length; i++) {
+
+	for (unsigned i = 0; i < output_length; i++) {
 		output_states = _output_buffer.get_from_index(i);
 		output_states.quat_nominal *= q_delta;
 		output_states.quat_nominal.normalize();
 		output_states.vel += vel_delta;
 		output_states.pos += pos_delta;
-		_output_buffer.push_to_index(i,output_states);
+		_output_buffer.push_to_index(i, output_states);
 	}
 }
 
@@ -356,12 +362,14 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 			// convert the observed quaternion to a rotation matrix
 			matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
 			// calculate the yaw angle for a 312 sequence
-			euler321(2) = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0));
+			euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
+
 		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
 			// rotate the magnetometer measurements into earth frame using a zero yaw angle
 			Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
 			// the angle of the projection onto the horizontal gives the yaw angle
 			euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
+
 		} else {
 			// there is no yaw observation
 			return false;
@@ -377,9 +385,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 		// Calculate the 312 sequence euler angles that rotate from earth to body frame
 		// See http://www.atacolorado.com/eulersequences.doc
 		Vector3f euler312;
-		euler312(0) = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw)
+		euler312(0) = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1));  // first rotation (yaw)
 		euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
-		euler312(2) = atan2f(-_R_to_earth(2, 0) , _R_to_earth(2, 2)); // third rotation (pitch)
+		euler312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2));  // third rotation (pitch)
 
 		// Set the first rotation (yaw) to zero and calculate the rotation matrix from body to earth frame
 		euler312(0) = 0.0f;
@@ -408,12 +416,14 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 			// convert the observed quaternion to a rotation matrix
 			matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
 			// calculate the yaw angle for a 312 sequence
-			euler312(0) = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1));
+			euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
+
 		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
 			// rotate the magnetometer measurements into earth frame using a zero yaw angle
 			Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
 			// the angle of the projection onto the horizontal gives the yaw angle
 			euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
+
 		} else {
 			// there is no yaw observation
 			return false;
@@ -444,6 +454,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 	if (_params.fusion_mode & MASK_USE_EVYAW) {
 		// using error estimate from external vision data
 		angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
+
 	} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
 		// using magnetic heading tuning parameter
 		angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
@@ -467,12 +478,13 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 	_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
 
 	// add the reset amount to the output observer buffered data
-	outputSample output_states;
+	outputSample output_states = {};
 	unsigned output_length = _output_buffer.get_length();
-	for (unsigned i=0; i < output_length; i++) {
+
+	for (unsigned i = 0; i < output_length; i++) {
 		output_states = _output_buffer.get_from_index(i);
 		output_states.quat_nominal *= _state_reset_status.quat_change;
-		_output_buffer.push_to_index(i,output_states);
+		_output_buffer.push_to_index(i, output_states);
 	}
 
 	// apply the change in attitude quaternion to our newest quaternion estimate
@@ -578,13 +590,13 @@ void Ekf::get_mag_innov(float mag_innov[3])
 // gets the innovations of the airspeed measnurement
 void Ekf::get_airspeed_innov(float *airspeed_innov)
 {
-	memcpy(airspeed_innov,&_airspeed_innov, sizeof(float));
+	memcpy(airspeed_innov, &_airspeed_innov, sizeof(float));
 }
 
 // gets the innovations of the synthetic sideslip measurements
 void Ekf::get_beta_innov(float *beta_innov)
 {
-	memcpy(beta_innov,&_beta_innov, sizeof(float));
+	memcpy(beta_innov, &_beta_innov, sizeof(float));
 }
 
 // gets the innovations of the heading measurement
@@ -670,9 +682,9 @@ void Ekf::get_state_delayed(float *state)
 void Ekf::get_accel_bias(float bias[3])
 {
 	float temp[3];
-	temp[0] = _state.accel_bias(0) /_dt_ekf_avg;
-	temp[1] = _state.accel_bias(1) /_dt_ekf_avg;
-	temp[2] = _state.accel_bias(2) /_dt_ekf_avg;
+	temp[0] = _state.accel_bias(0) / _dt_ekf_avg;
+	temp[1] = _state.accel_bias(1) / _dt_ekf_avg;
+	temp[2] = _state.accel_bias(2) / _dt_ekf_avg;
 	memcpy(bias, temp, 3 * sizeof(float));
 }
 
@@ -680,9 +692,9 @@ void Ekf::get_accel_bias(float bias[3])
 void Ekf::get_gyro_bias(float bias[3])
 {
 	float temp[3];
-	temp[0] = _state.gyro_bias(0) /_dt_ekf_avg;
-	temp[1] = _state.gyro_bias(1) /_dt_ekf_avg;
-	temp[2] = _state.gyro_bias(2) /_dt_ekf_avg;
+	temp[0] = _state.gyro_bias(0) / _dt_ekf_avg;
+	temp[1] = _state.gyro_bias(1) / _dt_ekf_avg;
+	temp[2] = _state.gyro_bias(2) / _dt_ekf_avg;
 	memcpy(bias, temp, 3 * sizeof(float));
 }
 
@@ -731,6 +743,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
 	float hpos_err;
 	float vpos_err;
 	bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
+
 	if (vel_pos_aiding && _NED_origin_initialised) {
 		hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph));
 		vpos_err = sqrtf(P[9][9] + sq(_gps_origin_epv));
@@ -745,7 +758,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
 	// The reason is that complete rejection of measurements is often casued by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
 	if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
-		hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3]*_vel_pos_innov[3] + _vel_pos_innov[4]*_vel_pos_innov[4]));
+		hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3] * _vel_pos_innov[3] + _vel_pos_innov[4] * _vel_pos_innov[4]));
 
 	}
 
@@ -761,6 +774,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
 	float hpos_err;
 	float vpos_err;
 	bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
+
 	if (vel_pos_aiding && _NED_origin_initialised) {
 		hpos_err = sqrtf(P[7][7] + P[8][8]);
 		vpos_err = sqrtf(P[9][9]);
@@ -775,7 +789,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
 	// The reason is that complete rejection of measurements is often casued by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
 	if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
-		hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3]*_vel_pos_innov[3] + _vel_pos_innov[4]*_vel_pos_innov[4]));
+		hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3] * _vel_pos_innov[3] + _vel_pos_innov[4] * _vel_pos_innov[4]));
 
 	}
 
@@ -790,6 +804,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon
 	float hvel_err;
 	float vvel_err;
 	bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
+
 	if (vel_pos_aiding && _NED_origin_initialised) {
 		hvel_err = sqrtf(P[4][4] + P[5][5]);
 		vvel_err = sqrtf(P[6][6]);
@@ -804,14 +819,19 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
 	float vel_err_conservative = 0.0f;
+
 	if (_is_dead_reckoning) {
 		if (_control_status.flags.opt_flow) {
 			float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
-			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(_flow_innov[0]*_flow_innov[0] + _flow_innov[1]*_flow_innov[1]);
+			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)),
+							 gndclearance) * sqrtf(_flow_innov[0] * _flow_innov[0] + _flow_innov[1] * _flow_innov[1]);
 		}
+
 		if (_control_status.flags.gps || _control_status.flags.ev_pos) {
-			vel_err_conservative = math::max(vel_err_conservative, sqrtf(_vel_pos_innov[0]*_vel_pos_innov[0] + _vel_pos_innov[1]*_vel_pos_innov[1]));
+			vel_err_conservative = math::max(vel_err_conservative,
+							 sqrtf(_vel_pos_innov[0] * _vel_pos_innov[0] + _vel_pos_innov[1] * _vel_pos_innov[1]));
 		}
+
 		hvel_err = math::max(hvel_err, vel_err_conservative);
 	}
 
@@ -830,11 +850,11 @@ void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, f
 	// return the integer bitmask containing the consistency check pass/fail satus
 	*status = _innov_check_fail_status.value;
 	// return the largest magnetometer innovation test ratio
-	*mag = sqrtf(math::max(_yaw_test_ratio,math::max(math::max(_mag_test_ratio[0],_mag_test_ratio[1]),_mag_test_ratio[2])));
+	*mag = sqrtf(math::max(_yaw_test_ratio, math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2])));
 	// return the largest NED velocity innovation test ratio
-	*vel = sqrtf(math::max(math::max(_vel_pos_test_ratio[0],_vel_pos_test_ratio[1]),_vel_pos_test_ratio[2]));
+	*vel = sqrtf(math::max(math::max(_vel_pos_test_ratio[0], _vel_pos_test_ratio[1]), _vel_pos_test_ratio[2]));
 	// return the largest NE position innovation test ratio
-	*pos = sqrtf(math::max(_vel_pos_test_ratio[3],_vel_pos_test_ratio[4]));
+	*pos = sqrtf(math::max(_vel_pos_test_ratio[3], _vel_pos_test_ratio[4]));
 	// return the vertical position innovation test ratio
 	*hgt = sqrtf(_vel_pos_test_ratio[5]);
 	// return the airspeed fusion innovation test ratio
@@ -872,6 +892,7 @@ void Ekf::fuse(float *K, float innovation)
 	for (unsigned i = 0; i < 4; i++) {
 		_state.quat_nominal(i) = _state.quat_nominal(i) - K[i] * innovation;
 	}
+
 	_state.quat_nominal.normalize();
 
 	for (unsigned i = 0; i < 3; i++) {
@@ -933,7 +954,8 @@ bool Ekf::global_position_is_valid()
 // return true if we are totally reliant on inertial dead-reckoning for position
 bool Ekf::inertial_dead_reckoning()
 {
-	bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos) && ((_time_last_imu - _time_last_pos_fuse <= 1E6) || (_time_last_imu - _time_last_vel_fuse <= 1E6));
+	bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos)
+			    && ((_time_last_imu - _time_last_pos_fuse <= 1E6) || (_time_last_imu - _time_last_vel_fuse <= 1E6));
 	bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= 1E6);
 	bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6);
 
@@ -944,14 +966,14 @@ bool Ekf::inertial_dead_reckoning()
 Vector3f EstimatorInterface::cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2)
 {
 	Vector3f vecOut;
-	vecOut(0) = vecIn1(1)*vecIn2(2) - vecIn1(2)*vecIn2(1);
-	vecOut(1) = vecIn1(2)*vecIn2(0) - vecIn1(0)*vecIn2(2);
-	vecOut(2) = vecIn1(0)*vecIn2(1) - vecIn1(1)*vecIn2(0);
+	vecOut(0) = vecIn1(1) * vecIn2(2) - vecIn1(2) * vecIn2(1);
+	vecOut(1) = vecIn1(2) * vecIn2(0) - vecIn1(0) * vecIn2(2);
+	vecOut(2) = vecIn1(0) * vecIn2(1) - vecIn1(1) * vecIn2(0);
 	return vecOut;
 }
 
 // calculate the inverse rotation matrix from a quaternion rotation
-Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion& quat)
+Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion &quat)
 {
 	float q00 = quat(0) * quat(0);
 	float q11 = quat(1) * quat(1);
@@ -965,15 +987,15 @@ Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion& quat)
 	float q23 = quat(2) * quat(3);
 
 	Matrix3f dcm;
-	dcm(0,0) = q00 + q11 - q22 - q33;
-	dcm(1,1) = q00 - q11 + q22 - q33;
-	dcm(2,2) = q00 - q11 - q22 + q33;
-	dcm(0,1) = 2.0f * (q12 - q03);
-	dcm(0,2) = 2.0f * (q13 + q02);
-	dcm(1,0) = 2.0f * (q12 + q03);
-	dcm(1,2) = 2.0f * (q23 - q01);
-	dcm(2,0) = 2.0f * (q13 - q02);
-	dcm(2,1) = 2.0f * (q23 + q01);
+	dcm(0, 0) = q00 + q11 - q22 - q33;
+	dcm(1, 1) = q00 - q11 + q22 - q33;
+	dcm(2, 2) = q00 - q11 - q22 + q33;
+	dcm(0, 1) = 2.0f * (q12 - q03);
+	dcm(0, 2) = 2.0f * (q13 + q02);
+	dcm(1, 0) = 2.0f * (q12 + q03);
+	dcm(1, 2) = 2.0f * (q23 - q01);
+	dcm(2, 0) = 2.0f * (q13 - q02);
+	dcm(2, 1) = 2.0f * (q23 + q01);
 
 	return dcm;
 }
@@ -982,12 +1004,14 @@ Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion& quat)
 Vector3f Ekf::calcRotVecVariances()
 {
 	Vector3f rot_var_vec;
-	float q0,q1,q2,q3;
+	float q0, q1, q2, q3;
+
 	if (_state.quat_nominal(0) >= 0.0f) {
 		q0 = _state.quat_nominal(0);
 		q1 = _state.quat_nominal(1);
 		q2 = _state.quat_nominal(2);
 		q3 = _state.quat_nominal(3);
+
 	} else {
 		q0 = -_state.quat_nominal(0);
 		q1 = -_state.quat_nominal(1);
@@ -1033,6 +1057,7 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 		q1 = _state.quat_nominal(1);
 		q2 = _state.quat_nominal(2);
 		q3 = _state.quat_nominal(3);
+
 	} else {
 		q0 = -_state.quat_nominal(0);
 		q1 = -_state.quat_nominal(1);
@@ -1120,17 +1145,17 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 		P[0][2] = 0.0f;
 		P[0][3] = 0.0f;
 		P[1][0] = 0.0f;
-		P[1][1] = 0.25f*rot_vec_var(0);
+		P[1][1] = 0.25f * rot_vec_var(0);
 		P[1][2] = 0.0f;
 		P[1][3] = 0.0f;
 		P[2][0] = 0.0f;
 		P[2][1] = 0.0f;
-		P[2][2] = 0.25f*rot_vec_var(1);
+		P[2][2] = 0.25f * rot_vec_var(1);
 		P[2][3] = 0.0f;
 		P[3][0] = 0.0f;
 		P[3][1] = 0.0f;
 		P[3][2] = 0.0f;
-		P[3][3] = 0.25f*rot_vec_var(2);
+		P[3][3] = 0.25f * rot_vec_var(2);
 
 	}
 }

EKF/estimator_interface.cpp

@@ -40,67 +40,15 @@
  * @author Siddharth B Purohit <siddharthbharatpurohit@gmail.com>
  */
 
-#include <inttypes.h>
-#include <math.h>
-#include "../ecl.h"
 #include "estimator_interface.h"
+
+#include "../ecl.h"
+#include <math.h>
 #include "mathlib.h"
 
-
-EstimatorInterface::EstimatorInterface():
-	_obs_buffer_length(10),
-	_imu_buffer_length(30),
-	_min_obs_interval_us(0),
-	_dt_imu_avg(0.0f),
-	_mag_sample_delayed{},
-	_baro_sample_delayed{},
-	_gps_sample_delayed{},
-	_range_sample_delayed{},
-	_airspeed_sample_delayed{},
-	_flow_sample_delayed{},
-	_ev_sample_delayed{},
-	_drag_sample_delayed{},
-	_drag_down_sampled{},
-	_drag_sample_count(0),
-	_drag_sample_time_dt(0.0f),
-	_air_density(1.225f),
-	_imu_ticks(0),
-	_imu_updated(false),
-	_initialised(false),
-	_NED_origin_initialised(false),
-	_gps_speed_valid(false),
-	_gps_origin_eph(0.0f),
-	_gps_origin_epv(0.0f),
-	_pos_ref{},
-	_gps_pos_prev{},
-	_gps_alt_prev(0.0f),
-	_yaw_test_ratio(0.0f),
-	_mag_test_ratio{},
-	_vel_pos_test_ratio{},
-	_tas_test_ratio(0.0f),
-	_terr_test_ratio(0.0f),
-	_beta_test_ratio(0.0f),
-	_drag_test_ratio{},
-	_is_dead_reckoning(false),
-	_vibe_metrics{},
-	_time_last_imu(0),
-	_time_last_gps(0),
-	_time_last_mag(0),
-	_time_last_baro(0),
-	_time_last_range(0),
-	_time_last_airspeed(0),
-	_time_last_ext_vision(0),
-	_time_last_optflow(0),
-	_mag_declination_gps(0.0f),
-	_mag_declination_to_save_deg(0.0f)
-{
-	_delta_ang_prev.setZero();
-	_delta_vel_prev.setZero();
-}
-
 // Accumulate imu data and store to buffer at desired rate
-void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float (&delta_ang)[3],
-				    float (&delta_vel)[3])
+void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt,
+				    float (&delta_ang)[3], float (&delta_vel)[3])
 {
 	if (!_initialised) {
 		init(time_usec);
@@ -129,7 +77,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 	_imu_ticks++;
 
 	// calculate a metric which indicates the amount of coning vibration
-	Vector3f temp = cross_product(imu_sample_new.delta_ang , _delta_ang_prev);
+	Vector3f temp = cross_product(imu_sample_new.delta_ang, _delta_ang_prev);
 	_vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm();
 
 	// calculate a metric which indiates the amount of high frequency gyro vibration
@@ -160,6 +108,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 
 			// calculate the downsample ratio for drag specific force data
 			uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length);
+
 			if (min_sample_ratio < 5) {
 				min_sample_ratio = 5;
 
@@ -189,7 +138,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 
 		// calculate the minimum interval between observations required to guarantee no loss of data
 		// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
-		_min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us)/(_obs_buffer_length - 1);
+		_min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);
 
 	} else {
 		_imu_updated = false;
@@ -203,7 +152,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
 	if (time_usec - _time_last_mag > _min_obs_interval_us) {
 
 		magSample mag_sample_new = {};
-		mag_sample_new.time_us = time_usec  - _params.mag_delay_ms * 1000;
+		mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000;
 
 		mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
 		_time_last_mag = time_usec;
@@ -223,6 +172,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
 
 	// limit data rate to prevent data being lost
 	bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
+
 	if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps->fix_type > 2) {
 		gpsSample gps_sample_new = {};
 		gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
@@ -353,6 +303,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 			// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis.
 			// copy the optical and gyro measured delta angles
 			optflow_sample_new.gyroXYZ = - flow->gyrodata;
+
 			if (flow_quality_good) {
 				optflow_sample_new.flowRadXY = - flow->flowdata;
 
@@ -362,6 +313,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 				optflow_sample_new.flowRadXY(1) = + flow->gyrodata(1);
 
 			}
+
 			// compensate for body motion to give a LOS rate
 			optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0);
 			optflow_sample_new.flowRadXYcomp(1) = optflow_sample_new.flowRadXY(1) - optflow_sample_new.gyroXYZ(1);
@@ -418,7 +370,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	_obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1;
 
 	// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
-	_obs_buffer_length = math::min(_obs_buffer_length,_imu_buffer_length);
+	_obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length);
 
 	if (!(_imu_buffer.allocate(_imu_buffer_length) &&
 	      _gps_buffer.allocate(_obs_buffer_length) &&
@@ -436,7 +388,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	}
 
 	// zero the data in the observation buffers
-	for (int index=0; index < _obs_buffer_length; index++) {
+	for (int index = 0; index < _obs_buffer_length; index++) {
 		gpsSample gps_sample_init = {};
 		_gps_buffer.push(gps_sample_init);
 		magSample mag_sample_init = {};
@@ -456,7 +408,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	}
 
 	// zero the data in the imu data and output observer state buffers
-	for (int index=0; index < _imu_buffer_length; index++) {
+	for (int index = 0; index < _imu_buffer_length; index++) {
 		imuSample imu_sample_init = {};
 		_imu_buffer.push(imu_sample_init);
 		outputSample output_sample_init = {};
@@ -504,7 +456,7 @@ void EstimatorInterface::unallocate_buffers()
 bool EstimatorInterface::local_position_is_valid()
 {
 	// return true if the position estimate is valid
-	return (((_time_last_imu - _time_last_optflow) < 5e6) && _control_status.flags.opt_flow) || 
-		(((_time_last_imu - _time_last_ext_vision) < 5e6) && _control_status.flags.ev_pos) || 
-		global_position_is_valid();
+	return (((_time_last_imu - _time_last_optflow) < 5e6) && _control_status.flags.opt_flow) ||
+	       (((_time_last_imu - _time_last_ext_vision) < 5e6) && _control_status.flags.ev_pos) ||
+	       global_position_is_valid();
 }

EKF/estimator_interface.h

@@ -39,7 +39,6 @@
  *
  */
 
-#include <stdint.h>
 #include <matrix/matrix/math.hpp>
 #include "RingBuffer.h"
 #include "geo.h"
@@ -47,11 +46,12 @@
 #include "mathlib.h"
 
 using namespace estimator;
+
 class EstimatorInterface
 {
 
 public:
-	EstimatorInterface();
+	EstimatorInterface() = default;
 	~EstimatorInterface() = default;
 
 	virtual bool init(uint64_t timestamp) = 0;
@@ -65,7 +65,7 @@ public:
 	virtual void get_mag_innov(float mag_innov[3]) = 0;
 
 	// gets the innovation of airspeed measurement
- 	virtual void get_airspeed_innov(float *airspeed_innov) = 0;
+	virtual void get_airspeed_innov(float *airspeed_innov) = 0;
 
 	// gets the innovation of the synthetic sideslip measurement
 	virtual void get_beta_innov(float *beta_innov) = 0;
@@ -81,7 +81,7 @@ public:
 	virtual void get_mag_innov_var(float mag_innov_var[3]) = 0;
 
 	// gets the innovation variance of the airspeed measurement
- 	virtual void get_airspeed_innov_var(float *get_airspeed_innov_var) = 0;
+	virtual void get_airspeed_innov_var(float *get_airspeed_innov_var) = 0;
 
 	// gets the innovation variance of the synthetic sideslip measurement
 	virtual void get_beta_innov_var(float *get_beta_innov_var) = 0;
@@ -211,13 +211,14 @@ public:
 	void get_velocity(float *vel)
 	{
 		// calculate the average angular rate across the last IMU update
-		Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f/_imu_sample_new.delta_ang_dt);
+		Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f / _imu_sample_new.delta_ang_dt);
 		// calculate the velocity of the relative to the body origin
 		// Note % operator has been overloaded to performa cross product
-		Vector3f vel_imu_rel_body = cross_product(ang_rate , _params.imu_pos_body);
+		Vector3f vel_imu_rel_body = cross_product(ang_rate, _params.imu_pos_body);
 		// rotate the relative velocty into earth frame and subtract from the EKF velocity
 		// (which is at the IMU) to get velocity of the body origin
 		Vector3f vel_earth = _output_new.vel - _R_to_earth_now * vel_imu_rel_body;
+
 		// copy to output
 		for (unsigned i = 0; i < 3; i++) {
 			vel[i] = vel_earth(i);
@@ -228,6 +229,7 @@ public:
 	{
 		// rotate the position of the IMU relative to the boy origin into earth frame
 		Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
+
 		// subtract from the EKF position (which is at the IMU) to get position at the body origin
 		for (unsigned i = 0; i < 3; i++) {
 			pos[i] = _output_new.pos(i) - pos_offset_earth(i);
@@ -298,72 +300,73 @@ protected:
 	 max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS * 0.001)
 	 This can be adjusted to match the max sensor data rate plus some margin for jitter.
 	*/
-	uint8_t _obs_buffer_length;
+	uint8_t _obs_buffer_length{0};
+
 	/*
 	IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the
 	EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for.
 	max sensor time offet (msec) =  IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS
 	This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay.
 	*/
-	uint8_t _imu_buffer_length;
+	uint8_t _imu_buffer_length{0};
 	static const unsigned FILTER_UPDATE_PERIOD_MS = 12;	// ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
 
-	unsigned _min_obs_interval_us; // minimum time interval between observations that will guarantee data is not lost (usec)
+	unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
 
-	float _dt_imu_avg;	// average imu update period in s
+	float _dt_imu_avg{0.0f};	// 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;
-	baroSample _baro_sample_delayed;
-	gpsSample _gps_sample_delayed;
-	rangeSample _range_sample_delayed;
-	airspeedSample _airspeed_sample_delayed;
-	flowSample _flow_sample_delayed;
-	extVisionSample _ev_sample_delayed;
-	dragSample _drag_sample_delayed;
-	dragSample _drag_down_sampled;	// down sampled drag specific force data (filter prediction rate -> observation rate)
+	magSample _mag_sample_delayed{};
+	baroSample _baro_sample_delayed{};
+	gpsSample _gps_sample_delayed{};
+	rangeSample _range_sample_delayed{};
+	airspeedSample _airspeed_sample_delayed{};
+	flowSample _flow_sample_delayed{};
+	extVisionSample _ev_sample_delayed{};
+	dragSample _drag_sample_delayed{};
+	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
 
 	// Used by the multi-rotor specific drag force fusion
-	uint8_t _drag_sample_count;	// number of drag specific force samples assumulated at the filter prediction rate
-	float _drag_sample_time_dt;	// time integral across all samples used to form _drag_down_sampled (sec)
-	float _air_density;		// air density (kg/m**3)
+	uint8_t _drag_sample_count{0};	// number of drag specific force samples assumulated at the filter prediction rate
+	float _drag_sample_time_dt{0.0f};	// time integral across all samples used to form _drag_down_sampled (sec)
+	float _air_density{1.225f};		// air density (kg/m**3)
 
-	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
 	Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time
 
-	uint64_t _imu_ticks;	// counter for imu updates
+	uint64_t _imu_ticks{0};	// 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
+	bool _imu_updated{false};      // true if the ekf should update (completed downsampling process)
+	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized
 
-	bool _NED_origin_initialised;
-	bool _gps_speed_valid;
-	float _gps_origin_eph; // horizontal position uncertainty of the GPS origin
-	float _gps_origin_epv; // vertical position uncertainty of the GPS origin
-	struct map_projection_reference_s _pos_ref;    // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
-	struct map_projection_reference_s _gps_pos_prev;    // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
-	float _gps_alt_prev;	// height from the previous GPS message (m)
+	bool _NED_origin_initialised{false};
+	bool _gps_speed_valid{false};
+	float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin
+	float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin
+	struct map_projection_reference_s _pos_ref {};   // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
+	struct map_projection_reference_s _gps_pos_prev {};   // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
+	float _gps_alt_prev{0.0f};	// height from the previous GPS message (m)
 
 	// innovation consistency check monitoring ratios
-	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
-	float _tas_test_ratio;		// tas innovation consistency check ratio
-	float _terr_test_ratio;		// height above terrain measurement innovation consistency check ratio
-	float _beta_test_ratio;		// sideslip innovation consistency check ratio
-	float _drag_test_ratio[2];	// drag innovation cinsistency check ratio
+	float _yaw_test_ratio{0.0f};          // 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
+	float _tas_test_ratio{0.0f};		// tas innovation consistency check ratio
+	float _terr_test_ratio{0.0f};		// height above terrain measurement innovation consistency check ratio
+	float _beta_test_ratio{0.0f};		// sideslip innovation consistency check ratio
+	float _drag_test_ratio[2] {};	// drag innovation cinsistency check ratio
 	innovation_fault_status_u _innov_check_fail_status{};
 
-	bool _is_dead_reckoning;	// true if we are no longer fusing measurements that constrain horizontal velocity drift
+	bool _is_dead_reckoning{false};	// true if we are no longer fusing measurements that constrain horizontal velocity drift
 
 	// IMU vibration monitoring
 	Vector3f _delta_ang_prev;	// delta angle from the previous IMU measurement
 	Vector3f _delta_vel_prev;	// delta velocity from the previous IMU measurement
-	float _vibe_metrics[3];		// IMU vibration metrics
+	float _vibe_metrics[3] {};	// IMU vibration metrics
 					// [0] Level of coning vibration in the IMU delta angles (rad^2)
 					// [1] high frequency vibraton level in the IMU delta angle data (rad)
 					// [2] high frequency vibration level in the IMU delta velocity data (m/s)
@@ -380,14 +383,14 @@ protected:
 	RingBuffer<outputSample> _output_buffer;
 	RingBuffer<dragSample> _drag_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_airspeed;	// timestamp of last airspeed measurement in microseconds
-	uint64_t _time_last_ext_vision; // timestamp of last external vision measurement in microseconds
-	uint64_t _time_last_optflow;
+	uint64_t _time_last_imu{0};	// timestamp of last imu sample in microseconds
+	uint64_t _time_last_gps{0};	// timestamp of last gps measurement in microseconds
+	uint64_t _time_last_mag{0};	// timestamp of last magnetometer measurement in microseconds
+	uint64_t _time_last_baro{0};	// timestamp of last barometer measurement in microseconds
+	uint64_t _time_last_range{0};	// timestamp of last range measurement in microseconds
+	uint64_t _time_last_airspeed{0};	// timestamp of last airspeed measurement in microseconds
+	uint64_t _time_last_ext_vision{0}; // timestamp of last external vision measurement in microseconds
+	uint64_t _time_last_optflow{0};
 
 	fault_status_u _fault_status{};
 
@@ -397,8 +400,8 @@ protected:
 	// free buffer memory
 	void unallocate_buffers();
 
-	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)
+	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)
 
 	// this is the current status of the filter control modes
 	filter_control_status_u _control_status{};
@@ -410,6 +413,6 @@ protected:
 	Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2);
 
 	// calculate the inverse rotation matrix from a quaternion rotation
-	Matrix3f quat_to_invrotmat(const Quaternion& quat);
+	Matrix3f quat_to_invrotmat(const Quaternion &quat);
 
 };