Skip to content

Commit 798a612

Browse files
committed
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 #10314 May be related to #10567
1 parent a7932b9 commit 798a612

File tree

1 file changed

+1
-1
lines changed

1 file changed

+1
-1
lines changed

src/main/navigation/navigation_pos_estimator_agl.c

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -149,7 +149,7 @@ void estimationCalculateAGL(estimationContext_t * ctx)
149149
// Update estimate
150150
posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt;
151151
posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * posEstimator.imu.accWeightFactor;
152-
posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(posEstimator.imu.accWeightFactor);
152+
posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * posEstimator.imu.accWeightFactor;
153153

154154
// Apply correction
155155
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {

0 commit comments

Comments
 (0)