Review Summary by Qodo

Position estimator corrections separation and velocity filtering

✨ Enhancement

Walkthroughs

• Split XY and Z position estimator corrections into separate flags
  - Allows independent updates when XY or Z sensor data arrives
• Improved EPH/EPV degradation logic with 200ms timeout threshold
  - Only degrades when no sensor updates occur for >200ms
• Applied PT1 filtering to velocity estimates
  - Reduces noise in XY and Z velocity publications
• Optimized time macro for frequency-to-seconds conversion

flowchart LR
  A["Sensor Updates<br/>XY & Z"] --> B["Split Correction<br/>Flags"]
  B --> C["Independent<br/>XY Corrections"]
  B --> D["Independent<br/>Z Corrections"]
  C --> E["Apply Corrections<br/>Per Axis"]
  D --> E
  E --> F["PT1 Filter<br/>Velocity"]
  F --> G["Publish Filtered<br/>Velocity"]
  H["200ms Timeout<br/>Logic"] --> I["Degrade EPH/EPV<br/>Only on Timeout"]
  I --> J["Update Estimates"]

File Changes

Optimize frequency-to-seconds time macro

• Simplified HZ2S macro from nested macro calls to direct calculation
• Changed from US2S(HZ2US(hz)) to (1.0f / (hz)) for better efficiency

src/main/common/time.h

Separate XY/Z corrections and add velocity filtering

• Split applyCorrections flag into applyCorrectionsXY and applyCorrectionsZ
• Added separate timeout tracking for XY and Z sensor updates with 200ms threshold
• Modified EPH/EPV degradation to only occur when sensor timeout exceeded
• Refactored correction application loop to handle XY and Z axes independently
• Added PT1 filtering to velocity estimates before publishing
• Moved accelerometer bias correction inside per-axis loop

src/main/navigation/navigation_pos_estimator.c

Update flow estimator for split correction flags

• Updated optical flow correction to use new applyCorrectionsXY flag
• Changed from generic applyCorrections to axis-specific flag

src/main/navigation/navigation_pos_estimator_flow.c

Add velocity filter constant and split correction flags

• Added INAV_EST_VEL_F_CUT_HZ constant for velocity filter cutoff frequency
• Split applyCorrections boolean into applyCorrectionsXY and applyCorrectionsZ

src/main/navigation/navigation_pos_estimator_private.h

Code Review by Qodo

🐞 Bugs (2) 📘 Rule violations (2) 📎 Requirement gaps (0)

Description

The new sensor-update timeout checks mix float millisecond math (US2MS) with integer timeMs_t,
which can lose precision and yield incorrect timeout behavior after longer uptimes. This violates
the requirement to keep consistent units/types and avoid precision-losing conversions in math-heavy
code.

Code

src/main/navigation/navigation_pos_estimator.c[R764-767]

+    const bool XYSensorUpdateTimeout = US2MS(currentTimeUs) - lastXYSensorUpdateMs > 200;
+    ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv && XYSensorUpdateTimeout) ? 100.0f * ctx.dt : 0.0f);
+    const bool ZSensorUpdateTimeout = US2MS(currentTimeUs) - lastZSensorUpdateMs > 200;
+    ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv && ZSensorUpdateTimeout) ? 100.0f * ctx.dt : 0.0f);

Evidence

timeMs_t is an integer type, but the new timeout logic computes US2MS(currentTimeUs) using a
float macro and subtracts an integer timestamp, introducing mixed-type/precision behavior in core
estimator timing logic (rule 2).

src/main/navigation/navigation_pos_estimator.c[764-767]
src/main/common/time.h[36-38]
src/main/common/time.h[55-56]
Best Practice: Learned patterns

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
The new XY/Z sensor timeout checks use `US2MS(currentTimeUs)` which returns a float, then subtract `timeMs_t` integer timestamps. This mixes float and integer time units and can lose precision or behave unexpectedly over long uptimes.

## Issue Context
`timeMs_t` is `uint32_t`, while `US2MS()` is a float conversion macro. Timeout checks should be done using consistent integer time units.

## Fix Focus Areas
- src/main/navigation/navigation_pos_estimator.c[764-767]

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

Description

The new lastXYSensorUpdateMs/lastZSensorUpdateMs timestamps are initialized to 0, which is a
plausible real timestamp and conflates “no data yet” with “updated at boot”. This violates the
requirement to use explicit no-data sentinels for uninitialized external/sensor-driven state.

Code

src/main/navigation/navigation_pos_estimator.c[R745-747]

    estimationContext_t ctx;
+    static timeMs_t lastXYSensorUpdateMs = 0;
+    static timeMs_t lastZSensorUpdateMs = 0;

