ekf2: fix flow gyro bias corrections (#22145)

* ekf2-flow: fix flow gyro bias compensation
* ekf2-flow: apply flow gyro bias when used
* ekf2: log optical flow gyro bias
* ekf2: optical flow control always use provided flow gyro (with bias applied)
* ekf2-flow: log flow gyro and gyro reference
* ekf2-flow: support senrors with XY flow gyro

---------

Co-authored-by: Daniel Agar <daniel@agar.ca>
Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>

msg/VehicleOpticalFlowVel.msg

@@ -10,4 +10,8 @@ float32[2] flow_compensated_integral   # integrated optical flow measurement com
 float32[3] gyro_rate                   # gyro measurement synchronized with flow measurements (rad/s)
 float32[3] gyro_rate_integral          # gyro measurement integrated to flow rate and synchronized with flow measurements (rad)
 
+float32[3] gyro_bias
+float32[3] ref_gyro
+float32[3] meas_gyro
+
 # TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel

src/drivers/uavcan/sensors/flow.cpp

@@ -79,7 +79,7 @@ void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure<com::hex:
 	if (PX4_ISFINITE(msg.rate_gyro_integral[0]) && PX4_ISFINITE(msg.rate_gyro_integral[1])) {
 		flow.delta_angle[0] = msg.rate_gyro_integral[0];
 		flow.delta_angle[1] = msg.rate_gyro_integral[1];
-		flow.delta_angle[2] = 0.f;
+		flow.delta_angle[2] = NAN;
 		flow.delta_angle_available = true;
 
 	} else {

src/modules/ekf2/EKF/ekf.h

@@ -180,6 +180,9 @@ public:
 
 	const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
 	const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }
+	const Vector3f &getFlowGyroBias() const { return _flow_gyro_bias; }
+	const Vector3f &getRefBodyRate() const { return _ref_body_rate; }
+	const Vector3f &getMeasuredBodyRate() const { return _measured_body_rate; }
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_AUXVEL)
@@ -656,6 +659,8 @@ private:
 	Vector2f _flow_vel_body{};	///< velocity from corrected flow measurement (body frame)(m/s)
 	Vector2f _flow_vel_ne{};		///< velocity from corrected flow measurement (local frame) (m/s)
 	Vector3f _imu_del_ang_of{};	///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
+	Vector3f _ref_body_rate{};
+	Vector3f _measured_body_rate{};
 
 	float _delta_time_of{0.0f};	///< time in sec that _imu_del_ang_of was accumulated over (sec)
 	uint64_t _time_bad_motion_us{0};	///< last system time that on-ground motion exceeded limits (uSec)

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -185,7 +185,7 @@ Vector2f Ekf::predictFlowVelBody()
 
 	// calculate the velocity of the sensor relative to the imu in body frame
 	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
-	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;
+	const Vector3f vel_rel_imu_body = Vector3f(-(_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt - _flow_gyro_bias)) % pos_offset_body;
 
 	// calculate the velocity of the sensor in the earth frame
 	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
