From f2792aa6a43fa8c44b52ac9082140368bcdcc535 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 22 Jan 2026 16:02:52 +0000 Subject: [PATCH 1/3] Position estimator corrections fix --- .../navigation/navigation_pos_estimator.c | 34 +++++++++++-------- .../navigation_pos_estimator_private.h | 2 ++ 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index cd861dacd6c..22bc238393c 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -625,8 +625,8 @@ 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)) { - ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z; - ctx->estVelCorr.z += posEstimator.gps.vel.z - posEstimator.est.vel.z; + posEstimator.est.pos.z = posEstimator.gps.pos.z; + posEstimator.est.vel.z = posEstimator.gps.vel.z; ctx->newEPV = posEstimator.gps.epv; } else { @@ -660,10 +660,10 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) if (ctx->newFlags & EST_GPS_XY_VALID) { /* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */ if (!(ctx->newFlags & EST_XY_VALID)) { - ctx->estPosCorr.x += posEstimator.gps.pos.x - posEstimator.est.pos.x; - ctx->estPosCorr.y += posEstimator.gps.pos.y - posEstimator.est.pos.y; - ctx->estVelCorr.x += posEstimator.gps.vel.x - posEstimator.est.vel.x; - ctx->estVelCorr.y += posEstimator.gps.vel.y - posEstimator.est.vel.y; + posEstimator.est.pos.x = posEstimator.gps.pos.x; + posEstimator.est.pos.y = posEstimator.gps.pos.y; + posEstimator.est.vel.x = posEstimator.gps.vel.x; + posEstimator.est.vel.y = posEstimator.gps.vel.y; ctx->newEPH = posEstimator.gps.eph; } else { @@ -739,6 +739,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) /* 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); + ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs); vectorZero(&ctx.estPosCorr); vectorZero(&ctx.estVelCorr); @@ -762,17 +763,24 @@ 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.estVelCorr.x = -posEstimator.est.vel.x * positionEstimationConfig()->w_xy_res_v * ctx.dt; + ctx.estVelCorr.y = -posEstimator.est.vel.y * positionEstimationConfig()->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.estVelCorr.z = -posEstimator.est.vel.z * positionEstimationConfig()->w_z_res_v * ctx.dt; } + // 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 + for (uint8_t axis = 0; axis < 3; axis++) { + ctx.estPosCorr.v[axis] = constrainf(ctx.estPosCorr.v[axis], -INAV_EST_CORR_LIMIT_VALUE, INAV_EST_CORR_LIMIT_VALUE); + ctx.estVelCorr.v[axis] = constrainf(ctx.estPosCorr.v[axis], -INAV_EST_CORR_LIMIT_VALUE, INAV_EST_CORR_LIMIT_VALUE); + } + // Apply corrections vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr); vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); @@ -794,8 +802,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) estimationCalculateGroundCourse(currentTimeUs); /* Update uncertainty */ - posEstimator.est.eph = ctx.newEPH; - posEstimator.est.epv = ctx.newEPV; + posEstimator.est.eph = constrainf(ctx.newEPH, 0.0f, 2.0f * max_eph_epv); + posEstimator.est.epv = constrainf(ctx.newEPV, 0.0f, 2.0f * max_eph_epv); // Keep flags for further usage posEstimator.flags = ctx.newFlags; @@ -873,8 +881,6 @@ bool isEstimatedAglTrusted(void) { */ void initializePositionEstimator(void) { - int axis; - posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f; posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f; @@ -893,7 +899,7 @@ void initializePositionEstimator(void) restartGravityCalibration(); - for (axis = 0; axis < 3; axis++) { + for (uint8_t axis = 0; axis < 3; axis++) { posEstimator.imu.accelBias.v[axis] = 0; posEstimator.est.pos.v[axis] = 0; posEstimator.est.vel.v[axis] = 0; diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index abb389bddcb..684773d2ad0 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -34,6 +34,8 @@ #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 2.0f // Max allowed pos/vel estimate correction value per loop + #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 From 8ad4b1d1d07df31d02a4a9078eaa44d5f35c8c96 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 22 Jan 2026 17:42:08 +0000 Subject: [PATCH 2/3] Update navigation_pos_estimator.c --- src/main/navigation/navigation_pos_estimator.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 22bc238393c..8b06d059404 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -752,14 +752,11 @@ 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) { @@ -778,7 +775,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) // Constrain corrections to prevent instability for (uint8_t axis = 0; axis < 3; axis++) { ctx.estPosCorr.v[axis] = constrainf(ctx.estPosCorr.v[axis], -INAV_EST_CORR_LIMIT_VALUE, INAV_EST_CORR_LIMIT_VALUE); - ctx.estVelCorr.v[axis] = constrainf(ctx.estPosCorr.v[axis], -INAV_EST_CORR_LIMIT_VALUE, INAV_EST_CORR_LIMIT_VALUE); + ctx.estVelCorr.v[axis] = constrainf(ctx.estVelCorr.v[axis], -INAV_EST_CORR_LIMIT_VALUE, INAV_EST_CORR_LIMIT_VALUE); } // Apply corrections From 870f7f201d5d95a817dbd023a4e24832c58a4ce2 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 8 Feb 2026 20:19:30 +0000 Subject: [PATCH 3/3] Fix correction sanity limit, relate to loop rate --- src/main/navigation/navigation_pos_estimator.c | 5 +++-- src/main/navigation/navigation_pos_estimator_private.h | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 8b06d059404..35245d9f7f0 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -773,9 +773,10 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor); // Constrain corrections to prevent instability + const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * ctx.dt; for (uint8_t axis = 0; axis < 3; axis++) { - ctx.estPosCorr.v[axis] = constrainf(ctx.estPosCorr.v[axis], -INAV_EST_CORR_LIMIT_VALUE, INAV_EST_CORR_LIMIT_VALUE); - ctx.estVelCorr.v[axis] = constrainf(ctx.estVelCorr.v[axis], -INAV_EST_CORR_LIMIT_VALUE, INAV_EST_CORR_LIMIT_VALUE); + ctx.estPosCorr.v[axis] = constrainf(ctx.estPosCorr.v[axis], -corrLimit, corrLimit); + ctx.estVelCorr.v[axis] = constrainf(ctx.estVelCorr.v[axis], -corrLimit, corrLimit); } // Apply corrections diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 684773d2ad0..fc824eb534e 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -34,7 +34,7 @@ #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 2.0f // Max allowed pos/vel estimate correction value per loop +#define INAV_EST_CORR_LIMIT_VALUE 4000.0f // Sanity constraint limit for pos/vel estimate correction value (max 40m correction per s) #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