Skip to content

Position estimator corrections improvement and estimated velocity filtering #11387

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
sensei-hacker merged 3 commits into from
Mar 7, 2026
Merged

Position estimator corrections improvement and estimated velocity filtering #11387

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
7ff2464
pos est corr improvements + vel filtering
breadoven Mar 1, 2026
ed37ed2
Update navigation_pos_estimator.c
breadoven Mar 2, 2026
506ea42
Update navigation_pos_estimator.c
breadoven Mar 2, 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
2 changes: 1 addition & 1 deletion src/main/common/time.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ typedef uint32_t timeUs_t;
#define MS2S(ms) ((ms) * 1e-3f)
#define S2MS(s) ((s) * MILLISECS_PER_SEC)
#define DS2MS(ds) ((ds) * 100)
#define HZ2S(hz) US2S(HZ2US(hz))
#define HZ2S(hz) (1.0f / (hz))

// Use this function only to get small deltas (difference overflows at ~35 minutes)
static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); }
Expand Down
91 changes: 57 additions & 34 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 @@ -581,7 +581,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)

if (ctx->newFlags & EST_BARO_VALID && wBaro) {
if (posEstimator.baro.updateDt) { // only update corrections once every sensor update
ctx->applyCorrections = true;
ctx->applyCorrectionsZ = true;
const float dT = posEstimator.baro.updateDt;

bool isAirCushionEffectDetected = false;
Expand Down Expand Up @@ -641,7 +641,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
ctx->newEPV = posEstimator.gps.epv;
}
else {
ctx->applyCorrections = true;
ctx->applyCorrectionsZ = true;
const float dT = posEstimator.gps.updateDt;

// Altitude
Expand Down Expand Up @@ -686,7 +686,7 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
ctx->newEPH = posEstimator.gps.eph;
}
else {
ctx->applyCorrections = true;
ctx->applyCorrectionsXY = true;
const float dT = posEstimator.gps.updateDt;

const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x;
Expand Down Expand Up @@ -743,6 +743,8 @@ static void estimationCalculateGroundCourse(timeUs_t currentTimeUs)
static void updateEstimatedTopic(timeUs_t currentTimeUs)
{
estimationContext_t ctx;
static timeMs_t lastXYSensorUpdateMs = 0;
static timeMs_t lastZSensorUpdateMs = 0;

const float max_eph_epv = positionEstimationConfig()->max_eph_epv;

Expand All @@ -758,14 +760,18 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
return;
}

/* 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);
/* Calculate new degraded EPH and EPV for the case we didn't update estimation from sensors for > 200ms - linear degradation in max 10s */
const bool XYSensorUpdateTimeout = US2MS(currentTimeUs) - lastXYSensorUpdateMs > 200;
ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv && XYSensorUpdateTimeout) ? 100.0f * ctx.dt : 0.0f);
const bool ZSensorUpdateTimeout = US2MS(currentTimeUs) - lastZSensorUpdateMs > 200;
ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv && ZSensorUpdateTimeout) ? 100.0f * ctx.dt : 0.0f);
Comment on lines +764 to +767
Copy link
Copy Markdown
Contributor

@qodo-code-review qodo-code-review Bot Mar 2, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Action required

1. Float us2ms() in timeouts 📘 Rule violation ✓ Correctness

The new sensor-update timeout checks mix float millisecond math (US2MS) with integer timeMs_t,
which can lose precision and yield incorrect timeout behavior after longer uptimes. This violates
the requirement to keep consistent units/types and avoid precision-losing conversions in math-heavy
code.
Agent Prompt 
## Issue description
The new XY/Z sensor timeout checks use `US2MS(currentTimeUs)` which returns a float, then subtract `timeMs_t` integer timestamps. This mixes float and integer time units and can lose precision or behave unexpectedly over long uptimes.

## Issue Context
`timeMs_t` is `uint32_t`, while `US2MS()` is a float conversion macro. Timeout checks should be done using consistent integer time units.

## Fix Focus Areas
- src/main/navigation/navigation_pos_estimator.c[764-767]

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools


ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);
vectorZero(&ctx.estPosCorr);
vectorZero(&ctx.estVelCorr);
vectorZero(&ctx.accBiasCorr);
ctx.applyCorrectionsXY = false;
ctx.applyCorrectionsZ = false;

