Nav velocity Z estimation improvements

[breadoven]

PR should improve behaviour of Nav velocity Z estimation immediately after arming. This can show significant drift that upsets navigation altitude hold stability especially on multirotors.

Changes:

1. Velocity Z is held at zero prior to first arm.

2. Setting inav_default_alt_sensor added for preferred default sensor to use. Options GPS and BARO allow use of both sensors together but control behaviour when an altitude error exists between the Baro and GPS. Currently this is hard coded to only use the GPS which seems an odd choice given GPS altitude is generally considered unreliable compared to a functioning barometer. Selecting BARO now makes it possible to only use the Baro instead of the GPS. There are also an additional 2 options GPS_ONLY and BARO_ONLY which allow the user to only use the selected sensor even if the other sensor is working OK. The other sensor is only used as a backup if the selected sensor is unavailable/fails.

Seems to work as expected testing on multirotor and fixed wing but more testing needed.

[breadoven]

Setting added for preferred default sensor to use if there is an error between the Baro and GPS altitudes. If BARO is set the GPS won't be used if the altitude error exceeds 4 * inav_baro_epv. If GPS is set the Baro won't used if the altitude error exceeds 2 * inav_max_eph_epv. Tighter margin is used if the BARO is the default sensor given it shouldn't drift anywhere near as badly as the GPS altitude.

Not sure about the gravity zero datum issue. Checking other boards they don't seem to drift much at all after power up so it seems board related ... temperature related drift probably. Maybe this is worse for stacks which heat up more ?

[Jetrell]

I'd notice the dependency on GNSS altitude as the primary data source, to be more of an issue for my copters that have RF noise emitting hardware in the locality of the GNSS antenna. Meaning it also takes longer to get a 3D fix, as well as poor vertical estimation.

I set inav_default_alt_sensor = baro for this copter.
I observe the difference between navPos[2] and the baro was less at this temperature, (6°C) than it would be normally on a cold start with this copter. The difference ranged from 0.7 to 1.2m. With the baro being higher.. With 8 Sats and a HDOP of 1.7.

While on the second arming at the same location and same HDOP. The difference between navPos[2] and baro was consistently < 0.3m.
Showing the second arm was still more precise, as was always the case.. But it would seem the difference on the first arm and flight is now better than I normally see with the DPS310... I often see its first arm vertical estimation being out by up to 7m.

I wonder about the effect of ins_gravity_cmss being set at the acc calibration. And that at a specific temperature.

[breadoven]

I noticed recently, pre this PR, that altitude hold performance on multirotors is pretty bad initially then gradually improves. It was so bad at times you almost couldn't control it using the throttle stick, having to use full stick movements at times just to stop the drift. This was probably a combination of the zero gravity datum issue and GPS altitude inaccuracy with a low satellite count.

Doesn't make sense to me to default to using the GPS. The barometer should always be more accurate than the GPS when working properly and I'd expect it to either work fine or not work at all so any inaccuracy between the baro and the GPS is going to come from the GPS not the baro. Only issue is INAV might not pick up a failed baro if it's just outputting nonsense. If you're lucky it won't calibrate properly. Goes back to #8233 which from what I remember was never resolved properly.

[Jetrell]

I noticed recently, pre this PR, that altitude hold performance on multirotors is pretty bad initially then gradually improves. It was so bad at times you almost couldn't control it using the throttle stick, having to use full stick movements at times just to stop the drift. This was probably a combination of the zero gravity datum issue and GPS altitude inaccuracy with a low satellite count.

I'd noticed its only started happening this year.

I'd been running some test by hovering and flying figure 8's line of sight, then returning to a hover.. All this being done in Angle mode. Both with and without a GNSS sensor fix. (logic control switch on the radio)
The observation showed accelerometer instability occurs in Angle mode with a GNSS sensor fix, at the moment the copter was brought back to a hover. It would start to drift backward very quickly. I had to push a fair amount of forward stick to halt its motion.
While if I flew the same again in Angle mode without a GNSS sensor fix. I could bring the copter back to a hover with no accelerometer drift at all.
I performed this test many many times with the same result.

I'm aware that w_xyz_acc_p was replaced in 7.1, with a dynamic vibration clipping factor. And I'm thinking it may be over doing it. Especially when the temperature is colder. 4c4a74c
What I mean is. The accVib and gyro spectrograph plot are always worse on the first flight. Either with planes or copters.
I have placed this down to the inflexibility in the hardware, and specifically the props, for the first while, until the stresses are released as things warm up a bit.

