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.

[qodo-code-review]

Suggestion: Clamp the posEstimator.imu.accWeightFactor to a range of 0.0f to 1.0f before its use in the velocity update to prevent potential estimation instability. [general, importance: 6]

Suggested change

	posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * posEstimator.imu.accWeightFactor;
	{
	float w = posEstimator.imu.accWeightFactor;
	w = fmaxf(0.0f, fminf(1.0f, w));
	posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * w;
	}

[sensei-hacker]

Does the code that sets it allow it to be outside the range of 0.0 - 1.0?

[qodo-code-review]

Ask❓

Does the code that sets it allow it to be outside the range of 0.0 - 1.0?

Answer:

Based on the way accWeightFactor is described/used in iNav (and in the linked ticket discussion), the code that computes the accelerometer weight typically constrains the underlying vibration metric and then scales it into a weight range that is ≤ 1.0 (e.g. something like scaling a constrained vibration level [1.0 .. 4.0] into a weight [1.0 .. 0.3]). In that common path, it should not legitimately go outside [0.0, 1.0] (it stays in roughly [0.3, 1.0]).

However, in the diff you’re changing only the use site in estimationCalculateAGL(). There’s nothing here that guarantees posEstimator.imu.accWeightFactor is always bounded—if it can be influenced by:

• an alternative estimator path,
• a config/CLI parameter,
• or any computation that doesn’t clamp (or can produce NaN),
  then it could end up outside [0, 1] despite the intended design.

Recommendation: confirm the setter/source of posEstimator.imu.accWeightFactor clamps it (or is derived from a clamped/scaled value). If not, clamping once at the point of computation (preferred) or locally before use (as the bot suggested) would make this update more robust.