Evidence

The checklist requires explicit no-data sentinels; the new code uses 0 for last-update timestamps
even though an explicit sentinel (TIMEMS_MAX) exists for timeMs_t, making initialization
ambiguous (rule 1).

src/main/navigation/navigation_pos_estimator.c[745-747]
src/main/common/time.h[37-38]
Best Practice: Learned patterns

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
`lastXYSensorUpdateMs` and `lastZSensorUpdateMs` are initialized to `0`, which is a plausible timestamp and does not explicitly represent the “no data yet” state.

## Issue Context
The codebase defines `TIMEMS_MAX` for `timeMs_t`, which can be used as an explicit sentinel, or you can add a boolean initialization flag and set the timestamps on first use.

## Fix Focus Areas
- src/main/navigation/navigation_pos_estimator.c[745-747]
- src/main/navigation/navigation_pos_estimator.c[763-767]

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

Description

The new estimated-velocity PT1 filters are static-zero initialized and never reset to the current
velocity, so on first valid publish (and after eph/epv validity toggles) the published/controller
velocity transiently converges from 0 (or stale state) toward the true value. This creates a
predictable short-term bias/lag in the velocity used by navigation controllers.

Code

src/main/navigation/navigation_pos_estimator.c[R877-897]

        if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
+            static pt1Filter_t estVelFilterState_X;
+            static pt1Filter_t estVelFilterState_Y;
+            float filteredVelX = pt1FilterApply4(&estVelFilterState_X, posEstimator.est.vel.x, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));
+            float filteredVelY = pt1FilterApply4(&estVelFilterState_Y, posEstimator.est.vel.y, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));
+
            // FIXME!!!!!
-            updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y);
+            updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, filteredVelX, filteredVelY);
        }
        else {
            updateActualHorizontalPositionAndVelocity(false, false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0);
        }

        /* Publish altitude update and set altitude validity */
        if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
+            static pt1Filter_t estVelFilterState_Z;
+            float filteredVelZ = pt1FilterApply4(&estVelFilterState_Z, posEstimator.est.vel.z, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));
+
            const float gpsCfEstimatedAltitudeError = STATE(GPS_FIX) ? posEstimator.gps.pos.z - posEstimator.est.pos.z : 0;
            navigationEstimateStatus_e aglStatus = (posEstimator.est.aglQual == SURFACE_QUAL_LOW) ? EST_USABLE : EST_TRUSTED;
-            updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus, gpsCfEstimatedAltitudeError);
+            updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, filteredVelZ, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus, gpsCfEstimatedAltitudeError);

Evidence

pt1FilterApply4 uses the existing filter state as prior output; for static filters that start at 0
and are never pt1FilterReset(), the first outputs are heavily biased toward 0 and converge
exponentially based on cutoff. Navigation code elsewhere demonstrates the expected pattern of
calling pt1FilterReset() on state transitions/resets.

src/main/navigation/navigation_pos_estimator.c[877-897]
src/main/common/filter.c[93-109]
src/main/navigation/navigation_multicopter.c[192-209]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

### Issue description
The new `estVelFilterState_*` filters start with zero state and are never `pt1FilterReset()` to the current velocity. This creates a transient bias/lag in published/controller velocity on startup and after eph/epv validity toggles.

### Issue Context
`pt1FilterApply4()` uses previous `filter->state` as the prior output. Many other navigation filters explicitly call `pt1FilterReset()` on controller resets.

### Fix Focus Areas
- src/main/navigation/navigation_pos_estimator.c[877-897]
- src/main/common/filter.c[93-109]

### Suggested change sketch
- Track a small init flag per axis (or reuse validity transitions):
 - When `posEstimator.est.eph` becomes valid and the XY filters haven’t been initialized, call:
   - `pt1FilterReset(&estVelFilterState_X, posEstimator.est.vel.x);`
   - `pt1FilterReset(&estVelFilterState_Y, posEstimator.est.vel.y);`
 - Same for Z when EPV becomes valid.
- Consider resetting when eph/epv becomes invalid to avoid reusing stale filter history after long invalid periods.
- Optionally move filter states to file-scope statics so they can be reset from `initializePositionEstimator()`.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

Description

After introducing filtered velocities for the published ‘actual’ state, DEBUG_POS_EST still logs raw
posEstimator.est.vel.* (unfiltered). This makes blackbox/debug traces inconsistent with what the
navigation controllers are consuming and complicates tuning/diagnosis.

Code

src/main/navigation/navigation_pos_estimator.c[R880-885]