Others have reported the same effect with 7.x. But they only provided limited data on the matter. So we could only guess if it was totally GNSS accuracy related.. But then I stumbled upon the above.
If this is the case. It could be effecting position XYZ estimation differently, based on hardware or conditions.

[breadoven]

Removed gravity calibration changes since it appears to mainly affect one board so could be a board issue. (Baro calibration also drifts on this particular board more than others).

[mmosca]

Removed gravity calibration changes since it appears to mainly affect one board so could be a board issue. (Baro calibration also drifts on this particular board more than others).

Do you have the same board with same issue as #10308 ?

[breadoven]

Removed gravity calibration changes since it appears to mainly affect one board so could be a board issue. (Baro calibration also drifts on this particular board more than others).

Do you have the same board with same issue as #10308 ?

No different board, HGLRCF722 in a quad stack. I did notice though that the gravity calibration drifts in one direction before flight then back again after flying possibly because prop wash cools it down so would seem to be temperature related. I didn't check the temperatures though. need to do that to see if there's any correlation.

[breadoven]

Looking at a recent fixed wing log I was trying to work out what was causing odd vertical velocity changes and it became apparent it's probably the GPS vertical velocity that's now used in the estimate. It would randomly change in a way that wasn't reflected by the Baro measurements or even contradicted by the Baro measurements. Even though GPS velocity has a small bias applied to the vertical velocity estimation it still seems to significantly affect it, probably because the magnitude of the GPS velocity drift can be relatively large at times. Not really convinced GPS vertical velocity is reliable enough to be used the way it is. Next testing I do will be with it turned off to see if it makes vertical velocity estimates less erratic.

Ideally I'd prefer to have the option to not use GPS at all for vertical position/velocity estimates unless the Baro is unavailable when it would be used as a fall back only (how it used to be until recently).

[sensei-hacker]

probably the GPS vertical velocity that's now used in the estimate. It would randomly change in a way that wasn't reflected by the Baro measurements or even contradicted by the Baro measurements. Even though GPS velocity has a small bias applied to the vertical velocity estimation it still seems to significantly affect it, probably because the magnitude of the GPS velocity drift can be relatively large at times. Not really convinced GPS vertical velocity is reliable enough to be used the way it is. Next testing I do will be with it turned off to see if it makes vertical velocity estimates less erratic.

Ideally I'd prefer to have the option to not use GPS at all for vertical position/velocity estimates unless the Baro is unavailable when it would be used as a fall back only (how it used to be until recently).

From what you've seen in the logs, I wonder if what I would predict is true. I would expect that the vertical velocity from GPS would be poor compared to baro. There's short-term "jitter", a meter up, a meter down, which presents as sudden spikes of fairly large velocity. I would also predict that over the long haul, on a long distance flight, baro position would slowly drift perhaps by many meters because it never gets recalibrated in, while GPS would continue to have the same accuracy as it has at launch - a meter or two. So GPS would be useful for vertical position, but not so useful for vertical velocity. Because GPS doesn't slowly get worse and worse over time. Does that match up with what you're seeing?

[breadoven]

From what you've seen in the logs, I wonder if what I would predict is true. I would expect that the vertical velocity from GPS would be poor compared to baro. There's short-term "jitter", a meter up, a meter down, which presents as sudden spikes of fairly large velocity. I would also predict that over the long haul, on a long distance flight, baro position would slowly drift perhaps by many meters because it never gets recalibrated in, while GPS would continue to have the same accuracy as it has at launch - a meter or two. So GPS would be useful for vertical position, but not so useful for vertical velocity. Because GPS doesn't slowly get worse and worse over time. Does that match up with what you're seeing?

That's not what I see in fact. After a 15 min flight out to over 1km the Baro altitude was 1m at arming and 3m on landing but the plane landed probably 2m higher than the arming altitude, so the baro altitude barely drifted at all. I don't see any reason why it should other than thermal affects. However this probably depends on the specific FC/Baro used etc, some may be better than others. The GPS on the other hand was 8m out between arming and landing. I did also notice some odd random offsets in GPS altitude in the order of several meters when the Baro showed no altitude change, a bit strange ... couldn't see any reason for it. It wasn't a gradual drift more like a sudden switch in the altitude estimation.

