Sub: Yaw drift issue

[Williangalvani]

We can sporadically see a persistent yaw drift despite having a very good magnetic heading.
This happens randomly and seems to away when rebooting.

This has happened both on Navigators and Pixhawk 1

It seems to be related to SUB having INS_GYR_CAL=0 for default, which can lead to significant gyro bias at boot.

We have multiple reports of it happening in 4.0.
Related thread in discuss: https://discuss.bluerobotics.com/t/steady-uncommanded-yaw-in-depth-and-stabilize-modes/10260

As it is not trivial to replicate, I'm not 100% sure if this still happens in master.
We still need a good log with LOG_DISARMED=1 and LOG_REPLAY=1.

Current reports/logs:

gcelec at discuss.bluerobotics

[rmackay9]

I think this can be reprodcued in SITL by doing the following:

• INS_GYRCAL = 0 (the default for sub anyway)
• set EK3_IMU_MASK = 1 (only a single IMU and EKF core to simplify things)
• set by setting INS_GYROFFS_Z 0.2 <-- bigger values might be required..

To resolve it a couple of things come to mind:

1. increase the EKF's maximum gyro bias (I have no idea how to do this but there is probably a definition)
2. try and get the gyro cal working even on a rocking boat. It probably can work but may need to have the acceptable range of motion increased greatly.

[Williangalvani]

Thank you for the tips!

increase the EKF's maximum gyro bias (I have no idea how to do this but there is probably a definition)

A while ago I tried increasing EK3_GBIAS_P_NSE with interesting results: it stopped spinning but got oscillatory.

If I'm able to replicate this in master, I'll check the gyro offsets and try to play with this parameter again.

[rmackay9]

@Williangalvani,

OK. It wouldn't be the noise or the igate parameter that would help. Instead there is some inbuilt limit on how large the gyro biases can get.

Maybe it is this line?

            // prevent a single large error from contaminating bias estimate
            const ftype bias_limit = radians(5);
            error.x = constrain_ftype(error.x, -bias_limit, bias_limit);

This is not quite what I imagined when I heard about it from @priseborough but maybe this is it.

[Williangalvani]

Interesting, thanks! I'll experiment with that.
I'll focus on the other issues for now, maybe it happens again here and I'll have a good log to work with. Or we get an user to replicate it.

[Williangalvani]

I'm closing this as I haven't been able to reproduce it in 4.1,
If someone runs into it on >= 4.1, please re-open.

[Williangalvani]

Re-opening. Some people are still seeing this. Upon further investigation, the issue seems to be that the ekf is unable to estimate the earth magnetic fields with no origin set.

The current solution I have in mind is to create parameters for default lat/lon in vehicles with no positioning systems.

We'll also need to tweak some code in the ekf in order for it to check for origin set instead of happy GPS.

This seems to be easier to reproduce in some places than others, probably due to the local magnetic field inclination/declination.

[Williangalvani]

The way forward for me:

• Make all calibrations take Lat/Lon from GCS if there's no positioning sensor
• Create params for backup lat/lon, used for the WMM
• Make sure we have a pre-arm check that complains if we try to arm with no position/backup position

[clydemcqueen]

Thanks for the tips, @rmackay9. I was able to induce yaw drift in SITL and eliminate it by causing a yaw / mag reset.

Here is the trick to causing a yaw / mag reset in sub:

• arm
• descend deeper than 2.5m
• disarm
• arm -- this will set posDownAtTakeoff to the current altitude
• ascend

At some point the sub will be 2.5m higher than posDownAtTakeoff, finalResetRequest will become true, the yaw and mag settings will be reset, and 3d mag fusion will start.

(I scanned several dozen tlog files for real dives and found 2 dives where this actually happened.)

I took a stab at making it easier for the pilot to cause a yaw reset in #26394 -- basically flipping the sign so you have to go down 2.5m, not up. But now I'm thinking sub should ignore posDownAtTakeoff altogether and request a "final" reset every time you arm the sub when it is below 1m. Perhaps there is a better way to sense "away from metal structures" for sub?

I do like @Williangalvani 's idea of using parameters to set the default origin. Right now the table_earth_field_ga field is set from the latest mag reading every time the mag settings are reset, which will happen on boot (usually on the dock or boat) and again whenever you switch from 1d to 3d fusion (which can be on the boat, the dock, in the water, etc.) It would be much better to use a table lookup.

[rmackay9]

@clydemcqueen,

Very interesting. So another solution may be to change the EK3_MAG_CAL parameter default from 3 (After first climb yaw reset) to perhaps 2 (Never) or 0(When Flying). Rover uses "2", Plane uses "0". Copter uses "3".

[Williangalvani]

Another note:
We need to change this so it takes any position sources, not only gps:
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Compass/CompassCalibrator.cpp#L1068-L1073

[rmackay9]

@Williangalvani,

Yes, we could add an or to that condition so that its either a 2D fix or a position estimate.

[clydemcqueen]

I bumped into yaw drift on a dive Monday while testing the Water Linked USBL system.

ArduSub V4.5.0-beta1 (bd91022)
LOG_DISARMED and LOG_REPLAY enabled

https://drive.google.com/file/d/1h2npsu_z4UO-8kSVQ9xcpAakQgzCqxX0/