Vehicle_odometry: protect angular_velocity field against aliasing

Accumulate delta-angles between each publication to get the correct
angular velocity between two attitudes

src/modules/ekf2/EKF/estimator_interface.h

@@ -247,6 +247,7 @@ public:
 	int getNumberOfActiveVerticalVelocityAidingSources() const;
 
 	const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); }
+	Vector3f getAngularVelocityAndResetAccumulator() { return _output_predictor.getAngularVelocityAndResetAccumulator(); }
 	float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); }
 	Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
 

src/modules/ekf2/EKF/output_predictor/output_predictor.cpp

@@ -113,6 +113,9 @@ void OutputPredictor::reset()
 	_delta_vel_sum.setZero();
 	_delta_vel_dt = 0.f;
 
+	_delta_angle_sum.setIdentity();
+	_delta_angle_sum_dt = 0.f;
+
 	_delta_angle_corr.setZero();
 
 	_vel_err_integ.setZero();
@@ -247,13 +250,18 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
 
 	// update auxiliary yaw estimate
 	// rotate the state quternion by the delta quaternion only corrected for bias without EKF corrections
-	const Vector3f delta_angle_unaided = delta_angle - delta_angle_bias_scaled;
+	const Quatf delta_quat_unaided = Quatf(AxisAnglef(delta_angle - delta_angle_bias_scaled));
 	const float yaw_state = Eulerf(quat_nominal_before_update).psi();
-	const Quatf quat_unaided = quat_nominal_before_update * Quatf(AxisAnglef(delta_angle_unaided));
+	const Quatf quat_unaided = quat_nominal_before_update * delta_quat_unaided;
 	const float yaw_without_aiding = Eulerf(quat_unaided).psi();
 	// Yaw before delta quaternion applied and yaw after. The difference is the delta yaw. Accumulate it.
 	const float unaided_delta_yaw = yaw_without_aiding - yaw_state;
 	_unaided_yaw = matrix::wrap_pi(_unaided_yaw + unaided_delta_yaw);
+
+	// angular velocity downsampling
+	_delta_angle_sum *= delta_quat_unaided;
+	_delta_angle_sum.normalize();
+	_delta_angle_sum_dt += delta_angle_dt;
 }
 
 void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
@@ -412,3 +420,17 @@ void OutputPredictor::resetVelocityDerivativeAccumulation()
 	_delta_vel_dt = 0.f;
 	_delta_vel_sum.setZero();
 }
+
+Vector3f OutputPredictor::getAngularVelocityAndResetAccumulator()
+{
+	Vector3f angular_velocity;
+
+	if (_delta_angle_sum_dt > FLT_EPSILON) {
+		angular_velocity = AxisAnglef(_delta_angle_sum) / _delta_angle_sum_dt;
+	}
+
+	_delta_angle_sum.setIdentity();
+	_delta_angle_sum_dt = 0.f;
+
+	return angular_velocity;
+}

src/modules/ekf2/EKF/output_predictor/output_predictor.h

@@ -95,6 +95,8 @@ public:
 
 	const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
 
+	matrix::Vector3f getAngularVelocityAndResetAccumulator();
+
 	// get a yaw value solely based on bias-removed gyro integration
 	float getUnaidedYaw() const { return _unaided_yaw; }
 
@@ -192,6 +194,8 @@ private:
 
 	matrix::Vector3f _imu_pos_body{};                ///< xyz position of IMU in body frame (m)
 
+	matrix::Quatf _delta_angle_sum{};
+	float _delta_angle_sum_dt{0.f};
 	float _unaided_yaw{};
 
 	// output complementary filter tuning

src/modules/ekf2/EKF2.cpp

@@ -1676,9 +1676,7 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sa
 	_ekf.getVelocity().copyTo(odom.velocity);
 
 	// angular_velocity
-	const Vector3f rates{imu_sample.delta_ang / imu_sample.delta_ang_dt};
-	const Vector3f angular_velocity = rates - _ekf.getGyroBias();
-	angular_velocity.copyTo(odom.angular_velocity);
+	_ekf.getAngularVelocityAndResetAccumulator().copyTo(odom.angular_velocity);
 
 	// velocity covariances
 	_ekf.getVelocityVariance().copyTo(odom.velocity_variance);