ekf2: filter flow vel (used for flow velocity reset)

- individual flow samples can be quite erratic

msg/VehicleOpticalFlowVel.msg

@@ -4,6 +4,9 @@ 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[2] vel_body_filtered           # filtered velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
+float32[2] vel_ne_filtered             # filtered same as vel_body_filtered but in local frame (m/s)
+
 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

@@ -122,8 +122,16 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		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));
 
+		if (_flow_counter == 0) {
+			_flow_vel_body_lpf.reset(_flow_vel_body);
+			_flow_counter = 1;
+
+		} else {
+
+			_flow_vel_body_lpf.update(_flow_vel_body);
+			_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
@@ -144,6 +152,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
 
@@ -219,7 +228,7 @@ void Ekf::resetFlowFusion()
 	_information_events.flags.reset_vel_to_flow = true;
 
 	const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(_flow_sample_delayed);
-	resetHorizontalVelocityTo(_flow_vel_ne, flow_vel_var);
+	resetHorizontalVelocityTo(getFilteredFlowVelNE(), flow_vel_var);
 
 	// reset position, estimate is relative to initial position in this mode, so we start with zero error
 	if (!_control_status.flags.in_air) {
@@ -273,6 +282,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/ekf.h

@@ -122,7 +122,10 @@ public:
 	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; }
+	Vector2f getFlowVelNE() const { return Vector2f(_R_to_earth * Vector3f(getFlowVelBody()(0), getFlowVelBody()(1), 0.f)); }
+
+	const Vector2f &getFilteredFlowVelBody() const { return _flow_vel_body_lpf.getState(); }
+	Vector2f getFilteredFlowVelNE() const { return Vector2f(_R_to_earth * Vector3f(getFilteredFlowVelBody()(0), getFilteredFlowVelBody()(1), 0.f)); }
 
 	const Vector2f &getFlowCompensated() const { return _flow_rate_compensated; }
 	const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_rate; }
@@ -634,10 +637,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{};
 
+	Vector2f _flow_vel_body{};                      ///< velocity from corrected flow measurement (body frame)(m/s)
+	AlphaFilter<Vector2f> _flow_vel_body_lpf{0.1f}; ///< filtered velocity from corrected flow measurement (body frame)(m/s)
+	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
 

src/modules/ekf2/EKF2.cpp

@@ -2027,6 +2027,9 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
 		_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
 		_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
 
+		_ekf.getFilteredFlowVelBody().copyTo(flow_vel.vel_body_filtered);
+		_ekf.getFilteredFlowVelNE().copyTo(flow_vel.vel_ne_filtered);
+
 		_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();