From 798a61202bd3982fb11d7c51219ca9facfb310b6 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 17 Jan 2026 02:18:01 -0600 Subject: [PATCH 1/2] Fix AGL velocity estimation using squared acceleration weight factor The AGL velocity update was incorrectly using squared acceleration weight factor (sq(accWeightFactor)) while the altitude update uses linear weight. This creates physical inconsistency where position and velocity respond differently to the same acceleration. With accWeightFactor=0.5 (moderate confidence): - Altitude receives 50% of acceleration (correct) - Velocity was receiving 25% of acceleration (wrong - squared) This fix removes sq() from the velocity update to match the altitude update, making both integrations physically consistent with standard kinematic equations. May fix https://github.com/iNavFlight/inav/pull/10314 May be related to https://github.com/iNavFlight/inav/issues/10567 --- src/main/navigation/navigation_pos_estimator_agl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/src/main/navigation/navigation_pos_estimator_agl.c index 03b5bd8ccf0..38788e20a77 100644 --- a/src/main/navigation/navigation_pos_estimator_agl.c +++ b/src/main/navigation/navigation_pos_estimator_agl.c @@ -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) { From 5cf56e8d463bb7f755a753e9560c4e578befc8f6 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Tue, 20 Jan 2026 23:42:34 -0600 Subject: [PATCH 2/2] Fix comment/code mismatch in position estimator weight factors Comments incorrectly stated 0.3 when code uses 0.5f. Updated comments to match actual implementation. --- src/main/navigation/navigation_pos_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index cd861dacd6c..b696813393c 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -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;