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

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

@@ -109,13 +109,20 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		const float range = predictFlowRange();
 		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++;
 		}
 

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

@@ -123,6 +123,7 @@ public:
 
 	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; }
@@ -621,6 +622,7 @@ private:
 	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
@@ -868,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);