AP_NavEKF2: Correct optical flow data for sensor position offset

Correction requires the body rates averaged across the flow sensor sampling interval. This data has been added to the sensor buffer.
The body rate data from the flow sensor driver does not contain the Z component, so an equivalent value sampled from the navigation IMU has been used instead.
The variable omegaAcrossFlowTime has been moved out of the class and into the only function that uses it.

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -136,17 +136,28 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
     stateStruct.quat.rotation_matrix(Tbn_flow);
     // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
     if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
-        // correct flow sensor rates for bias
+        // correct flow sensor body rates for bias and write
-        omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x;
+        ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
-        omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y;
+        ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
-        // write uncorrected flow rate measurements that will be used by the focal length scale factor estimator
+        // the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
+        if (delTimeOF > 0.001f) {
+            // first preference is to use the rate averaged over the same sampling period as the flow sensor
+            ofDataNew.bodyRadXYZ.z = delAngBodyOF.z / delTimeOF;
+        } else if (imuDataNew.delAngDT > 0.001f){
+            // second preference is to use most recent IMU data
+            ofDataNew.bodyRadXYZ.z = imuDataNew.delAng.z / imuDataNew.delAngDT;
+        } else {
+            // third preference is use zero
+            ofDataNew.bodyRadXYZ.z =  0.0f;
+        }
+        // write uncorrected flow rate measurements
         // note correction for different axis and sign conventions used by the px4flow sensor
         ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
         // write the flow sensor position in body frame
         ofDataNew.body_offset = posOffset;
         // write flow rate measurements corrected for body rates
-        ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + omegaAcrossFlowTime.x;
+        ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
-        ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + omegaAcrossFlowTime.y;
+        ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
         // record time last observation was received so we can detect loss of data elsewhere
         flowValidMeaTime_ms = imuSampleTime_ms;
         // estimate sample time of the measurement

libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

@@ -303,8 +303,15 @@ void NavEKF2_core::FuseOptFlow()
         // calculate range from ground plain to centre of sensor fov assuming flat earth
         float range = constrain_float((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f);
 
-        // calculate relative velocity in sensor frame
+        // correct range for flow sensor offset body frame position offset
-        relVelSensor = prevTnb*stateStruct.velocity;
+        // the corrected value is the predicted range from the sensor focal point to the
+        // centre of the image on the ground assuming flat terrain
+        Vector3f posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
+        Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
+        range -= posOffsetEarth.z / prevTnb.c.z;
+
+        // calculate relative velocity in sensor frame including the relative motion due to rotation
+        relVelSensor = prevTnb*stateStruct.velocity + ofDataDelayed.bodyRadXYZ % posOffsetBody;
 
         // divide velocity by range  to get predicted angular LOS rates relative to X and Y axes
         losPred[0] =  relVelSensor.y/range;

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -403,6 +403,7 @@ private:
         Vector2f    flowRadXYcomp;  // 2..3
         uint32_t    time_ms;        // 4
         Vector3f    body_offset;    // 5..7
+        Vector3f    bodyRadXYZ;     //8..10
     };
 
     // update the navigation filter status
@@ -875,7 +876,6 @@ private:
     uint32_t rngValidMeaTime_ms;    // time stamp from latest valid range measurement (msec)
     uint32_t flowMeaTime_ms;        // time stamp from latest flow measurement (msec)
     uint32_t gndHgtValidTime_ms;    // time stamp from last terrain offset state update (msec)
-    Vector3f omegaAcrossFlowTime;   // body angular rates averaged across the optical flow sample period
     Matrix3f Tbn_flow;              // transformation matrix from body to nav axes at the middle of the optical flow sample period
     Vector2 varInnovOptFlow;        // optical flow innovations variances (rad/sec)^2
     Vector2 innovOptFlow;           // optical flow LOS innovations (rad/sec)