Merge pull request #11255 from sensei-hacker/fix/agl-velocity-weight-squared

Fix AGL velocity estimation using squared acceleration weight factor

Author: sensei-hacker

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;

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) {