EKF/Scripting: support automatic switching between GPS and optical flow

libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp

@@ -187,8 +187,8 @@ void NavEKF3_core::Log_Write_XKF5(uint64_t time_us) const
         time_us : time_us,
         core    : DAL_CORE(core_index),
         normInnov : (uint8_t)(MIN(100*MAX(flowTestRatio[0],flowTestRatio[1]),255)),  // normalised innovation variance ratio for optical flow observations fused by the main nav filter
-        FIX : (int16_t)(1000*innovOptFlow[0]),  // optical flow LOS rate vector innovations from the main nav filter
-        FIY : (int16_t)(1000*innovOptFlow[1]),  // optical flow LOS rate vector innovations from the main nav filter
+        FIX : (int16_t)(1000*flowInnov[0]),  // optical flow LOS rate vector innovations from the main nav filter
+        FIY : (int16_t)(1000*flowInnov[1]),  // optical flow LOS rate vector innovations from the main nav filter
         AFI : (int16_t)(1000*norm(auxFlowObsInnov.x,auxFlowObsInnov.y)),  // optical flow LOS rate innovation from terrain offset estimator
         HAGL : (int16_t)(100*(terrainState - stateStruct.position.z)),    // height above ground level
         offset : (int16_t)(100*terrainState),           // filter ground offset state error

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -193,11 +193,10 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
     // need to run the optical flow takeoff detection
     detectOptFlowTakeoff();
 
-    // calculate rotation matrices at mid sample time for flow observations
-    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 body rates for bias and write
+        of_elements ofDataNew {};
         ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
         ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
         // the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead

libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp

@@ -54,12 +54,11 @@ void NavEKF3_core::SelectFlowFusion()
 
     // Fuse optical flow data into the main filter
     if (flowDataToFuse && tiltOK) {
-        if ((frontend->_flowUse == FLOW_USE_NAV) && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW)) {
-            // Set the flow noise used by the fusion processes
-            R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
-            // Fuse the optical flow X and Y axis data into the main filter sequentially
-            FuseOptFlow(ofDataDelayed);
-        }
+        const bool fuse_optflow = (frontend->_flowUse == FLOW_USE_NAV) && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW);
+        // Set the flow noise used by the fusion processes
+        R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
+        // Fuse the optical flow X and Y axis data into the main filter sequentially
+        FuseOptFlow(ofDataDelayed, fuse_optflow);
     }
 }
 
@@ -265,12 +264,13 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
  * The script file used to generate these and other equations in this filter can be found here:
  * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
  * Requires a valid terrain height estimate.
+ *
+ * really_fuse should be true to actually fuse into the main filter, false to only calculate variances
 */
-void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
+void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fuse)
 {
     Vector24 H_LOS;
     Vector3F relVelSensor;
-    Vector14 SH_LOS;
     Vector2 losPred;
 
     // Copy required states to local variable names
@@ -285,23 +285,6 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
 
     // constrain height above ground to be above range measured on ground
     ftype heightAboveGndEst = MAX((terrainState - pd), rngOnGnd);
-    ftype ptd = pd + heightAboveGndEst;
-
-    // Calculate common expressions for observation jacobians
-    SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
-    SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2);
-    SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2);
-    SH_LOS[3] = 1/(pd - ptd);
-    SH_LOS[4] = vd*SH_LOS[0] - ve*(2*q0*q1 - 2*q2*q3) + vn*(2*q0*q2 + 2*q1*q3);
-    SH_LOS[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
-    SH_LOS[6] = 2.0f*q0*q1 + 2.0f*q2*q3;
-    SH_LOS[7] = q0*q0;
-    SH_LOS[8] = q1*q1;
-    SH_LOS[9] = q2*q2;
-    SH_LOS[10] = q3*q3;
-    SH_LOS[11] = q0*q3*2.0f;
-    SH_LOS[12] = pd-ptd;
-    SH_LOS[13] = 1.0f/(SH_LOS[12]*SH_LOS[12]);
 
     // Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
     for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first
@@ -441,10 +424,11 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
                 faultStatus.bad_xflow = true;
                 return;
             }
-            varInnovOptFlow[0] = t77;
+            flowVarInnov[0] = t77;
 
             // calculate innovation for X axis observation
-            innovOptFlow[0] = losPred[0] - ofDataDelayed.flowRadXYcomp.x;
+            // flowInnovTime_ms will be updated when Y-axis innovations are calculated
+            flowInnov[0] = losPred[0] - ofDataDelayed.flowRadXYcomp.x;
 
             // calculate Kalman gains for X-axis observation
             Kfusion[0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
@@ -617,10 +601,11 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
                 faultStatus.bad_yflow = true;
                 return;
             }
-            varInnovOptFlow[1] = t77;
+            flowVarInnov[1] = t77;
 
             // calculate innovation for Y observation
-            innovOptFlow[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y;
+            flowInnov[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y;
+            flowInnovTime_ms = AP_HAL::millis();
 
             // calculate Kalman gains for the Y-axis observation
             Kfusion[0] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
@@ -679,10 +664,10 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
         }
 
         // calculate the innovation consistency test ratio
-        flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]);
+        flowTestRatio[obsIndex] = sq(flowInnov[obsIndex]) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * flowVarInnov[obsIndex]);
 
         // Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