I'd say GPS altitude is probably more accurate than GPS vertical velocity but then again how is it measuring velocity other than from altitude rate of change ? Baro might be affected by temperature and pressure effects within a fuselage but otherwise should be reasonably accurate even over an extended time period surely.

[breadoven]

Added 2 additional options for default altitude sensor setting, GPS_ONLY and BARO_ONLY. With these settings only the selected sensor will be used even if the other sensor is working OK. The other sensor will only be used if the selected sensor isn't available/fails.

[Jetrell]

I've seen a lot of conflicting log data over the last year.. Most of which appears to be centered around GNSS data, more so than barometer. Although, I wouldn't say a barometer is highly trustworthy either.. But GNSS data seems to randomly fluctuate more wildly now, than I'd experienced in my years of testing INAV before.

The Avatar HD VTX's are responsible for a reduction in position accuracy (emitted RF).. I can quantify that back to back on many builds, against Analog and DJI WTFOS, which maintain consistent and slightly higher level of Z and XY accuracy in Poshold.

I did also notice some odd random offsets in GPS altitude in the order of several meters when the Baro showed no altitude change, a bit strange.

I have also experienced this in logs, and cross referenced it to the DVR footage.. Sometimes its related to a bank or pitch angle change in a mission, for either a plane or copter.. I would consider both these events somewhat understandable.
But its the times when GNSS accuracy just reduces out of nowhere, that has me wondering.
Maybe the kalman and complementary filters have more trouble dealing with EPH/EPV shifting from the higher accuracy now obtainable, to a sudden reduction in position accuracy, with the loss of as little as one prominent satellite.. Its not uncommon now to see EPH get down to 0.3m with 4 constellations (HDOP = 0.8).. While I can rarely remember GPS alone getting below 1m. (HDOP = 1.1)

[Jetrell]

@breadoven I ran many dozens of test with your last commit. And went over the logs and line of sight footage from the flights many times.
The log data is very often hard to decipher unless you compare it with the actual height it is hovering above the ground.

To give some perspective. These tests were done in Poshold. With copters that have the highest quality GNSS modules I have. Both M9 and M10, with plenty of RF separation and the VTX's turned off.
With respect to inav_default_alt_sensor. I consistently found BARO ONLY showed greater signs of altitude drift than GNSS altitude. With GPS ONLY working the best with these modules.
From observation, both LOS and logs, showed GPS alone, or Galileo, provided the most stable altitude position.. While altitude consistently drifted more when Beidou and/or Glonass was enabled.
This had me stumped for a while, because EPH and EPV are always lower when these extra constellations were enabled.

But after looking over and over the logs and the LOS footage.. It appears the issue is related to the now increased precision available with these extra two constellations. And that combine with higher precision GNSS modules.. And this is linked to the way navTgtPos[2] altitude target is captured, when Poshold is enabled, or when the throttle stick is moved to change the altitude target.
The example is this... When Poshold is enabled, the target altitude is set based on vertical estimation precision at that point in time. But if you let the quad sit there in a fixed hover for e.g. 45 seconds. The altitude precision can increase or decrease. Leading to the estimated altitude, moving closer or farther away from the ACTUAL altitude. And that based on what it ASSUMED was accurate, when the original target was set.
So I ran a test, by moving the throttle stick up and down a tiny bit to reset the altitude target. every 20 seconds or so... This allowed it to maintain a constant altitude in the hover for the whole battery pack.

[breadoven]

But after looking over and over the logs and the LOS footage.. It appears the issue is related to the now increased precision available with these extra two constellations. And that combine with higher precision GNSS modules.. And this is linked to the way navTgtPos[2] altitude target is captured, when Poshold is enabled, or when the throttle stick is moved to change the altitude target. The example is this... When Poshold is enabled, the target altitude is set based on vertical estimation precision at that point in time. But if you let the quad sit there in a fixed hover for e.g. 45 seconds. The altitude precision can increase or decrease. Leading to the estimated altitude, moving closer or farther away from the ACTUAL altitude. And that based on what it ASSUMED was accurate, when the original target was set. So I ran a test, by moving the throttle stick up and down a tiny bit to reset the altitude target. every 20 seconds or so... This allowed it to maintain a constant altitude in the hover for the whole battery pack.

