Skip to content

Fix AGL velocity estimation using squared acceleration weight factor #11255

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

Open
sensei-hacker wants to merge 2 commits into
base: maintenance-9.x
Choose a base branch
from
+2 −2
Open

Fix AGL velocity estimation using squared acceleration weight factor #11255

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
798a612
Fix AGL velocity estimation using squared acceleration weight factor
sensei-hacker Jan 17, 2026
5cf56e8
Fix comment/code mismatch in position estimator weight factors
sensei-hacker Jan 21, 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/navigation/navigation_pos_estimator.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -354,7 +354,7 @@ static bool gravityCalibrationComplete(void)
static void updateIMUEstimationWeight(const float dt)
{
static float acc_clip_factor = 1.0f;
// If accelerometer measurement is clipped - drop the acc weight to 0.3
// If accelerometer measurement is clipped - drop the acc weight to 0.5
// and gradually restore weight back to 1.0 over time
if (accIsClipped()) {
acc_clip_factor = 0.5f;
Expand Down
2 changes: 1 addition & 1 deletion src/main/navigation/navigation_pos_estimator_agl.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@ void estimationCalculateAGL(estimationContext_t * ctx)
// Update estimate
posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt;
posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * posEstimator.imu.accWeightFactor;
posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(posEstimator.imu.accWeightFactor);
posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * posEstimator.imu.accWeightFactor;
Copy link
Contributor

@qodo-code-review qodo-code-review bot Jan 17, 2026

Choose a reason for hiding this comment

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

Suggestion: Clamp the posEstimator.imu.accWeightFactor to a range of 0.0f to 1.0f before its use in the velocity update to prevent potential estimation instability. [general, importance: 6]

Suggested change
posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * posEstimator.imu.accWeightFactor;
{
float w = posEstimator.imu.accWeightFactor;
w = fmaxf(0.0f, fminf(1.0f, w));
posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * w;
}

Copy link
Member Author

@sensei-hacker sensei-hacker Jan 17, 2026

Choose a reason for hiding this comment

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

Does the code that sets it allow it to be outside the range of 0.0 - 1.0?

Copy link
Contributor

@qodo-code-review qodo-code-review bot Jan 17, 2026

Choose a reason for hiding this comment

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

Ask❓

Does the code that sets it allow it to be outside the range of 0.0 - 1.0?

Answer:

Based on the way accWeightFactor is described/used in iNav (and in the linked ticket discussion), the code that computes the accelerometer weight typically constrains the underlying vibration metric and then scales it into a weight range that is ≤ 1.0 (e.g. something like scaling a constrained vibration level [1.0 .. 4.0] into a weight [1.0 .. 0.3]). In that common path, it should not legitimately go outside [0.0, 1.0] (it stays in roughly [0.3, 1.0]).

However, in the diff you’re changing only the use site in estimationCalculateAGL(). There’s nothing here that guarantees posEstimator.imu.accWeightFactor is always bounded—if it can be influenced by:

  • an alternative estimator path,
  • a config/CLI parameter,
  • or any computation that doesn’t clamp (or can produce NaN),
    then it could end up outside [0, 1] despite the intended design.

Recommendation: confirm the setter/source of posEstimator.imu.accWeightFactor clamps it (or is derived from a clamped/scaled value). If not, clamping once at the point of computation (preferred) or locally before use (as the bot suggested) would make this update more robust.


// Apply correction
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
Expand Down