Skip to content

Position estimator position and velocity corrections fix #11270

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
breadoven merged 3 commits into from
Feb 10, 2026
+25 −19
Merged

Position estimator position and velocity corrections fix #11270

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f2792aa
Position estimator corrections fix
breadoven Jan 22, 2026
8ad4b1d
Update navigation_pos_estimator.c
breadoven Jan 22, 2026
870f7f2
Fix correction sanity limit, relate to loop rate
breadoven Feb 8, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 23 additions & 19 deletions src/main/navigation/navigation_pos_estimator.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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);
Expand All @@ -751,28 +752,33 @@ 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.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
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], -corrLimit, corrLimit);
ctx.estVelCorr.v[axis] = constrainf(ctx.estVelCorr.v[axis], -corrLimit, corrLimit);
}

// Apply corrections
vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
Expand All @@ -794,8 +800,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;
Expand Down Expand Up @@ -873,8 +879,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;

Expand All @@ -893,7 +897,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;
Expand Down
2 changes: 2 additions & 0 deletions src/main/navigation/navigation_pos_estimator_private.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 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

Expand Down