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

Merged
sensei-hacker merged 2 commits into from
Feb 13, 2026
+2 −2
Merged

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;

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