Fix AGL velocity estimation using squared acceleration weight factor

[sensei-hacker]

User description

Summary

Fixes mathematical inconsistency in AGL velocity estimation where velocity update uses squared acceleration weight factor while altitude update uses linear weight factor.

Problem

In navigation_pos_estimator_agl.c line 152, the velocity update incorrectly uses sq(accWeightFactor) while line 151 (altitude update) correctly uses linear accWeightFactor. This creates physical inconsistency where position and velocity respond differently to the same acceleration measurement.

Impact with accWeightFactor=0.3

• Altitude receives 30% of acceleration contribution (correct)
• Velocity receives 9% of acceleration contribution (wrong - squared factor)
• Result: Large error in velocity response during degraded accelerometer conditions

Changes

• Removed sq() wrapper from accWeightFactor in AGL velocity update (line 152)
• Makes velocity integration physically consistent with altitude integration
• Both now use linear weight factor as per standard kinematic equations

Testing

• Built SITL target successfully with fix applied
• Code review performed with inav-code-review agent - no issues found
• Fix verified mathematically: both altitude and velocity now scale linearly with weight factor
• Theoretical analysis confirms 50% velocity error is eliminated

Code Review

Reviewed with inav-code-review agent - APPROVED. The fix correctly addresses a mathematical inconsistency and makes the kinematic integration physically consistent.

Related Issues

May fix #10314
May be related to #10567

---

PR Type

Bug fix

---

Description

• Removes squared acceleration weight factor from AGL velocity update

• Makes velocity integration consistent with altitude integration

• Fixes velocity error during degraded accelerometer conditions

• Both altitude and velocity now use linear weight factor scaling

---

Diagram Walkthrough

flowchart LR
  accel["Acceleration measurement"]
  weight["Weight factor"]
  alt["Altitude update"]
  vel["Velocity update"]
  accel -- "linear factor" --> alt
  accel -- "squared factor (BEFORE)" --> vel
  accel -- "linear factor (AFTER)" --> vel
  weight --> alt
  weight --> vel

Loading

File Walkthrough

Relevant files

Bug fix

navigation_pos_estimator_agl.c Remove squared weight factor from velocity update                src/main/navigation/navigation_pos_estimator_agl.c Removed sq() wrapper from accWeightFactor in AGL velocity update at line 152 Changed velocity integration to use linear weight factor instead of squared Aligns velocity update with altitude update which already uses linear weight factor Maintains kinematic consistency between position and velocity integration +1/-1

---

[qodo-code-review]

PR Compliance Guide 🔍

All compliance sections have been disabled in the configurations.

[Jetrell]

Tested with Matek PMW3901. And I didn't find the results conclusive. Maybe because this sensor has such limited range under intense UV from the sun.
I also left a comment here.

I think it requires testing with a better lidar sensor like the MTF-01, which I don't have. To properly confirm the fix works.