Position estimator improvements

[breadoven]

Changes to position estimator mainly intended to improve issues with Z velocity accuracy but also includes code clean up and fixes:

1. posEstimator.imu.accelBias changed to Earth frame removing unnecessary Earth to Body conversion. INAV_ACC_BIAS_ACCEPTANCE_VALUE application changed to make more sense, i.e. accelBias is now always calculated not only when the accBiasCorr resultant is less than INAV_ACC_BIAS_ACCEPTANCE_VALUE
2. Signage of accBiasCorr corrections flipped so their effect is more obvious, i.e. under estimations require +ve corrections rather than -ve which appears counterintuitive
3. estVelCorr calculation based on PosResidual removed since this appears more of a fiddle factor than anything else. estVelCorr is now solely based on VelResidual which would seem more appropriate.
4. accBiasCorr calculation changed to include VelResidual. Based on the logic that velocity is largely derived from acceleration so acceleration bias should include velocity.
5. Corrections for Z axis doubled if only a single sensor used to compensate for the fact that corrections are applied for Baro and GPS when both sensors are used with the corrections for each sensor likely to be similar effectively doubling the magnitude of the final corrections.
6. Default settings increased to make correction factors more effective
7. Redundant function navGetAccelerometerWeight removed

HITL testing shows the changes work as expected, i.e. logic makes sense, but flight testing will be required to prove any real world dynamic benefits.

[Jetrell]

A few of the changes you made, do make more sense than what was there.

I've done more testing of this PR with a copter that has always been questionable, with respect to its barometric read. Not saying its DPS310 is broken, or can't ever get a good read. But its sure has its moments of inaccuracy. Especially when a rapid altitude change is made.

I'd noticed awful copter vertical estimation inaccuracy through 7.1 - 8.0 in most logs I viewed with this specific copter, more so than the others. Although none of them where heaps better in the area I'll explain next.

When I would make a forward vertical descent in Acro or Angle mode. The barometric altitude would drop relatively normally. But the estimator would often experience some odd contradiction with the Acc[Z] and navVel[Z]. Which would drop the Z estimated altitude far below the barometric altitude. Even below ground arming level. Like -25m. And this was with 20+ Sats and a HDOP below 1.1.
When this happened. And a navigation mode was enabled. Setting the navTgtPos[2] to something wild like -20m, if the baro is only out by a couple of meters. Caused all sorts of vertical velocity drift.

While in the tests I have conducted so far with this copter, using this PR.. I haven't seen the same Acc[Z] issues.
Only the normal barometric drift, within a meter or two occasionally, out of nowhere. Which is a pain in itself, but not related to these updates.

I still have more testing to do on a day when its colder. So it can provide a better idea of any effect temperature may have, comparing to the current tests.

[breadoven]

I was looking at the log from the recent multirotor crash caused by toilet bowling trying to understand why the vel Z was so inaccurate when it started a stable RTH landing causing it to think it was descending at 0.5 m/s when in fact it was climbing at 7 m/s. Part of the reason was because I'd removed the Vel Z correction derived from position error and forgotten to increase the Vel Z correction bias value so the correction was ineffective. However, this still didn't explain why the Acc Z driving the velocity estimation seemed to be wrong. Checking before it stabilised for the landing when pitch and roll were all over the place the estimated vel Z was showing as -8m/s at one point when in fact the GPS and Baro both had it climbing at 6+m/s. It appeared that the Nav neu acc Z was driving the -8m/s but it's hard to see where that came from given it was in fact climbing. Something seems to get confused when there's a lot of pitch/roll happening at speed, dynamic G forces perhaps. I couldn't entirely work out what was driving the problem in the end, possibly screwed up acc biasing.

I'm looking at adding some sort of sanity checker for the position estimations. It seems nuts to continue using estimations that are clearly wrong compared with GPS and Baro measurements. GPS and Baro obviously have some inaccuracy themselves but it's nowhere near as bad as the estimations can be when things go wrong. Currently if the checker considers the estimate for an axis to be blatantly wrong it simply defaults to using the GPS values instead until the estimate becomes sane again. As with all sanity checkers this is really just a last resort check to prevent runaway situations. Should be able to test it in HITL to see if it helps at all.