/* AGL estimation - separate process, decouples from Z coordinate */
estimationCalculateAGL(&ctx);
Expand All @@ -792,43 +798,52 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
}

// Only apply corrections if new sensor update available
if (ctx.applyCorrections) {
ctx.applyCorrections = false;

// 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
if (ctx.applyCorrectionsXY || ctx.applyCorrectionsZ) {
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++) {
float correctionLimit = INAV_EST_CORR_LIMIT_VALUE * maxUpdateDt;

uint8_t axisStart = 0;
uint8_t axisEnd = 2;
if (!ctx.applyCorrectionsXY) {
axisStart = 2;
} else if (!ctx.applyCorrectionsZ) {
axisEnd = 1;
}

for (uint8_t axis = axisStart; axis <= axisEnd; axis++) {
// Boost the corrections based on accWeight
ctx.estPosCorr.v[axis] *= 1.0f / posEstimator.imu.accWeightFactor;
ctx.estVelCorr.v[axis] *= 1.0f / posEstimator.imu.accWeightFactor;

// Constrain corrections to prevent instability
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);

/* 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);
// Apply corrections
posEstimator.est.pos.v[axis] += ctx.estPosCorr.v[axis];
posEstimator.est.vel.v[axis] += ctx.estVelCorr.v[axis];

/* Correct accelerometer bias */
const float w_acc_bias = positionEstimationConfig()->w_acc_bias;
if (w_acc_bias > 0.0f) {
/* Correct accel bias */
posEstimator.imu.accelBias.v[axis] += ctx.accBiasCorr.v[axis] * w_acc_bias;
posEstimator.imu.accelBias.v[axis] = constrainf(posEstimator.imu.accelBias.v[axis], -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;

if (ctx.applyCorrectionsXY) {
lastXYSensorUpdateMs = US2MS(currentTimeUs);
}
if (ctx.applyCorrectionsZ) {
lastZSensorUpdateMs = US2MS(currentTimeUs);
}
}

/* Update ground course */
Expand Down Expand Up @@ -860,18 +875,26 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)

/* Publish position update */
if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
static pt1Filter_t estVelFilterState_X;
static pt1Filter_t estVelFilterState_Y;
float filteredVelX = pt1FilterApply4(&estVelFilterState_X, posEstimator.est.vel.x, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));
float filteredVelY = pt1FilterApply4(&estVelFilterState_Y, posEstimator.est.vel.y, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));

// FIXME!!!!!
updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y);
updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, filteredVelX, filteredVelY);
}
else {
updateActualHorizontalPositionAndVelocity(false, false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0);
}

/* Publish altitude update and set altitude validity */
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
static pt1Filter_t estVelFilterState_Z;
float filteredVelZ = pt1FilterApply4(&estVelFilterState_Z, posEstimator.est.vel.z, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));

const float gpsCfEstimatedAltitudeError = STATE(GPS_FIX) ? posEstimator.gps.pos.z - posEstimator.est.pos.z : 0;
navigationEstimateStatus_e aglStatus = (posEstimator.est.aglQual == SURFACE_QUAL_LOW) ? EST_USABLE : EST_TRUSTED;
updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus, gpsCfEstimatedAltitudeError);
updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, filteredVelZ, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus, gpsCfEstimatedAltitudeError);
}
else {
updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE, 0);
Expand Down
2 changes: 1 addition & 1 deletion src/main/navigation/navigation_pos_estimator_flow.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +124,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 ctx->applyCorrections = true;
return ctx->applyCorrectionsXY = true;
#else
UNUSED(ctx);
return false;
Expand Down
5 changes: 4 additions & 1 deletion 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 @@ -60,6 +60,8 @@
#define RANGEFINDER_RELIABILITY_LOW_THRESHOLD (0.33f)
#define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD (0.75f)

#define INAV_EST_VEL_F_CUT_HZ 3.0f

typedef struct {
timeUs_t lastTriggeredTime;
timeUs_t deltaTime;
Expand Down Expand Up @@ -183,7 +185,8 @@ typedef struct {
fpVector3_t estPosCorr;
fpVector3_t estVelCorr;
fpVector3_t accBiasCorr;
bool applyCorrections;
bool applyCorrectionsXY;
bool applyCorrectionsZ;
} estimationContext_t;

extern navigationPosEstimator_t posEstimator;
Expand Down