From f2f6e0e80843f38160df3e6fcf7143289bc5739a Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 8 Feb 2026 23:52:40 +0000 Subject: [PATCH 1/5] improve pos est sensor corrections --- .../navigation/navigation_pos_estimator.c | 115 +++++++++++------- .../navigation_pos_estimator_flow.c | 26 ++-- .../navigation_pos_estimator_private.h | 5 + src/main/sensors/opflow.c | 1 + src/main/sensors/opflow.h | 1 + 5 files changed, 95 insertions(+), 53 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index cd861dacd6c..0898ce3bb6c 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -228,6 +228,7 @@ void onNewGPSData(void) /* If not the first update - calculate velocities */ if (!isFirstGPSUpdate) { float dT = US2S(getGPSDeltaTimeFilter(currentTimeUs - lastGPSNewDataTime)); + posEstimator.gps.updateDt = dT; /* Use VELNED provided by GPS if available, calculate from coordinates otherwise */ float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f); @@ -299,6 +300,8 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) posEstimator.baro.lastUpdateTime = currentTimeUs; if (baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) { + posEstimator.baro.updateDt = US2S(baroDtUs); + posEstimator.baro.alt = pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, US2S(baroDtUs)); // baro altitude rate @@ -576,6 +579,12 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } if (ctx->newFlags & EST_BARO_VALID && wBaro) { + if (!posEstimator.baro.updateDt) { + return true; + } + const float dt = posEstimator.baro.updateDt; + const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; + bool isAirCushionEffectDetected = false; static float baroGroundAlt = 0.0f; @@ -609,14 +618,14 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; - ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt; - ctx->estVelCorr.z += baroVelZResidual * w_z_baro_v * ctx->dt; + ctx->estPosCorr.z = constrainf(baroAltResidual * w_z_baro_p * dt, -corrLimit, corrLimit); + ctx->estVelCorr.z = constrainf(baroVelZResidual * w_z_baro_v * dt, -corrLimit, corrLimit); - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); // Accelerometer bias if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z += (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); + ctx->accBiasCorr.z = dt * (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed @@ -629,19 +638,23 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->estVelCorr.z += posEstimator.gps.vel.z - posEstimator.est.vel.z; ctx->newEPV = posEstimator.gps.epv; } - else { + else if (posEstimator.gps.updateDt) { + const float dt = posEstimator.gps.updateDt; + const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; + // Altitude const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; const float w_z_gps_v = positionEstimationConfig()->w_z_gps_v; - ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt; - ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * ctx->dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); + ctx->estPosCorr.z += constrainf(gpsAltResidual * w_z_gps_p * dt, -corrLimit, corrLimit); + ctx->estVelCorr.z += constrainf(gpsVelZResidual * w_z_gps_v * dt, -corrLimit, corrLimit); + + ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); // Accelerometer bias - ctx->accBiasCorr.z += (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v)); + ctx->accBiasCorr.z += dt * (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v)); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed @@ -652,7 +665,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->estVelCorr.z *= 2.0f / (wGps + wBaro); ctx->accBiasCorr.z *= 2.0f / (wGps + wBaro); - return correctOK; + return ctx->applyCorrections = correctOK; } static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) @@ -666,7 +679,10 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) ctx->estVelCorr.y += posEstimator.gps.vel.y - posEstimator.est.vel.y; ctx->newEPH = posEstimator.gps.eph; } - else { + else if (posEstimator.gps.updateDt) { + const float dt = posEstimator.gps.updateDt; + const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; + const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x; const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y; const float gpsVelXResidual = posEstimator.gps.vel.x - posEstimator.est.vel.x; @@ -680,19 +696,21 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) const float w_xy_gps_v = positionEstimationConfig()->w_xy_gps_v * sq(gpsWeightScaler); // Coordinates - ctx->estPosCorr.x += gpsPosXResidual * w_xy_gps_p * ctx->dt; - ctx->estPosCorr.y += gpsPosYResidual * w_xy_gps_p * ctx->dt; + ctx->estPosCorr.x = constrainf(gpsPosXResidual * w_xy_gps_p * dt, -corrLimit, corrLimit); + ctx->estPosCorr.y = constrainf(gpsPosYResidual * w_xy_gps_p * dt, -corrLimit, corrLimit); // Velocity from direct measurement - ctx->estVelCorr.x += gpsVelXResidual * w_xy_gps_v * ctx->dt; - ctx->estVelCorr.y += gpsVelYResidual * w_xy_gps_v * ctx->dt; + ctx->estVelCorr.x = constrainf(gpsVelXResidual * w_xy_gps_v * dt, -corrLimit, corrLimit); + ctx->estVelCorr.y = constrainf(gpsVelYResidual * w_xy_gps_v * dt, -corrLimit, corrLimit); // Accelerometer bias - ctx->accBiasCorr.x += (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v)); - ctx->accBiasCorr.y += (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v)); + ctx->accBiasCorr.x = dt * (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v)); + ctx->accBiasCorr.y = dt * (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v)); /* Adjust EPH */ - ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p); + ctx->newEPH = updateEPE(posEstimator.est.eph, dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p); + + ctx->applyCorrections = true; } return true; @@ -736,9 +754,10 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) return; } - /* Calculate new EPH and EPV for the case we didn't update position */ - ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); - ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); + /* Calculate new EPH and EPV for the case we didn't update position - linear degradation in around 10s */ + ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); + ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); + ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs); vectorZero(&ctx.estPosCorr); vectorZero(&ctx.estVelCorr); @@ -751,43 +770,49 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) estimationPredict(&ctx); /* Correction stage: Z */ - const bool estZCorrectOk = - estimationCalculateCorrection_Z(&ctx); + const bool estZCorrectOk = estimationCalculateCorrection_Z(&ctx); /* Correction stage: XY: GPS, FLOW */ // FIXME: Handle transition from FLOW to GPS and back - seamlessly fly indoor/outdoor - const bool estXYCorrectOk = - estimationCalculateCorrection_XY_GPS(&ctx) || - estimationCalculateCorrection_XY_FLOW(&ctx); + const bool estXYCorrectOk = estimationCalculateCorrection_XY_GPS(&ctx) || estimationCalculateCorrection_XY_FLOW(&ctx); // If we can't apply correction or accuracy is off the charts - decay velocity to zero if (!estXYCorrectOk || ctx.newEPH > max_eph_epv) { ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt; ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt; + ctx.applyCorrections = true; } if (!estZCorrectOk || ctx.newEPV > max_eph_epv) { ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt; + ctx.applyCorrections = true; } - // Boost the corrections based on accWeight - vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor); - vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor); - - // Apply corrections - vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr); - vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); - - /* Correct accelerometer bias */ - const float w_acc_bias = positionEstimationConfig()->w_acc_bias; - if (w_acc_bias > 0.0f) { - /* Correct accel bias */ - posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt; - posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt; - posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt; - - posEstimator.imu.accelBias.x = constrainf(posEstimator.imu.accelBias.x, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); - posEstimator.imu.accelBias.y = constrainf(posEstimator.imu.accelBias.y, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); - posEstimator.imu.accelBias.z = constrainf(posEstimator.imu.accelBias.z, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); + + if (ctx.applyCorrections) { + posEstimator.gps.updateDt = 0.0f; + posEstimator.baro.updateDt = 0.0f; + posEstimator.flow.updateDt = 0.0f; + + // Boost the corrections based on accWeight + vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor); + vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor); + + // Apply corrections + vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr); + vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); + + /* Correct accelerometer bias */ + const float w_acc_bias = positionEstimationConfig()->w_acc_bias; + if (w_acc_bias > 0.0f) { + /* Correct accel bias */ + posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias; + posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias; + posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias; + + posEstimator.imu.accelBias.x = constrainf(posEstimator.imu.accelBias.x, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); + posEstimator.imu.accelBias.y = constrainf(posEstimator.imu.accelBias.y, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); + posEstimator.imu.accelBias.z = constrainf(posEstimator.imu.accelBias.z, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); + } } /* Update ground course */ diff --git a/src/main/navigation/navigation_pos_estimator_flow.c b/src/main/navigation/navigation_pos_estimator_flow.c index 8656c4919f4..d192ec1dd19 100644 --- a/src/main/navigation/navigation_pos_estimator_flow.c +++ b/src/main/navigation/navigation_pos_estimator_flow.c @@ -73,6 +73,14 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) return false; } + if (!opflow.updateDt) { + return true; + } + posEstimator.flow.updateDt = opflow.updateDt; + + const float dt = posEstimator.flow.updateDt; + const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; + // Calculate linear velocity based on angular velocity and altitude // Technically we should calculate arc length here, but for fast sampling this is accurate enough fpVector3_t flowVel = { @@ -88,8 +96,9 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) const float flowVelXInnov = flowVel.x - posEstimator.est.vel.x; const float flowVelYInnov = flowVel.y - posEstimator.est.vel.y; - ctx->estVelCorr.x = flowVelXInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt; - ctx->estVelCorr.y = flowVelYInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt; + const float w_xy_flow_v = positionEstimationConfig()->w_xy_flow_v; + ctx->estVelCorr.x = constrainf(flowVelXInnov * w_xy_flow_v * dt, -corrLimit, corrLimit); + ctx->estVelCorr.y = constrainf(flowVelYInnov * w_xy_flow_v * dt, -corrLimit, corrLimit); // Calculate position correction if possible/allowed if ((ctx->newFlags & EST_GPS_XY_VALID)) { @@ -98,16 +107,17 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) posEstimator.est.flowCoordinates[Y] = posEstimator.gps.pos.y; } else if (positionEstimationConfig()->allow_dead_reckoning) { - posEstimator.est.flowCoordinates[X] += flowVel.x * ctx->dt; - posEstimator.est.flowCoordinates[Y] += flowVel.y * ctx->dt; + posEstimator.est.flowCoordinates[X] += flowVel.x * dt; + posEstimator.est.flowCoordinates[Y] += flowVel.y * dt; const float flowResidualX = posEstimator.est.flowCoordinates[X] - posEstimator.est.pos.x; const float flowResidualY = posEstimator.est.flowCoordinates[Y] - posEstimator.est.pos.y; - ctx->estPosCorr.x = flowResidualX * positionEstimationConfig()->w_xy_flow_p * ctx->dt; - ctx->estPosCorr.y = flowResidualY * positionEstimationConfig()->w_xy_flow_p * ctx->dt; + const float w_xy_flow_p = positionEstimationConfig()->w_xy_flow_p; + ctx->estPosCorr.x = constrainf(flowResidualX * w_xy_flow_p * dt, -corrLimit, corrLimit); + ctx->estPosCorr.y = constrainf(flowResidualY * w_xy_flow_p * dt, -corrLimit, corrLimit); - ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, calc_length_pythagorean_2D(flowResidualX, flowResidualY), positionEstimationConfig()->w_xy_flow_p); + ctx->newEPH = updateEPE(posEstimator.est.eph, dt, calc_length_pythagorean_2D(flowResidualX, flowResidualY), w_xy_flow_p); } DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X])); @@ -115,7 +125,7 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) DEBUG_SET(DEBUG_FLOW, 2, posEstimator.est.flowCoordinates[X]); DEBUG_SET(DEBUG_FLOW, 3, posEstimator.est.flowCoordinates[Y]); - return true; + return ctx->applyCorrections = true; #else UNUSED(ctx); return false; diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index abb389bddcb..6f4c738e10d 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -33,6 +33,7 @@ #define INAV_GPS_ACCEPTANCE_EPE 500.0f // 5m acceptance radius #define INAV_ACC_BIAS_ACCEPTANCE_VALUE (GRAVITY_CMSS * 0.25f) // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway +#define INAV_EST_CORR_LIMIT_VALUE 4000.0f #define INAV_GPS_GLITCH_RADIUS 250.0f // 2.5m GPS glitch radius #define INAV_GPS_GLITCH_ACCEL 1000.0f // 10m/s/s max possible acceleration for GPS glitch detection @@ -69,6 +70,7 @@ typedef struct { fpVector3_t vel; // GPS velocity (cms) float eph; float epv; + float updateDt; } navPositionEstimatorGPS_t; typedef struct { @@ -77,6 +79,7 @@ typedef struct { float alt; // Raw barometric altitude (cm) float epv; float baroAltRate; // Baro altitude rate of change (cm/s) + float updateDt; } navPositionEstimatorBARO_t; typedef struct { @@ -103,6 +106,7 @@ typedef struct { float quality; float flowRate[2]; float bodyRate[2]; + float updateDt; } navPositionEstimatorFLOW_t; typedef struct { @@ -178,6 +182,7 @@ typedef struct { fpVector3_t estPosCorr; fpVector3_t estVelCorr; fpVector3_t accBiasCorr; + bool applyCorrections; } estimationContext_t; extern navigationPosEstimator_t posEstimator; diff --git a/src/main/sensors/opflow.c b/src/main/sensors/opflow.c index f82d8f16a58..024c5d403cf 100755 --- a/src/main/sensors/opflow.c +++ b/src/main/sensors/opflow.c @@ -171,6 +171,7 @@ void opflowUpdate(timeUs_t currentTimeUs) if (opflow.dev.updateFn(&opflow.dev)) { // Indicate valid update opflow.isHwHealty = true; + opflow.updateDt = US2S(currentTimeUs - opflow.lastValidUpdate); opflow.lastValidUpdate = currentTimeUs; opflow.rawQuality = opflow.dev.rawData.quality; diff --git a/src/main/sensors/opflow.h b/src/main/sensors/opflow.h index afe94ec79ac..a2f82745816 100755 --- a/src/main/sensors/opflow.h +++ b/src/main/sensors/opflow.h @@ -53,6 +53,7 @@ typedef struct opflow_s { opflowQuality_e flowQuality; timeUs_t lastValidUpdate; + float updateDt; bool isHwHealty; float flowRate[2]; // optical flow angular rate in rad/sec measured about the X and Y body axis float bodyRate[2]; // body inertial angular rate in rad/sec measured about the X and Y body axis From 13e56b74952a2c062c829eb8d2a4c411567c213e Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 9 Feb 2026 14:14:54 +0000 Subject: [PATCH 2/5] fixes and improvements --- .../navigation/navigation_pos_estimator.c | 47 +++++++++++-------- .../navigation_pos_estimator_flow.c | 11 ++--- 2 files changed, 32 insertions(+), 26 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 0898ce3bb6c..6b275d1809c 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -300,13 +300,14 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) posEstimator.baro.lastUpdateTime = currentTimeUs; if (baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) { - posEstimator.baro.updateDt = US2S(baroDtUs); + const float baroDtSec = US2S(baroDtUs); + posEstimator.baro.updateDt = baroDtSec; - posEstimator.baro.alt = pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, US2S(baroDtUs)); + posEstimator.baro.alt = pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, baroDtSec); // baro altitude rate static float baroAltPrevious = 0; - posEstimator.baro.baroAltRate = (posEstimator.baro.alt - baroAltPrevious) / US2S(baroDtUs); + posEstimator.baro.baroAltRate = (posEstimator.baro.alt - baroAltPrevious) / baroDtSec; baroAltPrevious = posEstimator.baro.alt; updateBaroAltitudeRate(posEstimator.baro.baroAltRate); } @@ -583,7 +584,6 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) return true; } const float dt = posEstimator.baro.updateDt; - const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; bool isAirCushionEffectDetected = false; static float baroGroundAlt = 0.0f; @@ -618,8 +618,8 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; - ctx->estPosCorr.z = constrainf(baroAltResidual * w_z_baro_p * dt, -corrLimit, corrLimit); - ctx->estVelCorr.z = constrainf(baroVelZResidual * w_z_baro_v * dt, -corrLimit, corrLimit); + ctx->estPosCorr.z = baroAltResidual * w_z_baro_p * dt; + ctx->estVelCorr.z = baroVelZResidual * w_z_baro_v * dt; ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); @@ -640,7 +640,6 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } else if (posEstimator.gps.updateDt) { const float dt = posEstimator.gps.updateDt; - const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; // Altitude const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); @@ -648,8 +647,8 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; const float w_z_gps_v = positionEstimationConfig()->w_z_gps_v; - ctx->estPosCorr.z += constrainf(gpsAltResidual * w_z_gps_p * dt, -corrLimit, corrLimit); - ctx->estVelCorr.z += constrainf(gpsVelZResidual * w_z_gps_v * dt, -corrLimit, corrLimit); + ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * dt; + ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * dt; ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); @@ -681,7 +680,6 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) } else if (posEstimator.gps.updateDt) { const float dt = posEstimator.gps.updateDt; - const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x; const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y; @@ -696,12 +694,12 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) const float w_xy_gps_v = positionEstimationConfig()->w_xy_gps_v * sq(gpsWeightScaler); // Coordinates - ctx->estPosCorr.x = constrainf(gpsPosXResidual * w_xy_gps_p * dt, -corrLimit, corrLimit); - ctx->estPosCorr.y = constrainf(gpsPosYResidual * w_xy_gps_p * dt, -corrLimit, corrLimit); + ctx->estPosCorr.x = gpsPosXResidual * w_xy_gps_p * dt; + ctx->estPosCorr.y = gpsPosYResidual * w_xy_gps_p * dt; // Velocity from direct measurement - ctx->estVelCorr.x = constrainf(gpsVelXResidual * w_xy_gps_v * dt, -corrLimit, corrLimit); - ctx->estVelCorr.y = constrainf(gpsVelYResidual * w_xy_gps_v * dt, -corrLimit, corrLimit); + ctx->estVelCorr.x = gpsVelXResidual * w_xy_gps_v * dt; + ctx->estVelCorr.y = gpsVelYResidual * w_xy_gps_v * dt; // Accelerometer bias ctx->accBiasCorr.x = dt * (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v)); @@ -754,7 +752,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) return; } - /* Calculate new EPH and EPV for the case we didn't update position - linear degradation in around 10s */ + /* Calculate new EPH and EPV for the case we didn't update position - linear degradation in max 10s */ ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); @@ -778,17 +776,19 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) // If we can't apply correction or accuracy is off the charts - decay velocity to zero if (!estXYCorrectOk || ctx.newEPH > max_eph_epv) { - ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt; - ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt; - ctx.applyCorrections = true; + const float w_xy_res_v = positionEstimationConfig()->w_xy_res_v; + posEstimator.est.vel.x -= posEstimator.est.vel.x * w_xy_res_v * ctx.dt; + posEstimator.est.vel.y -= posEstimator.est.vel.y * w_xy_res_v * ctx.dt; } if (!estZCorrectOk || ctx.newEPV > max_eph_epv) { - ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt; - ctx.applyCorrections = true; + posEstimator.est.vel.z -= posEstimator.est.vel.z * positionEstimationConfig()->w_z_res_v * ctx.dt; } if (ctx.applyCorrections) { + float maxUpdateDt = MAX(posEstimator.gps.updateDt, posEstimator.baro.updateDt); + maxUpdateDt = MAX(maxUpdateDt, posEstimator.flow.updateDt); + posEstimator.gps.updateDt = 0.0f; posEstimator.baro.updateDt = 0.0f; posEstimator.flow.updateDt = 0.0f; @@ -797,6 +797,13 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor); vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor); + // Constrain corrections to prevent instability + float correctionLimit = INAV_EST_CORR_LIMIT_VALUE * maxUpdateDt; + for (uint8_t axis = 0; axis < 3; axis++) { + ctx.estPosCorr.v[axis] = constrainf(ctx.estPosCorr.v[axis], -correctionLimit, correctionLimit); + ctx.estVelCorr.v[axis] = constrainf(ctx.estVelCorr.v[axis], -correctionLimit, correctionLimit); + } + // Apply corrections vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr); vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); diff --git a/src/main/navigation/navigation_pos_estimator_flow.c b/src/main/navigation/navigation_pos_estimator_flow.c index d192ec1dd19..36325f5dc1a 100644 --- a/src/main/navigation/navigation_pos_estimator_flow.c +++ b/src/main/navigation/navigation_pos_estimator_flow.c @@ -77,9 +77,8 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) return true; } posEstimator.flow.updateDt = opflow.updateDt; - + opflow.updateDt = 0.0f; const float dt = posEstimator.flow.updateDt; - const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; // Calculate linear velocity based on angular velocity and altitude // Technically we should calculate arc length here, but for fast sampling this is accurate enough @@ -97,8 +96,8 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) const float flowVelYInnov = flowVel.y - posEstimator.est.vel.y; const float w_xy_flow_v = positionEstimationConfig()->w_xy_flow_v; - ctx->estVelCorr.x = constrainf(flowVelXInnov * w_xy_flow_v * dt, -corrLimit, corrLimit); - ctx->estVelCorr.y = constrainf(flowVelYInnov * w_xy_flow_v * dt, -corrLimit, corrLimit); + ctx->estVelCorr.x = flowVelXInnov * w_xy_flow_v * dt; + ctx->estVelCorr.y = flowVelYInnov * w_xy_flow_v * dt; // Calculate position correction if possible/allowed if ((ctx->newFlags & EST_GPS_XY_VALID)) { @@ -114,8 +113,8 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) const float flowResidualY = posEstimator.est.flowCoordinates[Y] - posEstimator.est.pos.y; const float w_xy_flow_p = positionEstimationConfig()->w_xy_flow_p; - ctx->estPosCorr.x = constrainf(flowResidualX * w_xy_flow_p * dt, -corrLimit, corrLimit); - ctx->estPosCorr.y = constrainf(flowResidualY * w_xy_flow_p * dt, -corrLimit, corrLimit); + ctx->estPosCorr.x = flowResidualX * w_xy_flow_p * dt; + ctx->estPosCorr.y = flowResidualY * w_xy_flow_p * dt; ctx->newEPH = updateEPE(posEstimator.est.eph, dt, calc_length_pythagorean_2D(flowResidualX, flowResidualY), w_xy_flow_p); } From 1c39cd89da1075be57f303df973c612e4e30b36c Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 10 Feb 2026 10:59:07 +0000 Subject: [PATCH 3/5] Update navigation_pos_estimator.c --- .../navigation/navigation_pos_estimator.c | 81 ++++++++++--------- 1 file changed, 42 insertions(+), 39 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 43bd57acad7..b0f8bba4c8c 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -580,52 +580,53 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } if (ctx->newFlags & EST_BARO_VALID && wBaro) { - if (!posEstimator.baro.updateDt) { - return true; - } - const float dt = posEstimator.baro.updateDt; + if (posEstimator.baro.updateDt) { + ctx->applyCorrections = true; + const float dt = posEstimator.baro.updateDt; - bool isAirCushionEffectDetected = false; - static float baroGroundAlt = 0.0f; + bool isAirCushionEffectDetected = false; + static float baroGroundAlt = 0.0f; - if (STATE(MULTIROTOR)) { - static bool isBaroGroundValid = false; + if (STATE(MULTIROTOR)) { + static bool isBaroGroundValid = false; - if (!ARMING_FLAG(ARMED)) { - baroGroundAlt = posEstimator.baro.alt; - isBaroGroundValid = true; - } - else if (isBaroGroundValid) { - // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it - if (isMulticopterThrottleAboveMidHover()) { - // Disable ground effect detection at lift off when est alt and baro alt converge. Always disable if baro alt > 1m. - isBaroGroundValid = fabsf(posEstimator.est.pos.z - posEstimator.baro.alt) > 20.0f && posEstimator.baro.alt < 100.0f; + if (!ARMING_FLAG(ARMED)) { + baroGroundAlt = posEstimator.baro.alt; + isBaroGroundValid = true; + } + else if (isBaroGroundValid) { + // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it + if (isMulticopterThrottleAboveMidHover()) { + // Disable ground effect detection at lift off when est alt and baro alt converge. Always disable if baro alt > 1m. + isBaroGroundValid = fabsf(posEstimator.est.pos.z - posEstimator.baro.alt) > 20.0f && posEstimator.baro.alt < 100.0f; + } + + isAirCushionEffectDetected = (isEstimatedAglTrusted() && posEstimator.surface.alt < 20.0f) || + posEstimator.baro.alt < baroGroundAlt + 20.0f; } - - isAirCushionEffectDetected = (isEstimatedAglTrusted() && posEstimator.surface.alt < 20.0f) || posEstimator.baro.alt < baroGroundAlt + 20.0f; } - } - // Altitude - float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z); + // Altitude + float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z); - // Disable alt pos correction at point of lift off if ground effect active - if (isAirCushionEffectDetected && isMulticopterThrottleAboveMidHover()) { - baroAltResidual = 0.0f; - } + // Disable alt pos correction at point of lift off if ground effect active + if (isAirCushionEffectDetected && isMulticopterThrottleAboveMidHover()) { + baroAltResidual = 0.0f; + } - const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z); - const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; - const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; + const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z); + const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; + const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; - ctx->estPosCorr.z = baroAltResidual * w_z_baro_p * dt; - ctx->estVelCorr.z = baroVelZResidual * w_z_baro_v * dt; + ctx->estPosCorr.z = baroAltResidual * w_z_baro_p * dt; + ctx->estVelCorr.z = baroVelZResidual * w_z_baro_v * dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); - // Accelerometer bias - if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z = dt * (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); + // Accelerometer bias + if (!isAirCushionEffectDetected) { + ctx->accBiasCorr.z = dt * (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); + } } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed @@ -639,6 +640,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->newEPV = posEstimator.gps.epv; } else if (posEstimator.gps.updateDt) { + ctx->applyCorrections = true; const float dt = posEstimator.gps.updateDt; // Altitude @@ -650,7 +652,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * dt; ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); + ctx->newEPV = updateEPE(ctx->newEPV, dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); // Accelerometer bias ctx->accBiasCorr.z += dt * (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v)); @@ -664,7 +666,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->estVelCorr.z *= 2.0f / (wGps + wBaro); ctx->accBiasCorr.z *= 2.0f / (wGps + wBaro); - return ctx->applyCorrections = correctOK; + return correctOK; } static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) @@ -679,6 +681,7 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) ctx->newEPH = posEstimator.gps.eph; } else if (posEstimator.gps.updateDt) { + ctx->applyCorrections = true; const float dt = posEstimator.gps.updateDt; const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x; @@ -707,8 +710,6 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) /* Adjust EPH */ ctx->newEPH = updateEPE(posEstimator.est.eph, dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p); - - ctx->applyCorrections = true; } return true; @@ -786,6 +787,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) } if (ctx.applyCorrections) { + ctx.applyCorrections = false; + float maxUpdateDt = MAX(posEstimator.gps.updateDt, posEstimator.baro.updateDt); maxUpdateDt = MAX(maxUpdateDt, posEstimator.flow.updateDt); From ffd4b9ebcf0776af0df5bf3b5dc3a58ea1751a04 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 10 Feb 2026 23:23:41 +0000 Subject: [PATCH 4/5] Update navigation_pos_estimator.c --- .../navigation/navigation_pos_estimator.c | 92 ++++++++++--------- 1 file changed, 49 insertions(+), 43 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index b0f8bba4c8c..cdfb3c87ed6 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -580,9 +580,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } if (ctx->newFlags & EST_BARO_VALID && wBaro) { - if (posEstimator.baro.updateDt) { + if (posEstimator.baro.updateDt) { // only update corrections once every sensor update ctx->applyCorrections = true; - const float dt = posEstimator.baro.updateDt; + const float dT = posEstimator.baro.updateDt; bool isAirCushionEffectDetected = false; static float baroGroundAlt = 0.0f; @@ -618,14 +618,14 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; - ctx->estPosCorr.z = baroAltResidual * w_z_baro_p * dt; - ctx->estVelCorr.z = baroVelZResidual * w_z_baro_v * dt; + ctx->estPosCorr.z = baroAltResidual * w_z_baro_p * dT; + ctx->estVelCorr.z = baroVelZResidual * w_z_baro_v * dT; - ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, dT, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); // Accelerometer bias if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z = dt * (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); + ctx->accBiasCorr.z = dT * (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); } } @@ -633,29 +633,31 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } if (ctx->newFlags & EST_GPS_Z_VALID && (wGps || !(ctx->newFlags & EST_Z_VALID))) { - // Reset current estimate to GPS altitude if estimate not valid (used for GPS and Baro) - if (!(ctx->newFlags & EST_Z_VALID)) { - posEstimator.est.pos.z = posEstimator.gps.pos.z; - posEstimator.est.vel.z = posEstimator.gps.vel.z; - ctx->newEPV = posEstimator.gps.epv; - } - else if (posEstimator.gps.updateDt) { - ctx->applyCorrections = true; - const float dt = posEstimator.gps.updateDt; + if (posEstimator.gps.updateDt) { // only update corrections once every sensor update + // Reset current estimate to GPS altitude if estimate not valid (used for GPS and Baro) + if (!(ctx->newFlags & EST_Z_VALID)) { + posEstimator.est.pos.z = posEstimator.gps.pos.z; + posEstimator.est.vel.z = posEstimator.gps.vel.z; + ctx->newEPV = posEstimator.gps.epv; + } + else { + ctx->applyCorrections = true; + const float dT = posEstimator.gps.updateDt; - // Altitude - const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); - const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); - const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; - const float w_z_gps_v = positionEstimationConfig()->w_z_gps_v; + // Altitude + const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); + const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); + const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; + const float w_z_gps_v = positionEstimationConfig()->w_z_gps_v; - ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * dt; - ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * dt; + ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * dT; + ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * dT; - ctx->newEPV = updateEPE(ctx->newEPV, dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); + ctx->newEPV = updateEPE(ctx->newEPV, dT, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); - // Accelerometer bias - ctx->accBiasCorr.z += dt * (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v)); + // Accelerometer bias + ctx->accBiasCorr.z += dT * (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v)); + } } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed @@ -672,6 +674,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) { if (ctx->newFlags & EST_GPS_XY_VALID) { + if (!posEstimator.gps.updateDt) { // only update corrections once every sensor update + return true; + } /* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */ if (!(ctx->newFlags & EST_XY_VALID)) { posEstimator.est.pos.x = posEstimator.gps.pos.x; @@ -680,9 +685,9 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) posEstimator.est.vel.y = posEstimator.gps.vel.y; ctx->newEPH = posEstimator.gps.eph; } - else if (posEstimator.gps.updateDt) { + else { ctx->applyCorrections = true; - const float dt = posEstimator.gps.updateDt; + const float dT = posEstimator.gps.updateDt; const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x; const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y; @@ -697,19 +702,19 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) const float w_xy_gps_v = positionEstimationConfig()->w_xy_gps_v * sq(gpsWeightScaler); // Coordinates - ctx->estPosCorr.x = gpsPosXResidual * w_xy_gps_p * dt; - ctx->estPosCorr.y = gpsPosYResidual * w_xy_gps_p * dt; + ctx->estPosCorr.x = gpsPosXResidual * w_xy_gps_p * dT; + ctx->estPosCorr.y = gpsPosYResidual * w_xy_gps_p * dT; // Velocity from direct measurement - ctx->estVelCorr.x = gpsVelXResidual * w_xy_gps_v * dt; - ctx->estVelCorr.y = gpsVelYResidual * w_xy_gps_v * dt; + ctx->estVelCorr.x = gpsVelXResidual * w_xy_gps_v * dT; + ctx->estVelCorr.y = gpsVelYResidual * w_xy_gps_v * dT; // Accelerometer bias - ctx->accBiasCorr.x = dt * (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v)); - ctx->accBiasCorr.y = dt * (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v)); + ctx->accBiasCorr.x = dT * (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v)); + ctx->accBiasCorr.y = dT * (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v)); /* Adjust EPH */ - ctx->newEPH = updateEPE(posEstimator.est.eph, dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p); + ctx->newEPH = updateEPE(posEstimator.est.eph, dT, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p); } return true; @@ -753,7 +758,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) return; } - /* Calculate new EPH and EPV for the case we didn't update position - linear degradation in max 10s */ + /* Calculate new degraded EPH and EPV for the case we didn't update estimation from sensors - linear degradation in max 10s */ ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); @@ -786,22 +791,18 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) posEstimator.est.vel.z -= posEstimator.est.vel.z * positionEstimationConfig()->w_z_res_v * ctx.dt; } + // Only apply corrections if new sensor update available if (ctx.applyCorrections) { ctx.applyCorrections = false; - float maxUpdateDt = MAX(posEstimator.gps.updateDt, posEstimator.baro.updateDt); - maxUpdateDt = MAX(maxUpdateDt, posEstimator.flow.updateDt); - - posEstimator.gps.updateDt = 0.0f; - posEstimator.baro.updateDt = 0.0f; - posEstimator.flow.updateDt = 0.0f; - // Boost the corrections based on accWeight vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor); vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor); // Constrain corrections to prevent instability - float correctionLimit = INAV_EST_CORR_LIMIT_VALUE * maxUpdateDt; + float maxUpdateDt = MAX(posEstimator.gps.updateDt, posEstimator.baro.updateDt); + maxUpdateDt = MAX(maxUpdateDt, posEstimator.flow.updateDt); + const float correctionLimit = INAV_EST_CORR_LIMIT_VALUE * maxUpdateDt; for (uint8_t axis = 0; axis < 3; axis++) { ctx.estPosCorr.v[axis] = constrainf(ctx.estPosCorr.v[axis], -correctionLimit, correctionLimit); ctx.estVelCorr.v[axis] = constrainf(ctx.estVelCorr.v[axis], -correctionLimit, correctionLimit); @@ -823,6 +824,11 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) posEstimator.imu.accelBias.y = constrainf(posEstimator.imu.accelBias.y, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); posEstimator.imu.accelBias.z = constrainf(posEstimator.imu.accelBias.z, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); } + + // Reset sensor update time deltas once sensor corrections applied after sensor update + posEstimator.gps.updateDt = 0.0f; + posEstimator.baro.updateDt = 0.0f; + posEstimator.flow.updateDt = 0.0f; } /* Update ground course */ From 7cdf9546360587432eb99317d8de1c16703b7f9c Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 11 Feb 2026 20:52:29 +0000 Subject: [PATCH 5/5] fixes --- src/main/navigation/navigation_pos_estimator.c | 2 +- src/main/navigation/navigation_pos_estimator_private.h | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index cdfb3c87ed6..3db5aa2ae43 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -299,7 +299,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) posEstimator.baro.epv = positionEstimationConfig()->baro_epv; posEstimator.baro.lastUpdateTime = currentTimeUs; - if (baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) { + if (baroDtUs > 0 && baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) { const float baroDtSec = US2S(baroDtUs); posEstimator.baro.updateDt = baroDtSec; diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 34133d52b1f..8a91afe268b 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -33,7 +33,6 @@ #define INAV_GPS_ACCEPTANCE_EPE 500.0f // 5m acceptance radius #define INAV_ACC_BIAS_ACCEPTANCE_VALUE (GRAVITY_CMSS * 0.25f) // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway -#define INAV_EST_CORR_LIMIT_VALUE 4000.0f #define INAV_EST_CORR_LIMIT_VALUE 4000.0f // Sanity constraint limit for pos/vel estimate correction value (max 40m correction per s)