Pr opt flow (#482)

* optical flow: fixed calculation of velocity of the flow sensor relative to
the IMU

- gyroXYZ holds a delta angle and first needs to converted to a gyro rate

Signed-off-by: Roman <bapstroman@gmail.com>

* optical flow: calculate height above the ground with respect to the flow
camera
- the flow camera can be offset from the IMU which needs to be considered

Signed-off-by: Roman <bapstroman@gmail.com>

* estimator interface: fixed comment regarding optical flow sample timestamp

- the timestamp on an optical flow sample corresponds to the trailing
edge of the flow integration period

Signed-off-by: Roman <bapstroman@gmail.com>

EKF/estimator_interface.cpp

@@ -380,7 +380,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 		// If flow quality fails checks on ground, assume zero flow rate after body rate compensation
 		if ((delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow)) || !_control_status.flags.in_air) {
 			flowSample optflow_sample_new;
-			// calculate the system time-stamp for the leading edge of the flow data integration period
+			// calculate the system time-stamp for the trailing edge of the flow data integration period
 			optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000;
 
 			// copy the quality metric returned by the PX4Flow sensor

EKF/optflow_fusion.cpp

@@ -67,9 +67,6 @@ void Ekf::fuseOptFlow()
 	float H_LOS[2][24] = {}; // Optical flow observation Jacobians
 	float Kfusion[24][2] = {}; // Optical flow Kalman gains
 
-	// constrain height above ground to be above minimum height when sitting on ground
-	float heightAboveGndEst = math::max((_terrain_vpos - _state.pos(2)), gndclearance);
-
 	// get rotation nmatrix from earth to body
 	Dcmf earth_to_body(_state.quat_nominal);
 	earth_to_body = earth_to_body.transpose();
@@ -78,7 +75,7 @@ void Ekf::fuseOptFlow()
 	Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
 
 	// calculate the velocity of the sensor reelative to the imu in body frame
-	Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ, pos_offset_body);
+	Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt, pos_offset_body);
 
 	// calculate the velocity of the sensor in the earth frame
 	Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
@@ -86,6 +83,19 @@ void Ekf::fuseOptFlow()
 	// rotate into body frame
 	Vector3f vel_body = earth_to_body * vel_rel_earth;
 
+	// height above ground of the IMU
+	float heightAboveGndEst = _terrain_vpos - _state.pos(2);
+
+	// calculate the sensor position relative to the IMU in earth frame
+	Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+
+	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
+	// a positve offset in earth frame leads to a a smaller height above the ground.
+	heightAboveGndEst -= pos_offset_earth(2);
+
+	// constrain minimum height above ground
+	heightAboveGndEst = math::max(heightAboveGndEst, gndclearance);
+
 	// calculate range from focal point to centre of image
 	float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view