But this is just resetting the target altitude to the current estimated altitude which compensates for the fact it's drifted over time due to the GPS altitude having drifted, something I'd have thought is only loosely linked to the GPS altitude precision, i.e. you could have better precision but the altitude reported by the GPS remains unchanged depending on how the numbers work out. As you say it's only obvious when you can visually see the altitude drift by comparing with a ground reference or I guess you have LIDAR giving you ground distances.

I'm surprised the Baro doesn't work better by itself given it shouldn't really be that affected by drift if working correctly. Maybe some Baro's are better than others. It drifts on the HGLC board I was checking the other day, together with the IMU ... both caused by temperature changes. Other boards don't seem to suffer this same drift because the temperature reported by the IMU is much more stable (barely changes at all after power on). Atmospheric pressure changes would also throw off the Baro calibration but this would take some time to have an affect unless there's some significant weather event happening. I did think it wouldn't be too difficult to recalibrate the Baro against the GPS in flight to compensate for atmospheric pressure changes just so long as you use some heavy filtering of the altitude error between the sensors. Hard to see that this would make sense though for the typical 20-30 min flights and it still comes back to the question of ... which sensor do you trust the most.

I think the main benefit of this PR is it lets you choose how to use the Baro and GPS altitude sensors, i.e. use both with priority given to one when they disagree ... or only ever use one with the other still available as a backup if the chosen one fails.

[sensei-hacker]

I notice you both mentioned temperature effects. Am I remembering correctly that INAV doesn't currently do temperature compensation, TCO, for the baro? I see the typical TCO of the BMP280 is 12.6cm /k(or C). Which is somewhat significant. It sounds like there is discussion of which sensor may be best, given the temperature drift.

Would it perhaps make sense to look at using temperature compensation, if indeed it's not already used? I might be able to look into that next weekend or so.

[breadoven]

I notice you both mentioned temperature effects. Am I remembering correctly that INAV doesn't currently do temperature compensation, TCO, for the baro? I see the typical TCO of the BMP280 is 12.6cm /k(or C). Which is somewhat significant. It sounds like there is discussion of which sensor may be best, given the temperature drift.

Would it perhaps make sense to look at using temperature compensation, if indeed it's not already used? I might be able to look into that next weekend or so.

Well there is #10308 that compensates for temperature drift. Not sure why it was closed. Only problem with it is it's not very user friendly having to work out the correction values.

[Jetrell]

i.e. you could have better precision but the altitude reported by the GPS remains unchanged depending on how the numbers work out.

Unless the kalman filter has a constant to limit fusion accuracy, I'm not sure how that could be.. Being that zero meters is absolute accuracy.
And if the vertical estimator does have a filtered limit, surely it was designed to obtain the best accuracy possible 8 years ago.
And as you said, if the Target Altitude does not have an absolute ground reference when set, then its expected the GNSS altitude will start drifting towards the incorrect target, as position accuracy becomes more trusted, the longer a copter is held in Poshold.. I often see this occurrence after first fix, when the copter is placed in Poshold with only 8 Sats and HDOP of 1.6. It will set the Target Altitude at something like 3m. While in reality the copter is only 1m above the ground. Then once the precision starts to increase, the copter will start to climb to the position the incorrect target was set. i.e 3m

With the M9 devices, I've seen navEPV get as low as 32, with 34 Satellites. Which is extremely precise.. While it generally averages around 200 much of the time.. This sort of variance between those figures, can produce an inaccuracy of 2 - 3m.

I'm surprised the Baro doesn't work better by itself given it shouldn't really be that affected by drift if working correctly.

Being mindful that my tests were based on a Poshold ground reference, as with the GNSS altitude..
While on a fixedwing that doesn't have a pressurized fuselage i.e. the outlet is larger than the inlet.. Barometric altitude is generally accurate enough. As it is for a copter in a mission, unless the mission is set close to the ground.

It drifts on the HGLC board I was checking the other day, together with the IMU ... both caused by temperature changes.

I've also found this with every board I have, that uses the DPS310/SPL06 barometer.
Over the years I've tested this barometer against the BMP280 many hundreds of times, in controlled situations.. And found the DPS310 to be effected by temperature considerably more.

[sensei-hacker]

While testing altitude control, it may (or may not) be useful to include the new #10367