diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index cd861dacd6c..35245d9f7f0 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); @@ -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); @@ -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; @@ -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; @@ -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; diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index abb389bddcb..fc824eb534e 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 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