@@ -200,41 +200,30 @@ Vector2f Ekf::predictFlowVelBody()
 bool Ekf::calcOptFlowBodyRateComp()
 {
 	bool is_body_rate_comp_available = false;
-	const bool use_flow_sensor_gyro = _flow_sample_delayed.gyro_xyz.isAllFinite();
 
-	if (use_flow_sensor_gyro) {
-
-		// if accumulation time differences are not excessive and accumulation time is adequate
-		// compare the optical flow and and navigation rate data and calculate a bias error
-		if ((_delta_time_of > FLT_EPSILON)
-		    && (_flow_sample_delayed.dt > FLT_EPSILON)
-		    && (fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f)) {
-
-			const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of));
-
-			const Vector3f measured_body_rate(_flow_sample_delayed.gyro_xyz * (1.0f / _flow_sample_delayed.dt));
-
-			// calculate the bias estimate using  a combined LPF and spike filter
-			_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f;
-
-			// apply gyro bias
-			_flow_sample_delayed.gyro_xyz -= (_flow_gyro_bias * _flow_sample_delayed.dt);
-
-			is_body_rate_comp_available = true;
-		}
-
-	} else {
-		// Use the EKF gyro data if optical flow sensor gyro data is not available
-		// for clarification of the sign see definition of flowSample and imuSample in common.h
 	if ((_delta_time_of > FLT_EPSILON)
 	    && (_flow_sample_delayed.dt > FLT_EPSILON)) {
+		const Vector3f reference_body_rate = -_imu_del_ang_of / _delta_time_of; // flow gyro has opposite sign convention
+		_ref_body_rate = reference_body_rate;
 
-			_flow_sample_delayed.gyro_xyz = -_imu_del_ang_of / _delta_time_of * _flow_sample_delayed.dt;
+		if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1))) {
-			_flow_gyro_bias.zero();
+			_flow_sample_delayed.gyro_xyz = reference_body_rate * _flow_sample_delayed.dt;
+
+		} else if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) {
+			// Some flow modules only provide X ind Y angular rates. If this is the case, complete the vector with our own Z gyro
+			_flow_sample_delayed.gyro_xyz(2) = reference_body_rate(2) * _flow_sample_delayed.dt;
+		}
+
+		const Vector3f measured_body_rate = _flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt;
+		_measured_body_rate = measured_body_rate;
+
+		if (fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) {
+			// calculate the bias estimate using  a combined LPF and spike filter
+			_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f;
+		}
 
 		is_body_rate_comp_available = true;
 	}
-	}
 
 	// reset the accumulators
 	_imu_del_ang_of.setZero();

src/modules/ekf2/EKF2.cpp

@@ -2001,6 +2001,10 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
 		_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate);
 		_ekf.getFlowGyroIntegral().copyTo(flow_vel.gyro_rate_integral);
 
+		_ekf.getFlowGyroBias().copyTo(flow_vel.gyro_bias);
+		_ekf.getRefBodyRate().copyTo(flow_vel.ref_gyro);
+		_ekf.getMeasuredBodyRate().copyTo(flow_vel.meas_gyro);
+
 		flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 
 		_estimator_optical_flow_vel_pub.publish(flow_vel);

src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp

@@ -126,9 +126,20 @@ void VehicleOpticalFlow::Run()
 
 		// delta angle
 		//  - from sensor_optical_flow if available, otherwise use synchronized sensor_gyro if available
-		if (sensor_optical_flow.delta_angle_available && Vector3f(sensor_optical_flow.delta_angle).isAllFinite()) {
+		if (sensor_optical_flow.delta_angle_available && Vector2f(sensor_optical_flow.delta_angle).isAllFinite()) {
 			// passthrough integrated gyro if available
-			_delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle};
+			Vector3f delta_angle(sensor_optical_flow.delta_angle);
+
+			if (!PX4_ISFINITE(delta_angle(2))) {
+				// Some sensors only provide X and Y angular rates, rotate them but place back the NAN on the Z axis
+				delta_angle(2) = 0.f;
+				_delta_angle += _flow_rotation * delta_angle;
+				_delta_angle(2) = NAN;
+
+			} else {
+				_delta_angle += _flow_rotation * delta_angle;
+			}
+
 			_delta_angle_available = true;
 
 		} else {
@@ -320,10 +331,12 @@ void VehicleOpticalFlow::Run()
 				// gyro_rate
 				flow_vel.gyro_rate[0] = measured_body_rate(0);
 				flow_vel.gyro_rate[1] = measured_body_rate(1);
+				flow_vel.gyro_rate[2] = measured_body_rate(2);
 
 				// gyro_rate_integral
 				flow_vel.gyro_rate_integral[0] = gyro_xyz(0);
 				flow_vel.gyro_rate_integral[1] = gyro_xyz(1);
+				flow_vel.gyro_rate_integral[2] = gyro_xyz(2);
 
 				flow_vel.timestamp = hrt_absolute_time();