ekf2: use bias corrected angular velocity

src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp

@@ -33,12 +33,12 @@
 
 #include "ekf.h"
 
-void Ekf::controlAuxVelFusion()
+void Ekf::controlAuxVelFusion(const imuSample &imu_sample)
 {
 	if (_auxvel_buffer) {
 		auxVelSample sample;
 
-		if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &sample)) {
+		if (_auxvel_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
 
 			updateAidSourceStatus(_aid_src_aux_vel,
 					      sample.time_us,                                           // sample timestamp

src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp

@@ -38,7 +38,7 @@
 
 #include "ekf.h"
 
-void Ekf::controlBaroHeightFusion()
+void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "baro";
 
@@ -49,10 +49,10 @@ void Ekf::controlBaroHeightFusion()
 
 	baroSample baro_sample;
 
-	if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) {
+	if (_baro_buffer && _baro_buffer->pop_first_older_than(imu_sample.time_us, &baro_sample)) {
 
 #if defined(CONFIG_EKF2_BARO_COMPENSATION)
-		const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt);
+		const float measurement = compensateBaroForDynamicPressure(imu_sample, baro_sample.hgt);
 #else
 		const float measurement = baro_sample.hgt;
 #endif
@@ -137,7 +137,7 @@ void Ekf::controlBaroHeightFusion()
 					// reset vertical velocity
 					resetVerticalVelocityToZero();
 
-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;
 
 				} else if (is_fusion_failing) {
 					// Some other height source is still working
@@ -166,7 +166,7 @@ void Ekf::controlBaroHeightFusion()
 					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
 				}
 
-				aid_src.time_last_fuse = _time_delayed_us;
+				aid_src.time_last_fuse = imu_sample.time_us;
 				bias_est.setFusionActive();
 				_control_status.flags.baro_hgt = true;
 			}
@@ -195,15 +195,16 @@ void Ekf::stopBaroHgtFusion()
 }
 
 #if defined(CONFIG_EKF2_BARO_COMPENSATION)
-float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
+float Ekf::compensateBaroForDynamicPressure(const imuSample &imu_sample, const float baro_alt_uncompensated) const
 {
 	if (_control_status.flags.wind && local_position_is_valid()) {
 		// calculate static pressure error = Pmeas - Ptruth
 		// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
 		// negative X and Y directions. Used to correct baro data for positional errors
 
 		// Calculate airspeed in body frame
-		const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body);
+		const Vector3f angular_velocity = (imu_sample.delta_ang / imu_sample.delta_ang_dt) - _state.gyro_bias;
+		const Vector3f vel_imu_rel_body_ned = _R_to_earth * (angular_velocity % _params.imu_pos_body);
 		const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned;
 
 		const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f);

src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp

@@ -38,15 +38,15 @@
 
 #include "ekf.h"
 
-void Ekf::controlExternalVisionFusion()
+void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
 {
 	_ev_pos_b_est.predict(_dt_ekf_avg);
 	_ev_hgt_b_est.predict(_dt_ekf_avg);
 
 	// Check for new external vision data
 	extVisionSample ev_sample;
 
-	if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_time_delayed_us, &ev_sample)) {
+	if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(imu_sample.time_us, &ev_sample)) {
 
 		bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter);
 
@@ -62,10 +62,11 @@ void Ekf::controlExternalVisionFusion()
 				&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
 
 		updateEvAttitudeErrorFilter(ev_sample, ev_reset);
-		controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
-		controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
-		controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
-		controlEvHeightFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt);
+		controlEvYawFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
+		controlEvVelFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
+		controlEvPosFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
+		controlEvHeightFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient,
+				      _aid_src_ev_hgt);
 
 		if (quality_sufficient) {
 			_ev_sample_prev = ev_sample;

src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp

@@ -38,8 +38,9 @@
 
 #include "ekf.h"
 
-void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-				const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src)
+void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+				const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+				estimator_aid_source1d_s &aid_src)
 {
 	static constexpr const char *AID_SRC_NAME = "EV height";
 
@@ -152,7 +153,8 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 				if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
 
 					// correct velocity for offset relative to IMU
-					const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+					const Vector3f angular_velocity = (imu_sample.delta_ang / imu_sample.delta_ang_dt) - _state.gyro_bias;
+					const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
 					const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 
 					switch (ev_sample.vel_frame) {

src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp

@@ -41,8 +41,9 @@
 static constexpr const char *EV_AID_SRC_NAME = "EV position";
 
 
-void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src)
+void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+			     const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+			     estimator_aid_source2d_s &aid_src)
 {
 	const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
 					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);

src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp

@@ -41,8 +41,9 @@
 #include <ekf_derivation/generated/compute_ev_body_vel_hy.h>
 #include <ekf_derivation/generated/compute_ev_body_vel_hz.h>
 
-void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src)
+void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+			     const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+			     estimator_aid_source3d_s &aid_src)
 {
 	static constexpr const char *AID_SRC_NAME = "EV velocity";
 
@@ -55,8 +56,9 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 					     && ev_sample.vel.isAllFinite();
 
 	// correct velocity for offset relative to IMU
+	const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
 	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+	const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
 	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 
 	// rotate measurement into correct earth frame if required

src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp

@@ -38,8 +38,9 @@
 
 #include "ekf.h"
 
-void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src)
+void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+			     const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+			     estimator_aid_source1d_s &aid_src)
 {
 	static constexpr const char *AID_SRC_NAME = "EV yaw";
 

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -86,7 +86,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 			updateGnssPos(gnss_sample, _aid_src_gnss_pos);
 		}
 