+            float filteredVelX = pt1FilterApply4(&estVelFilterState_X, posEstimator.est.vel.x, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));
+            float filteredVelY = pt1FilterApply4(&estVelFilterState_Y, posEstimator.est.vel.y, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));
+
            // FIXME!!!!!
-            updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y);
+            updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, filteredVelX, filteredVelY);
        }

Evidence

The PR changes updateActual* calls to use filtered velocities, but the DEBUG_SET fields remain based
on the raw estimator velocity values.

src/main/navigation/navigation_pos_estimator.c[877-918]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

### Issue description
Velocity used by `updateActual*` is now filtered, but DEBUG_POS_EST continues to log raw `posEstimator.est.vel.*`, making logs inconsistent with controller inputs.

### Issue Context
This is a diagnosability/tuning concern introduced by the new filtering.

### Fix Focus Areas
- src/main/navigation/navigation_pos_estimator.c[877-918]

### Suggested change sketch
- Either:
 - log filtered velocities instead of raw when publishing, or
 - add separate debug indices for filtered velocities while keeping raw ones.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

ⓘ The new review experience is currently in Beta. Learn more

I ran this on a few copters. They ranged from very well setup, to poorly balanced due to mildly damaged props. etc

For the one that is well setup and even still held position very well using the 8.x firmware. Its position accuracy is now extremely locked in on all axis's. And held a fixed altitude as good as any Lidar I've used.

But my main focus has been on poorer setup copters. i.e. ones that have some sort of vibrations from a bent prop or damaged motor bearings in a crash. Or a poorly position GNSS module.
After spending a fair amount of time this week logging flights of those copters against the good one. And making setting changes, then running back to back tests. I would have to say you have this working very well. It works as good as can be expected.

But in saying that. After assessing stacks of logs from those poorly setup copters. Its very noticeable to see that a much better accelerometer xyz weight factor is required to handle those vibrations. By scaling back the amount of input the ACC adds to the position estimator in a dynamic and intelligent way. Not the way it is at present. Which is of no use at all.

I can flash back to 7.0 and reduce inav_w_xyz_acc_p to a more suitable value for the vibration a given quad is producing. And its no longer jittery on XY and Z.

The jitteriness I have experienced with these less than perfect mechanically tuned copters, even occurs when the HDOP is 1.1 and below. Which means the accelerometer is doing a good deal of work, which is evident by how well my well setup test copter works.
But unfortunately the other ones are more likely to be the normal, for many peoples builds. This is why I ran tests with these type of builds. To find out how it would perform in those cases.

After doing more flights with these newer changes that have been implemented over the last couple of months. And more recently using a broader range of copter sizes.
I have come to the conclusion that these changes are the pinnacle of what I've experienced with INAV copter position accuracy. IF the model is clean and mostly free from accelerometer vibrations.

However, unfortunately INAV software has to work with a wide range of build qualities... From the high-end, to the not so much.

I know I mentioned this in the previous comment, but never heard anything back. That could either mean Breadoven is secretly working on it in the background. Or the issue is in the too hard basket for now.
But unfortunately this sort of precision comes at a price. And I fear when this is released we may hear of more copters either shooting vertically or jittering.. As I have experienced this in flight with subpar hardware, that I was using to find the worse cases.

Even though most users never touch inav_w_xyz_acc_p before it was removed in 7.0... It certainly allowed tune-ability for someone who knew their quad was always going to produce a certain level of vibrations that could not be gotten rid of mechanically or by filters.
While at present users have no such luxury.

Accelerometer noise is also over looked more than Gyro noise. Yet its the one more susceptible to these vibrations.

My take on it would be something like this -

Any ACC dynamic weight control logic, would need to take an average sample for a few seconds at the beginning of the flight. Then use that as a baseline for the ACC weight feed to the position estimator.
With any other higher amplitude samples taken throughout the flight, only being taken when the quad is at close to a zero xyz acceleration state.
Then only apply any extra changes to the weight if the average over an extended period of time is constant.

And in the case of INAV's gyro filter which can be DYNAMIC. Could such a method also be applied to the accelerometer, based on throttle position. Because the sky-rocket effect that occurs in INAV altitude controlled modes, more often occurs as the throttle is increased, on noisy copters.

It would seem the original ACC clipping factor should have been able to rapidly pickup ACC noise spikes and reduce the weight accordingly. And it may do that in extreme cases. But it also dampened the data far too often when it isn't required.
I am aware the minimum weight has been reduced from 0.3 to 0.5. And I believe this also helped when combine with all these resent changes. Although it almost seems like there needs to be a dual stage filter. One for random high amplitude spikes, and the other for sustain noise above an acceptable level.

