ekf2: flow log calculated range from compensated flow and velocity estimate

- individual flow samples can be quite erratic

msg/VehicleOpticalFlowVel.msg

@@ -3,6 +3,7 @@ uint64 timestamp_sample                # the timestamp of the raw data (microsec
 
 float32[2] vel_body                    # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
 float32[2] vel_ne                      # same as vel_body but in local frame (m/s)
+float32 flow_range                     # range calculated from compensated flow and velocity estimate (m)
 
 float32[2] flow_rate_uncompensated     # integrated optical flow measurement (rad/s)
 float32[2] flow_rate_compensated       # integrated optical flow measurement compensated for angular motion (rad/s)

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp

@@ -107,10 +107,24 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 
 		// compute the velocities in body and local frames from corrected optical flow measurement for logging only
 		const float range = predictFlowRange();
-		_flow_vel_body(0) = -flow_compensated(1) * range;
-		_flow_vel_body(1) =  flow_compensated(0) * range;
-		_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
+		const Vector2f flow_vel_body{-flow_compensated(1) *range, flow_compensated(0) *range};
 
+		// compute the range from the compensated optical flow and current velocity state
+		const Vector2f vel_body = flowSensorVelocity(flow_gyro_corrected).xy();
+		const float flow_range = 0.5f * (math::max(0.f, vel_body(0) / -flow_compensated(1))
+						 + math::max(0.f, vel_body(1) / flow_compensated(0)));
+
+		if (_flow_counter == 0) {
+			_flow_vel_body.reset(flow_vel_body);
+			_flow_range.reset(flow_range);
+			_flow_counter = 1;
+
+		} else {
+
+			_flow_vel_body.update(flow_vel_body);
+			_flow_range.update(flow_range);
+			_flow_counter++;
+		}
 
 		// Check if we are in-air and require optical flow to control position drift
 		bool is_flow_required = _control_status.flags.in_air
@@ -131,6 +145,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				&& is_quality_good
 				&& is_magnitude_good
 				&& is_tilt_good
+				&& (_flow_counter > 10)
 				&& (isTerrainEstimateValid() || isHorizontalAidingActive())
 				&& isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
 
@@ -204,7 +219,7 @@ void Ekf::resetFlowFusion()
 {
 	ECL_INFO("reset velocity to flow");
 	_information_events.flags.reset_vel_to_flow = true;
-	resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
+	resetHorizontalVelocityTo(getFlowVelNE(), calcOptFlowMeasVar(_flow_sample_delayed));
 
 	// reset position, estimate is relative to initial position in this mode, so we start with zero error
 	if (!_control_status.flags.in_air) {
@@ -258,6 +273,8 @@ void Ekf::stopFlowFusion()
 
 		_innov_check_fail_status.flags.reject_optflow_X = false;
 		_innov_check_fail_status.flags.reject_optflow_Y = false;
+
+		_flow_counter = 0;
 	}
 }
 

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp

@@ -109,6 +109,22 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 	return false;
 }
 
+Vector3f Ekf::flowSensorVelocity(const Vector3f &flow_gyro) const
+{
+	// calculate the sensor position relative to the IMU
+	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
+
+	// calculate the velocity of the sensor relative to the imu in body frame
+	// Note: flow gyro is the negative of the body angular velocity, thus use minus sign
+	const Vector3f vel_rel_imu_body = -flow_gyro % 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;
+
+	// rotate into body frame
+	return _state.quat_nominal.rotateVectorInverse(vel_rel_earth);
+}
+
 float Ekf::predictFlowRange() const
 {
 	// calculate the sensor position relative to the IMU
@@ -134,18 +150,8 @@ float Ekf::predictFlowRange() const
 
 Vector2f Ekf::predictFlow(const Vector3f &flow_gyro) const
 {
-	// calculate the sensor position relative to the IMU
-	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
-
-	// calculate the velocity of the sensor relative to the imu in body frame
-	// Note: flow gyro is the negative of the body angular velocity, thus use minus sign
-	const Vector3f vel_rel_imu_body = -flow_gyro % 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;
-
 	// rotate into body frame
-	const Vector2f vel_body = _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy();
+	const Vector2f vel_body = flowSensorVelocity(flow_gyro).xy();
 
 	// calculate range from focal point to centre of image
 	const float range = predictFlowRange();

src/modules/ekf2/EKF/ekf.h

@@ -121,8 +121,9 @@ public:
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
 
-	const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
-	const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
+	const Vector2f &getFlowVelBody() const { return _flow_vel_body.getState(); }
+	Vector2f getFlowVelNE() const { return Vector2f(_R_to_earth * Vector3f(getFlowVelBody()(0), getFlowVelBody()(1), 0.f)); }
+	float getFlowRange() const { return _flow_range.getState(); }
 
 	const Vector2f &getFlowCompensated() const { return _flow_rate_compensated; }
 	const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_rate; }
@@ -618,10 +619,12 @@ private:
 
 	// optical flow processing
 	Vector3f _flow_gyro_bias{};	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
-	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 _ref_body_rate{};
 
+	AlphaFilter<Vector2f> _flow_vel_body{0.1f}; ///< filtered velocity from corrected flow measurement (body frame)(m/s)
+	AlphaFilter<float> _flow_range{0.1f};       ///< range from corrected flow measurement (m)
+	uint32_t _flow_counter{0};                  ///< number of flow samples read for initialization
+
 	Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
@@ -867,6 +870,9 @@ private:
 	// calculate optical flow body angular rate compensation
 	void calcOptFlowBodyRateComp(const flowSample &flow_sample);
 
+	// velocity in the optical flow sensor body frame
+	Vector3f flowSensorVelocity(const Vector3f &flow_gyro) const;
+
 	float predictFlowRange() const;
 	Vector2f predictFlow(const Vector3f &flow_gyro) const;
 

src/modules/ekf2/EKF2.cpp

@@ -2030,6 +2030,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
 
 		_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
 		_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
+		flow_vel.flow_range = _ekf.getFlowRange();
 
 		_ekf.getFlowUncompensated().copyTo(flow_vel.flow_rate_uncompensated);
 		_ekf.getFlowCompensated().copyTo(flow_vel.flow_rate_compensated);

src/modules/ekf2/test/test_EKF_flow.cpp

@@ -203,10 +203,8 @@ TEST_F(EkfFlowTest, inAirConvergence)
 	_sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity);
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
-	// Let it reset but not fuse more measurements. We actually need to send 2
-	// samples to get a reset because the first one cannot be used as the gyro
-	// compensation needs to be accumulated between two samples.
-	_sensor_simulator.runTrajectorySeconds(0.14);
+
+	_sensor_simulator.runTrajectorySeconds(1.0);
 
 	// THEN: estimated velocity should match simulated velocity
 	Vector3f estimated_velocity = _ekf->getVelocity();