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.

[Jetrell]

I tested this again with the last commit. Didn't have much time, so I only got in one flight. But it once again held its XY position for 4 minutes in Poshold. Wandering no more than 20cm at most.
And the Z was also more solid this time too.. Not sure if that was due to no wind at all blowing across the baro. Or because I had 10 satellites this time.

I hoped to analyze the log. But this was the first time I ever remember this F722 FCs flash memory not recording anything.. Likely logging rate. Ive always had it 20%. I'll see how it goes with 13% from here on in.

I tried making some changes to the inav_w_z_p baro and gps settings with the last commit. I feel it may have helped a little. But its often hard to say conclusively when testing on a day that has wind.. Have you tried making any tuning alterations to those settings ?

[breadoven]

I think I'll merge this because the main change, removing the GPS reset using corrections, is easy to revert if need be and the constraints are required regardless.

[sensei-hacker]

Could you do me a favor, for the next 48 hours don't merge stuff? I'm in the middle of making 9.0.1.
I actually had two more PRs to merge and we would have had 9.0.1 out the door.

I'll find a way to redo it. Probably make a different branch to rewind and then rebase the commits that need to be in 9.0.1, modify the CI to build that branch instead , wait for nightly, build the release, then merge those back into maintenance-9.0.1

[breadoven]

Oh sorry for that, didn't realise there was a 9.0.1 in the pipeline. Do you not want to include this PR given it's a fix for something that seems to be causing not insignificant problems all of a sudden ?

[sensei-hacker]

Do you not want to include this PR given it's a fix for something that seems to be causing not insignificant problems all of a sudden ?

Getting rid of the indirection through the "correction" value seems like a good idea and I love that you did it. This PR also touches enough code that I would like to have more than one person fly it before we declare that it's a pure fix and there is no typo that could cause any other problem. Am I understanding correctly you haven't flown it, right? Maybe have someone try it on a plane once. :)

[breadoven]

Yes that is the main reason for not using it now ... it needs more testing to be certain it doesn't cause other unexpected behaviour.

[sensei-hacker]

That reminds me, is there a good way to kinda keep you in the loop of what's going on, given you aren't on Discord?

You do important work that very few people CAN do. I don't want to be leaving you out, but generally we communicate in the Discord channel. Any good way to let you know about release plans and that kind of thing? And to be able to get your input? You're a wise old-timer compared to me and it would be cool to hear what you have to say about things.