-		updateGnssVel(gnss_sample, _aid_src_gnss_vel);
+		updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel);
 
 	} else if (_control_status.flags.gps) {
 		if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
@@ -173,12 +173,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 	}
 }
 
-void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
+void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
 {
 	// correct velocity for offset relative to IMU
 	const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
 
-	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+	const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
+	const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
 	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 	const Vector3f velocity = gnss_sample.vel - vel_offset_earth;
 

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -41,7 +41,7 @@
 
 #include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
 
-void Ekf::controlMagFusion()
+void Ekf::controlMagFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *AID_SRC_NAME = "mag";
 	estimator_aid_source3d_s &aid_src = _aid_src_mag;
@@ -59,7 +59,7 @@ void Ekf::controlMagFusion()
 
 	magSample mag_sample;
 
-	if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) {
+	if (_mag_buffer && _mag_buffer->pop_first_older_than(imu_sample.time_us, &mag_sample)) {
 
 		if (mag_sample.reset || (_mag_counter == 0)) {
 			// sensor or calibration has changed, reset low pass filter
@@ -185,7 +185,7 @@ void Ekf::controlMagFusion()
 				if (mag_sample.reset || checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) {
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;
 
 				} else {
 					// The normal sequence is to fuse the magnetometer data first before fusing
@@ -213,7 +213,7 @@ void Ekf::controlMagFusion()
 					if (no_ne_aiding_or_pre_takeoff) {
 						ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
 						resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
-						aid_src.time_last_fuse = _time_delayed_us;
+						aid_src.time_last_fuse = imu_sample.time_us;
 
 					} else {
 						ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
@@ -242,7 +242,7 @@ void Ekf::controlMagFusion()
 					bool reset_heading = !_control_status.flags.yaw_align;
 
 					resetMagStates(_mag_lpf.getState(), reset_heading);
-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;
 
 					if (reset_heading) {
 						_control_status.flags.yaw_align = true;

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -39,23 +39,23 @@
 #include "ekf.h"
 #include "ekf_derivation/generated/compute_hagl_innov_var.h"
 
-void Ekf::controlRangeHaglFusion()
+void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "RNG";
 
 	bool rng_data_ready = false;
 
 	if (_range_buffer) {
 		// Get range data from buffer and check validity
-		rng_data_ready = _range_buffer->pop_first_older_than(_time_delayed_us, _range_sensor.getSampleAddress());
+		rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
 		_range_sensor.setDataReadiness(rng_data_ready);
 
 		// update range sensor angle parameters in case they have changed
 		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
 
-		_range_sensor.runChecks(_time_delayed_us, _R_to_earth);
+		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
 
 		if (_range_sensor.isDataHealthy()) {
 			// correct the range data for position offset relative to the IMU
@@ -72,7 +72,7 @@ void Ekf::controlRangeHaglFusion()
 
 				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
 				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
-							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us);
+							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us);
 			}
 
 		} else {
@@ -151,7 +151,7 @@ void Ekf::controlRangeHaglFusion()
 					_control_status.flags.rng_hgt = true;
 					stopRngTerrFusion();
 
-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;
 				}
 
 			} else {
@@ -183,7 +183,7 @@ void Ekf::controlRangeHaglFusion()
 					// reset vertical velocity
 					resetVerticalVelocityToZero();
 
-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;
 
 				} else if (is_fusion_failing) {
 					// Some other height source is still working

src/modules/ekf2/EKF/control.cpp

@@ -102,7 +102,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control use of observations for aiding
-	controlMagFusion();
+	controlMagFusion(imu_delayed);
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -137,12 +137,12 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// Additional data odometry data from an external estimator can be fused.
-	controlExternalVisionFusion();
+	controlExternalVisionFusion(imu_delayed);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_AUXVEL)
 	// Additional horizontal velocity data from an auxiliary sensor can be fused
-	controlAuxVelFusion();
+	controlAuxVelFusion(imu_delayed);
 #endif // CONFIG_EKF2_AUXVEL
 	//
 #if defined(CONFIG_EKF2_TERRAIN)

src/modules/ekf2/EKF/covariance.cpp

@@ -134,8 +134,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 	// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
 	P = sym::PredictCovariance(_state.vector(), P,
-				   imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var,
-				   imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var,
+				   imu_delayed.delta_vel / imu_delayed.delta_vel_dt, accel_var,
+				   imu_delayed.delta_ang / imu_delayed.delta_ang_dt, gyro_var,
 				   dt);
 
 	// Construct the process noise variance diagonal for those states with a stationary process model

src/modules/ekf2/EKF/ekf.cpp

@@ -89,8 +89,6 @@ void Ekf::reset()
 	_control_status.flags.in_air = true;
 	_control_status_prev.flags.in_air = true;
 
-	_ang_rate_delayed_raw.zero();
-
 	_fault_status.value = 0;
 	_innov_check_fail_status.value = 0;
 
@@ -264,13 +262,6 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_state.vel = matrix::constrain(_state.vel, -1000.f, 1000.f);
 	_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);
 
-	// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
-	// protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate
-	// due to insufficient averaging
-	if (imu_delayed.delta_ang_dt > 0.25f * _dt_ekf_avg) {
-		_ang_rate_delayed_raw = imu_delayed.delta_ang / imu_delayed.delta_ang_dt;
-	}
-
 
 	// calculate a filtered horizontal acceleration with a 1 sec time constant
 	// this are used for manoeuvre detection elsewhere