fix delta angle bias usage:

the delta angle bias was applied to imu data which was not coming in the same
time intervall as the filter was operating. Therefore, the delta angle bias
applied to new imu data had to be scaled correctly in order to match
the imu time interval.

EKF/ekf.cpp

@@ -232,14 +232,14 @@ void Ekf::calculateOutputStates()
 {
 	imuSample imu_new = _imu_sample_new;
 	Vector3f delta_angle;
-	delta_angle(0) = imu_new.delta_ang(0) * _state.gyro_scale(0);
-	delta_angle(1) = imu_new.delta_ang(1) * _state.gyro_scale(1);
-	delta_angle(2) = imu_new.delta_ang(2) * _state.gyro_scale(2);
 
-	delta_angle -= _state.gyro_bias;
+	// Note: We do no not need to consider any bias or scale correction here
+	// since the base class has already corrected the imu sample
+	delta_angle(0) = imu_new.delta_ang(0);
+	delta_angle(1) = imu_new.delta_ang(1);
+	delta_angle(2) = imu_new.delta_ang(2);
 
 	Vector3f delta_vel = imu_new.delta_vel;
-	delta_vel(2) -= _state.accel_z_bias;
 
 	delta_angle += _delta_angle_corr;
 	Quaternion dq;
@@ -265,9 +265,7 @@ void Ekf::calculateOutputStates()
 		_imu_updated = false;
 	}
 
-	if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed)) {
-		return;
-	}
+	_output_sample_delayed = _output_buffer.get_oldest();
 
 	Quaternion quat_inv = _state.quat_nominal.inversed();
 	Quaternion q_error =  _output_sample_delayed.quat_nominal * quat_inv;

EKF/estimator_base.cpp

@@ -83,14 +83,15 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
 
 	imu_sample_new.time_us = time_usec;
 
-	_imu_sample_new = imu_sample_new;
-
 	imu_sample_new.delta_ang(0) = imu_sample_new.delta_ang(0) * _state.gyro_scale(0);
 	imu_sample_new.delta_ang(1) = imu_sample_new.delta_ang(1) * _state.gyro_scale(1);
 	imu_sample_new.delta_ang(2) = imu_sample_new.delta_ang(2) * _state.gyro_scale(2);
 
-	imu_sample_new.delta_ang -= _state.gyro_bias;
-	imu_sample_new.delta_vel(2) -= _state.accel_z_bias;
+	imu_sample_new.delta_ang -= _state.gyro_bias * imu_sample_new.delta_ang_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);
+	imu_sample_new.delta_vel(2) -= _state.accel_z_bias * imu_sample_new.delta_vel_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);;
+
+	// store the new sample for the complementary filter prediciton
+	_imu_sample_new = imu_sample_new;
 
 	_imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt;
 	_imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt;