diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index cd6420c8b7..08f6d710a2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/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); } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 057d28e2f0..fdd108c6a3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index cff45dc290..ca692bcb83 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 5b5e2daf78..60ea18570b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/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.