Position estimator position and velocity corrections fix

[breadoven]

User description

Should provide a fix for #10360, #10391, #10893 and #11049.

See #11049 (comment) for explanation and reason for change.

HITL testing shows the estimate instability is fixed with the PR changes. Not easy to flight test given the odd set of circumstances need to trigger the problem in the first place.

INAV_EST_CORR_LIMIT_VALUE may need tweaking. It corresponds to around 40m residual error for x/y position at the default loop rate.

---

PR Type

Bug fix

---

Description

• Fix position estimator instability by directly resetting estimates instead of accumulating corrections

• Add correction limiting to prevent excessive estimate adjustments per loop iteration

• Constrain uncertainty estimates to prevent unbounded growth

• Simplify velocity decay calculation and improve code clarity

---

Diagram Walkthrough

flowchart LR
  A["Invalid Estimate Detection"] -->|Direct Reset| B["Position/Velocity Set to GPS"]
  C["Correction Calculation"] -->|Limit Corrections| D["Constrained Corrections Applied"]
  E["Uncertainty Update"] -->|Bounded Growth| F["Capped EPH/EPV Values"]
  B --> D
  D --> F

Loading

File Walkthrough

	Relevant files
Bug fix	navigation_pos_estimator.c Correct estimate initialization and add correction limits src/main/navigation/navigation_pos_estimator.c Changed invalid estimate initialization from accumulating corrections to directly resetting position and velocity to GPS values Added correction limiting loop to constrain position and velocity corrections within INAV_EST_CORR_LIMIT_VALUE bounds Simplified velocity decay calculations by removing redundant 0.0f - operations Added constraints to EPH and EPV uncertainty estimates to prevent unbounded growth Improved code formatting with consistent spacing Changed loop variable declaration from int to uint8_t for type consistency +20/-14
Configuration changes	navigation_pos_estimator_private.h Define correction limit constant                                                  src/main/navigation/navigation_pos_estimator_private.h Added new constant INAV_EST_CORR_LIMIT_VALUE set to 2.0f to limit maximum position/velocity correction per loop iteration Constant corresponds to approximately 40m residual error for x/y position at default loop rate +2/-0

---

[qodo-code-review]

PR Compliance Guide 🔍

All compliance sections have been disabled in the configurations.

[Jetrell]

Tested this today. And it was like the old days of INAV :-)

I used one of my smaller copters that has poor separation and rarely gets a good satellite fix. Meaning it often has low position accuracy.
All three tests only had 6 to 7 Sats. With a HDOP ranging from 2.9 down to 1.9.

The first test was with these commits.
The second was with #11255
And the third was back to these changes again.

In the first test. I took off and switch straight to Poshold. And it sat there rock sold on the XY. I was really amazed!
With only slight wandering up and down occasionally. But only by half a meter of so. Which is typical for this copter when there is some breeze.

The second test with the commits from #11255. It experienced the same XY wandering as we have come to expect over the last few releases, when the GNSS position accuracy is poor.
@sensei-hacker However with that PR, I experienced a huge drop in navPos2 altitude. Not corresponding to the baro altitude at all. This caused the copter to drop out of the sky. And full throttle in Poshold wouldn't respond at all.
Not sure what the go there was. But it maybe interesting to see how it would go when added too the commits in this PR.

The third test was going back to this PR again. This time it had 7 Sats.
As soon as I again switched to Poshold, the copter just sat there still on the XY axis again.
With only a little more wondering on the Z axis. Due to the wind picking up.

I have to say. It was so refreshing to see a copter that has poor GNSS position accuracy, just sit there. And not wonder all over the place in Poshold. This is how I remember it before INAV 7.1.
Always thought that ACC vibration clipping factor was causing the problems.

[breadoven]

@Jetrell I wouldn't expect #11255 to have any affect if your using Baro given the PR is related to Surface mode surely. Or was the quad using sonar ?

I think that the ACC vibration clipping correction adjustment using posEstimator.imu.accWeightFactor may be OK in principle but I don't think it's been implemented correctly. It's too crude and causes unrealistic corrections when there's vibration at play so it really needs fixing properly over and above the constraints included in this PR.

One thing that was apparent when I was looking at this stuff for this PR was the fact that the estimation correction calculations are updated at loop rate. The issue here is that the sensor readings are updated relatively slowly so you're repeatedly applying corrections back to the last sensor reading even though that sensor reading rapidly became invalid to use as reference for correction. It would seem better perhaps to only apply a single correction on each sensor update with the estimate in-between sensor updates reliant on the accelerometer derived estimations. I did do a quick test using this method and it sort of worked but ended up with larger EPH/V values than you'd expect but that might just have been down to the bias values needing adjustment and other parts of the code tweaking. I'll test some more and see if this method works better, it would certainly cut down on what seems to be unnecessary number crunching.

[Jetrell]

Just out of interest, not criticism. How did you settle on 40 correction per update ?

I had wondered about adding a bent prop in testing. To induce some vibrations. Just to see if there is much difference in position drift.

[breadoven]

Just out of interest, not criticism. How did you settle on 40 correction per update ?

I had wondered about adding a bent prop in testing. To induce some vibrations. Just to see if there is much difference in position drift.

The 40m is just an arbitrary guess at a figure that's big enough to not interfere with the max correction that might realistically occur without allowing an excessively large correction value that will cause instability. I think you could probably get away with a smaller figure given this is the correction over 1s.

I did some more HITL testing with the idea mentioned above and it it seems to work fine. So corrections are only applied once, immediately after the GPS, Baro and Flow sensor are updated. I haven't tested the Flow stuff because I don't think HITL can do that, or at least I haven't checked if it can, so that would need testing for real which I can't do since I never bothered with rangerfinders. However, this is HITL testing so I don't know how useful it is other than to prove the basics. Would need to do some real testing to understand if it works better than the current method of correcting every loop to an old, out of date sensor reading.