As for this PR it seems to work fine on multirotor and fixed wing. There may be an improvement on before especially for multirotor which does seem to hold altitude during Poshold better even with gusty wind. There appears to be less Vel Z drift overall perhaps.

[Jetrell]

I'm looking at adding some sort of sanity checker for the position estimations.

Yeah. It would certainly help. But which sensor to trust isn't always an easy one to know.
I was running some more tests with these commits today. And seen this occur.
It's not an attack on your work here.. Its just an example of how messed up things can get.

And in this case it looks to be primarily due to the loss of a couple of prominent satellites, as the copters was pitching to slow down in Poshold. Which caused the navEPV to double at that time.. Then look how the vertical estimator reacted, compared to the actual visual altitude.

The video's a tiny bit out of sink. But close.

Log.mp4

[breadoven]

Well there's a load of problems from that video. Acc Z is changing abruptly although this may just be in response to the abrupt changes in throttle. But the change in est altitude when navEPV exceeds 10m doesn't make sense. I assume you're using inav_default_alt_sensor = GPS. Need to check what happens when EST_Z_VALID becomes invalid (navEPV exceeds 10m). Something is getting reset causing an unrealistic change in est altitude. Would be useful to include GPS data in the log file, GPS Altitude, GPS_velned[2] and GPS_epv.

I also need to do more testing with this. I realised I've been using it with BARO_ONLY since this works best for me by a country mile. Using GPS just seems to be unreliable ... as you might expect given GPS altitude is generally inaccurate compared to the horizontal position. Using Baro is much more consistent with the quad now holding altitude as it should do.

Edit:
And the other obvious problem which has been mentioned before is the altitude and vel Z contradict each other. You can't have a significantly -ve vel Z when the altitude is increasing, something that's very obvious in the video log.

[Jetrell]

Yes inav_default_alt_sensor = GPS is still using default.. Mostly because this copters baro is always incorrect the first boot. Then mostly okay each flight session after.. I've often wondered why the capacitive element of the DSP310 will drift like this on the first boot. It even seems irrespective of temperature. I also see this on some of my planes with this barometer. They all have open cell foam covering the sensor.

Would be useful to include GPS data in the log file, GPS Altitude, GPS_velned[2] and GPS_epv.

That would certainly help to track down the cause with more certainty.

I haven't checked the navigation code for this.. Do you know if the level of trust placed on GNSS data, is also reduced as the bank angle increases ? I assume EST_Z_VALID should cover this. But one is more reactive and the other proactive.

I've noticed similarity in many log reviews over the years, on multiple copters, that show an increase in position error as the copter banks.. It isn't always the case. But I guess it shouldn't be surprising. Depending on the satellite placement in the sky at a given time of the day.
e.g. There may be a greater number of usefully spaced satellites in the east, providing good position accuracy. And if the copter banks, so the GNSS modules antenna faces the west. Then we're likely to experience what my test above showed.

[breadoven]

The problem wasn't a worsening GPS_epv it was the error between the GPS altitude and the estimated altitude, called GPSresidual in the code, that caused the gradual increase in navEPV. The GPSresidual was added recently to the calculation of navEPV and it's that that likely caused the navEPV to exceed 10m because of the growing error between GPS altitude and estimated altitude. I'm guessing GPS altitude increased roughly in line with the Baro, i.e. +20m, when the estimated altitude only increased by around 3m which would have resulted in a significant GPSresidual value. When the navEPV reset it reset to the GPS_epv value, i.e. around 450 (similar to the navEPV value at the start of the video which was largely driven by the GPS_epv).

[breadoven]

Any chance of the log for that flight @Jetrell ? Might be useful for working out what's generally causing Z axis position estimate problems.