-        if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {
+        if (really_fuse && (flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {
             // record the last time observations were accepted for fusion
             prevFlowFuseTime_ms = imuSampleTime_ms;
             // notify first time only
@@ -693,16 +678,16 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
             // correct the covariance P = (I - K*H)*P
             // take advantage of the empty columns in KH to reduce the
             // number of operations
-            for (unsigned i = 0; i<=stateIndexLim; i++) {
-                for (unsigned j = 0; j<=6; j++) {
+            for (uint8_t i = 0; i<=stateIndexLim; i++) {
+                for (uint8_t j = 0; j<=6; j++) {
                     KH[i][j] = Kfusion[i] * H_LOS[j];
                 }
-                for (unsigned j = 7; j<=stateIndexLim; j++) {
+                for (uint8_t j = 7; j<=stateIndexLim; j++) {
                     KH[i][j] = 0.0f;
                 }
             }
-            for (unsigned j = 0; j<=stateIndexLim; j++) {
-                for (unsigned i = 0; i<=stateIndexLim; i++) {
+            for (uint8_t j = 0; j<=stateIndexLim; j++) {
+                for (uint8_t i = 0; i<=stateIndexLim; i++) {
                     ftype res = 0;
                     res += KH[i][0] * P[0][j];
                     res += KH[i][1] * P[1][j];
@@ -737,7 +722,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
 
                 // correct the state vector
                 for (uint8_t j= 0; j<=stateIndexLim; j++) {
-                    statesArray[j] = statesArray[j] - Kfusion[j] * innovOptFlow[obsIndex];
+                    statesArray[j] = statesArray[j] - Kfusion[j] * flowInnov[obsIndex];
                 }
                 stateStruct.quat.normalize();
 

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -477,6 +477,18 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
         variances = extNavVelVarInnov.tofloat();
         return true;
 #endif // EK3_FEATURE_EXTERNAL_NAV
+    case AP_NavEKF_Source::SourceXY::OPTFLOW:
+        // check for timeouts
+        if (AP_HAL::millis() - flowInnovTime_ms > 500) {
+            return false;
+        }
+        innovations.x = flowInnov[0];
+        innovations.y = flowInnov[1];
+        innovations.z = 0;
+        variances.x = flowVarInnov[0];
+        variances.y = flowVarInnov[1];
+        variances.z = 0;
+        return true;
     default:
         // variances are not available for this source
         return false;

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -819,7 +819,8 @@ class NavEKF3_core : public NavEKF_core_common
     void EstimateTerrainOffset(const of_elements &ofDataDelayed);
 
     // fuse optical flow measurements into the main filter
-    void FuseOptFlow(const of_elements &ofDataDelayed);
+    // really_fuse should be true to actually fuse into the main filter, false to only calculate variances
+    void FuseOptFlow(const of_elements &ofDataDelayed, bool really_fuse);
 
     // Control filter mode changes
     void controlFilterModes();
@@ -1163,22 +1164,22 @@ class NavEKF3_core : public NavEKF_core_common
 
     // variables added for optical flow fusion
     EKF_obs_buffer_t<of_elements> storedOF;    // OF data buffer
-    of_elements ofDataNew;          // OF data at the current time horizon
     bool flowDataValid;             // true while optical flow data is still fresh
     Vector2F auxFlowObsInnov;       // optical flow rate innovation from 1-state terrain offset estimator
     uint32_t flowValidMeaTime_ms;   // time stamp from latest valid flow measurement (msec)
     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)
-    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)
+    Vector2 flowVarInnov;           // optical flow innovations variances (rad/sec)^2
+    Vector2 flowInnov;              // optical flow LOS innovations (rad/sec)
+    uint32_t flowInnovTime_ms;      // system time that optical flow innovations and variances were recorded (to detect timeouts)
     ftype Popt;                     // Optical flow terrain height state covariance (m^2)
     ftype terrainState;             // terrain position state (m)
     ftype prevPosN;                 // north position at last measurement
     ftype prevPosE;                 // east position at last measurement
     ftype varInnovRng;              // range finder observation innovation variance (m^2)
     ftype innovRng;                 // range finder observation innovation (m)
+
     ftype hgtMea;                   // height measurement derived from either baro, gps or range finder data (m)
     bool inhibitGndState;           // true when the terrain position state is to remain constant
     uint32_t prevFlowFuseTime_ms;   // time both flow measurement components passed their innovation consistency checks

libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp

@@ -86,12 +86,17 @@ void AP_OpticalFlow_SITL::update(void)
     // correct relative velocity for rotation rates and sensor offset
     relVelSensor += gyro % posRelSensorBF;
 
+    // scaling based on parameters
+    const Vector2f flowScaler = _flowScaler();
+    const float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
+    const float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
+
     // Divide velocity by range and add body rates to get predicted sensed angular
     // optical rates relative to X and Y sensor axes assuming no misalignment or scale
     // factor error. Note - these are instantaneous values. The sensor sums these values across the interval from the last
     // poll to provide a delta angle across the interface
-    state.flowRate.x =  -relVelSensor.y/range + gyro.x + _sitl->flow_noise * rand_float();
-    state.flowRate.y =   relVelSensor.x/range + gyro.y + _sitl->flow_noise * rand_float();
+    state.flowRate.x = (-relVelSensor.y/range + gyro.x + _sitl->flow_noise * rand_float()) * flowScaleFactorX;
+    state.flowRate.y =  (relVelSensor.x/range + gyro.y + _sitl->flow_noise * rand_float()) * flowScaleFactorY;
 
     // The flow sensors body rates are assumed to be the same as the vehicle body rates (ie no misalignment)
     // Note - these are instantaneous values. The sensor sums these values across the interval from the last