AP_NavEKF2: Reduce effect of rounding errors on covariance prediction

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -393,7 +393,7 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
     gyroBiasNoiseScaler(2.0f),      // scale factor applied to imu gyro bias learning before the vehicle is armed
     hgtAvg_ms(100),                 // average number of msec between height measurements
     betaAvg_ms(100),                // average number of msec between synthetic sideslip measurements
-    covTimeStepMax(0.07f),          // maximum time (sec) between covariance prediction updates
+    covTimeStepMax(0.1f),           // maximum time (sec) between covariance prediction updates
     covDelAngMax(0.05f),            // maximum delta angle between covariance prediction updates
     DCM33FlowMin(0.71f),            // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
     fScaleFactorPnoise(1e-10f),     // Process noise added to focal length scale factor state variance at each time step
@@ -401,7 +401,8 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
     flowIntervalMax_ms(100),        // maximum allowable time between flow fusion events
     gndEffectTimeout_ms(1000),      // time in msec that baro ground effect compensation will timeout after initiation
     gndEffectBaroScaler(4.0f),      // scaler applied to the barometer observation variance when operating in ground effect
-    gndGradientSigma(2)             // RMS terrain gradient percentage assumed by the terrain height estimation
+    gndGradientSigma(2),            // RMS terrain gradient percentage assumed by the terrain height estimation
+    fusionTimeStep_ms(20)           // The nominal number of msec between covariance prediction and fusion operations
 {
     AP_Param::setup_object_defaults(this, var_info);
 }

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -297,6 +297,7 @@ private:
     const uint16_t gndEffectTimeout_ms; // time in msec that ground effect mode is active after being activated
     const float gndEffectBaroScaler;    // scaler applied to the barometer observation variance when ground effect mode is active
     const uint8_t gndGradientSigma;     // RMS terrain gradient percentage assumed by the terrain height estimation
+    const uint8_t fusionTimeStep_ms;    // The nominal time interval between covariance predictions and measurement fusions in msec
 };
 
 #endif //AP_NavEKF2

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -118,6 +118,9 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
         flowValidMeaTime_ms = imuSampleTime_ms;
         // estimate sample time of the measurement
         ofDataNew.time_ms = imuSampleTime_ms - frontend._flowDelay_ms - frontend.flowTimeDeltaAvg_ms/2;
+        // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
+        // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
+        ofDataNew.time_ms = roundToNearest(ofDataNew.time_ms, frontend.fusionTimeStep_ms);
         // Save data to buffer
         StoreOF();
         // Check for data at the fusion time horizon
@@ -193,6 +196,10 @@ void NavEKF2_core::readMagData()
         // estimate of time magnetometer measurement was taken, allowing for delays
         magMeasTime_ms = imuSampleTime_ms - frontend.magDelay_ms;
 
+        // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
+        // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
+        magMeasTime_ms = roundToNearest(magMeasTime_ms, frontend.fusionTimeStep_ms);
+
         // read compass data and scale to improve numerical conditioning
         magDataNew.mag = _ahrs->get_compass()->get_field() * 0.001f;
 
@@ -358,6 +365,10 @@ void NavEKF2_core::readGpsData()
             // ideally we should be using a timing signal from the GPS receiver to set this time
             gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend._gpsDelay_ms;
 
+            // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
+            // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
+            gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend.fusionTimeStep_ms);
+
             // read the NED velocity from the GPS
             gpsDataNew.vel = _ahrs->get_gps().velocity();
 
@@ -495,6 +506,9 @@ void NavEKF2_core::readGpsData()
             gpsDataNew.pos.x = lastKnownPositionNE.x;
             gpsDataNew.pos.y = lastKnownPositionNE.y;
             gpsDataNew.time_ms = imuSampleTime_ms-frontend._gpsDelay_ms;
+            // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
+            // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
+            gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend.fusionTimeStep_ms);
             // save measurement to buffer to be fused later
             StoreGPS();
         }
@@ -633,6 +647,10 @@ void NavEKF2_core::readHgtData()
         // estimate of time height measurement was taken, allowing for delays
         hgtMeasTime_ms = lastHgtReceived_ms - frontend._hgtDelay_ms;
 
+        // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
+        // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
+        hgtMeasTime_ms = roundToNearest(hgtMeasTime_ms, frontend.fusionTimeStep_ms);
+
         // save baro measurement to buffer to be fused later
         baroDataNew.time_ms = hgtMeasTime_ms;
         StoreBaro();
@@ -696,6 +714,9 @@ void NavEKF2_core::readAirSpdData()
         tasDataNew.tas = aspeed->get_airspeed() * aspeed->get_EAS2TAS();
         timeTasReceived_ms = aspeed->last_update_ms();
         tasDataNew.time_ms = timeTasReceived_ms - frontend.tasDelay_ms;
+        // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
+        // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
+        tasDataNew.time_ms = roundToNearest(tasDataNew.time_ms, frontend.fusionTimeStep_ms);
         newDataTas = true;
         StoreTAS();
         RecallTAS();
@@ -704,4 +725,10 @@ void NavEKF2_core::readAirSpdData()
     }
 }
 
+// Round to the nearest multiple of a integer
+uint32_t NavEKF2_core::roundToNearest(uint32_t dividend, uint32_t divisor )
+{
+  return ((uint32_t)round((float)dividend/float(divisor)))*divisor;
+}
+
 #endif // HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -593,6 +593,9 @@ private:
     // using a simple observer
     void calcOutputStatesFast();
 
+    // Round to the nearest multiple of a integer
+    uint32_t roundToNearest(uint32_t dividend, uint32_t divisor );
+
     // measurement buffer sizes
     static const uint32_t IMU_BUFFER_LENGTH = 100;  // number of IMU samples stored in the buffer. Samples*delta_time must be > max sensor delay
     static const uint32_t OBS_BUFFER_LENGTH = 5;    // number of non-IMU sensor samples stored in the buffer.