@breadoven @sensei-hacker I would be interested to hear you views on this ?

I know I mentioned this in the previous comment, but never heard anything back. That could either mean Breadoven is secretly working on it in the background.

I was waiting to see if the recent changes caused any problems. I did test them a fixed wing the other day and all worked as expected. Hard to say if it was any better.

I'll have a look at the acc related stuff next because I don't think it works well, too unpredictable if vibration becomes an issue. Can't help but think the old method is safer given it simply cuts out acc derived estimates and reverts to sensor based estimates only which may be more sluggish in response but are far less likely to cause unrealistic/unstable estimates. It would be useful to have a log that shows unstable vertical velocity so long as it includes NavAcc data so you can see exactly how the acceleration is behaving to cause problems.

It sure is a balancing act between relying more heavily on Accelerometer data than GNSS for the tightest position accuracy.
I guess its not a problem if the two can be accurately assessed over a period, to see if either or both data sources are within usable tolerances. Or they have fallen below that tolerance for a given time period.

I feel it maybe beneficial at some point when both data sources become poor, as it has been throughout 7.1 to recently. If the estimator just gives up, when in a mode like multicopter Poshold. And falls back to Angle mode, while displaying a comprehensive warning message on the OSD. So the user knows what the fault is.
Rather than the estimator persevering under impossible conditions, and just causing the copter to become a useless drifting hazard. While the user gets frustrated, with no idea what is going wrong.

I know this would be a final fall back. But I've certainly experienced times when this would have been a welcome message.

The navigation system has a hard cutoff when a satellite fix is lost. So maybe also interrogate the accelerometer data to a higher degree. After all, it's the IMU that is required for the AHRS and the Wind estimator as well for airplanes. And they also have no shortage of people experiencing attitude drift with poorly balanced hardware.

I know you have Vibration Level on the multi-function display. But I've never seen it appear. Likely because its based on extremes. Rather than the intermittent amplitude spikes that cause more long term grief to the estimator over the duration of the flight, than a large couple of spikes that come and go without long term effect.

Although it almost seems like there needs to be a dual stage filter. One for random high amplitude spikes, and the other for sustained noise above an acceptable level.

I know you have Vibration Level on the multi-function display. But I've never seen it appear. Likely because its based on extremes.

It's commented out in the code so not used. I have it enabled in my builds but I never really worked out what the best limits are so it pops up sometimes as a warning but probably not when vibration is really an issue. I don't think the method used to calculate vibration gives reliable results, e.g. it appears reacts to normal manoeuvring G loads which doesn't make sense.

@breadoven Today @sensei-hacker and I where assisting a guy on discord with an issue he had.
The copter had two issues. The 1st was he didn't calibrate the current sensor. Leaving the battery almost depleted after a 6km trip.
But the second was the Acc vibrations this copter had. Which was manifest at the moment an altitude controlled navigation mode was activated. (There was talk of a loose prop spinning on the motors drive plate.)
I only picked up on the accVib later in my assessment of his log, because I have never seen vibrations that severe before in a log.
But considering the effect it had on vertical altitude rate. I thought it might be worth your assessment of this matter. Being that we have been talking here about the bad effects Acc vibrations have on the estimators, and how to better handle such inevitable issues.
I also thought it interesting that this was 8.0. Which should have dropped the clipping factor to 0.3.

Here is a link to his Log , DVR and Diff .

@Jetrell The accVib spikes towards the end of that log are caused by an overflow by the looks of it. Not sure what's wrong but something is because accVib is also -ve just before and during the large spikes which shouldn't happen given it's calculated from the squares of a value calculated from the acc values. There's some sort of numerical bug somewhere.

The other thing is a pretty bad contradiction between Z velocity and Z position at the end of the log with Z pos descending and Z velocity climbing at over 5 m/s. Don't know if this has been improved or not with recent changes but something needs doing to stop this because it's clearly nonsense.

Don't know if this has been improved or not with recent changes but something needs doing to stop this because it's clearly nonsense.

I'm thinking it likely has been improved with the work you and Sensei have been doing in this area.
But I just thought it would be good for you to look at. Being that many people who experience these types of vibration issues that effect navigation, often only report on chat forums. Instead of raising the matter here where the cause could be determined, and the issue fixed.

On checking further the numerical bug is in Blackbox. It's only using a 16 bit integer to store the raw acc values which isn't big enough when acc.dev.acc_1G is 2048. At only 16g this overflows the 16 bit BB value. Not sure why BB needs to store the raw acc values, it would be easier to just store the G values which is what most people are interested in seeing although you'd need to store them in "centi G" to maintain some accuracy.