Position estimator corrections improvement and estimated velocity filtering#11387
Conversation
Review Summary by QodoPosition estimator corrections separation and velocity filtering
WalkthroughsDescription• 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 Diagramflowchart 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 Changes1. src/main/common/time.h
|
Code Review by Qodo
1. Float US2MS() in timeouts
|
![qodo-code-review[bot]](https://avatars.githubusercontent.com/in/484649?s=60&v=4){kind=small}
| 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); |
There was a problem hiding this comment.
1. Float us2ms() in timeouts 📘 Rule violation ✓ Correctness
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.
Agent Prompt
## 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
|
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. 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 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. |
|
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. 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. Even though most users never touch 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. 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. @breadoven @sensei-hacker I would be interested to hear you views on this ? |
3 participants
PR splits out handling of XY and Z position estimator corrections so the corrections are updated separately when new XY or Z sensor data is available.
Estimated EPH and EPV degradation logic also changed so EPH/EPV are only degraded if no sensor updates occur for >200ms rather than always degrading them between normal sensor update intervals.
Filtering has also been applied to the velocity estimates to remove some of the noise that seems to affect velocity in particular.
HITL tested and also on a quad without issues.