[Bug] Gryro Drift Issue During Longer Runs - How To Debug

[DrTom]

This is from this years WRO competition. One of the teams is experiencing quite severe gyro drift. This is an advanced team with their own gyro drive function base on reading imu.heading() and feeding the deviation with a simpleproportional controller into drivebase.drive(). This is used throughout the run for all straight movements.

The main run is composed of 4 sections, where in between those sections realignment against the boarder is performed and imu.reset_heading(angle) as well as drivebase.reset() is called.

This works very well for the first 3 sections or when each of the 4 sections is performed in isolation. However, if all sections are run in sequence imu.heading() drifts off significantly towards the very end of the run. We have observed deviations of more than 15 degrees. The deviation is so large that this seems not to be a simple accumulation as well as running isolated sections is inconspicuous. The deviation is always towards the same direction.

So we would like to debug this (unless there is a immediate solution).

For one thing we would suggest to investigate the current state of internal drift compensation and see how it changes under certain circumstances and influences. Is there a way to read the values (and output them to the console)? Is there a way to force the hub explicitly to recalibrate during a run? How long would we have to pause the robot so that auto recalibration occurs?

Do you have any other ideas of what we can look at?

I can rebuild the firmware and perform some simple changes like changing constants if that helps.

[laurensvalk]

Thanks for your detailed feedback.

This works very well for the first 3 sections or when each of the 4 sections is performed in isolation. However, if all sections are run in sequence imu.heading() drifts off significantly towards the very end of the run. We have observed deviations of more than 15 degrees.

• Is the robot flat during its 4th run, or perhaps driving over objects or driving on ramps?
• Does it also happen if you do the first 3 sections, twice?

Is there a way to read the values (and output them to the console)?

You can use the print command for this. You can print multiple columns:

For example you could do this in a loop, or as part of the rest of your code.

print(timer.time(), hub.imu.heading(), hub.imu.angular_velocity(Axis.Z), sep=",")

Then copy the output to Excel to easily make a graph.

I would be especially curious if that 15 degrees builds over time, or whether there is a discontinuous jump.

This is used throughout the run for all straight movements.

Do you run straight with respect to the start of that maneuver each time, or do you try to maintain an overal compass-like angle?

The main run is composed of 4 sections, where in between those sections realignment against the boarder is performed and imu.reset_heading(angle) as well as drivebase.reset() is called.

It's worth trying drivebase.stop(), as well. This will also reset any "holding mode", in case you were using it.

[DrTom]

Some preliminary observations:

• We can reproduce the drift by only running the first 3 sections (in a loop), too. But it takes quite a bit longer to manifest: about 5 to 6 minutes.

• The fourth section shows the problem in much shorter time. There is more stopping, starting, forth and back, and also turning in the fourth section.

• We suspect there is a particular operation in the fourth section which makes this worse. But we are still trying to isolate that.

Apart from that we found an external factor: ambient temperature. In hindsight the worst runs seem to have happen either when the ambient temperature was relatively high or wenn there were a lot of runs in a short time (we suspect some heat accumulated inside the hub) or both together. After heating up the entire robot to 45 degrees Celsius (above freezing point) the problem manifested rather quickly and much more severely.

Does the IMU have temperature compensation? It seems to be an LsM6DS3(TR-C). I did not find anything hinting to this. However https://community.st.com/t5/mems-sensors/lsm6ds3-temperature-compensation/td-p/294475 seems to confirm the problem.

[laurensvalk]

we suspect some heat accumulated inside the hub

Good point!

Does the IMU have temperature compensation?

It can measure temperature, but does not automatically account for it. Potentially, we could do that in our own driver.

I've attached the datasheet below in case anyone is looking for some light reading over the holidays :sun_with_face:

lsm6ds3tr-c.pdf

These are the nominal sensitivities to temperature, for both the rate magnitude and the drift (the "zero rate"). Both will affect the result of a turn.

[laurensvalk]

On a slightly unrelated note, I think we can also improve the usual calibration on boot a bit. Right now, we wait for stationarity but assume 0 until then. We could potentially save the offset values just like we will be saving a few other IMU calibration settings. Then it will drift a bit less until the first calibration has occured.

[afarago]

Some preliminary observations:

• We can reproduce the drift by only running the first 3 sections (in a loop), too. But it takes quite a bit longer to manifest: about 5 to 6 minutes.

• The fourth section shows the problem in much shorter time. There is more stopping, starting, forth and back, and also turning in the fourth section.

• We suspect there is a particular operation in the fourth section which makes this worse. But we are still trying to isolate that.

Apart from that we found an external factor: ambient temperature. In hindsight the worst runs seem to have happen either when the ambient temperature was relatively high or wenn there were a lot of runs in a short time (we suspect some heat accumulated inside the hub) or both together. After heating up the entire robot to 45 degrees Celsius (above freezing point) the problem manifested rather quickly and much more severely.

Does the IMU have temperature compensation? It seems to be an LsM6DS3(TR-C). I did not find anything hinting to this. However https://community.st.com/t5/mems-sensors/lsm6ds3-temperature-compensation/td-p/294475 seems to confirm the problem.

@DrTom Would you be willing to show the phenomena on a video here or even in pm? During FLL, WRO I have not seen this with pybricks and would be keen to lean and debug.

[DrTom]

@afarago

Would you be willing to show the phenomena on a video here or even in pm?

Yes we can do either. However, it will have to wait until we are back from vacations in about 3 weeks.

[scatwang]

I run into a similor issue. I'm using SPIKE Prime hub on a drive base. The run takes about 20secs. If the hub is freshly rebooted, it works fine. After running for several times, it start to drift.

After reading this thread, I tried to stop the base for 1sec between steps and print a serise of hub.imu.angular_velocity(Axis.Z). When it started to drift, the angular_velocity was >0.1 (sometimes >1 even). I can see the heading drifts quickly. When it starts to drift, only reboot could get it back to normal (angular_velocity ~= 0). But If I run again at this time, it's more likely to drift again during the run.

I also suspect that heat accumulation might be the reason, because I used run_until_stall() a lot which might be power consuming and heat up the motor driver chips.

I will conitnue diagnose and post my finding and update.

[scatwang]

Update: I found the root cause, there is a drive_base.turn(-135) usually cause the problem. It was fixed after I break that down like

drive_base.turn(-45)
drive_base.stop()
drive_base.turn(-45)
drive_base.stop()
drive_base.turn(-45)

[laurensvalk]

Thanks for your note. Breaking it down into multiple moves does not impact gyro calibration, so this may be masking something else.

At the moment, the gyro calibrates mainly the first time it is stationary after boot, and then only gradually updates the calibrated value. Maybe we should measure the temperature and allow for faster re-calibration if the temperature has changed.

[scatwang]

Since break down helps, maybe temperature is not the cause. I have also tried shaking and tile the base and table while running, it can't be reproduced. Only turn(-135) in the middle of the run could trigger that.

I will try to reproduce with simplest steps and provide a code later.

[laurensvalk]

Does it really produce drift? That is, does hub.imu.heading() give a wrong value and does it drift up or down?

Or is the robot not moving by the intended amount?

Thanks for looking into this!

[scatwang]

Yes, after this -135 turn, it produces a non-zero angular velocity. After the -135 turn when drive base is static, if I print hub.imu.angular_velocity(Axis.Z) the value will be around 0.5 to 1.0 instead of 0.

[scatwang]

I just figured out how to trigger drifting and filed an separated bug since I don't know if it's the same issue.

See: https://github.com/pybricks/support/issues/1840

[laurensvalk]

Thanks @scatwang for making a small reproducible case! @DrTom - the likely outcome for now is that the default calibration thresholds are a bit too lax.

# Defaults
print(hub.imu.settings())

# You can set stricter thresholds, so it won't easily start to recalibrate while turning steadily.
hub.imu.settings(1.5, 250)

The thresholds had been relaxed for the previous firmware release after feedback in https://github.com/pybricks/support/issues/989. Because if the thresholds are too strict it won't calibrate in a competition hall with kids jumping up and down :smile:

This will be addressed by making your settings (as above) persist across reboots. So you can choose what works best for you. With that done, I think we can use stricter defaults, so you can relax them in your user script if needed. This is easier for the user than the other way around.

---

So this is quite possibly the same issue, but let's keep both open for now.

[laurensvalk]

If this was indeed the same issue discussed above, then the fix is available for testing using these instructions to try the latest version

It would be great to know if this fixes the issue you saw, @DrTom. Thanks in advance for your time.

[DrTom]

So, after the team rebuild the robot and reprogrammed everything it seems that the drift problem ist not gone. I can confirm our drift problem with a recent beta build of pybricks. I performed some quick tests today and it seems either related to motion of the hub and/or increase of internal temperature.

Some preliminary experiments from today:

1. after the hub was off for quite a while and then kept stationary (allowing recalibration): no (relevant) drift observable
2. then one or more runs following a stationary period (recalibration was prevented): we observed drift up to 20 deg/min
3. letting the robot/hub rest without allowing recalibration the drift seemed to diminish again; but that took quite some while, somewhere between 10 to 20 minutes

For the next tests I plan to recalibrate, then prevent recalibration and exert external temperature; all while the hub ist completely stationary. That should differentiate between temperature or some other cause.

[dlech]

An easy way to change the temperature is to turn all of the light matrix lights on or off.

See https://github.com/orgs/pybricks/discussions/675#discussioncomment-3341131

[scatwang]

@DrTom, after #1860 was fixed, I can still trigger a small drifiting (<0.1 deg/s) with a recent nightly build firmware. I will try to find the minimal reproduce steps. I believe it's either caused by calibration, or can be fixed by calibration.

[DrTom]

@scatwang

I can still trigger a small drifiting (<0.1 deg/s) with a recent nightly build firmware

We have also seen some small drift <= 2 deg/min; but that isn't much of a problem for a competition run. There are other causes (motion, some hits with elements) which cause the gyro deviate over time by a few degrees. It all depends but you would usually realign a few times (say 2 to 4 times) during the run which will take care of that and some minor drift.

However if the drift is in the magnitude of 20 deg/min you would have to realign (and give time to recalibrate) like 8 or 10 times during one run of 2 minutes. Even if that would be possible it would take between 30 to 60 seconds time.

[DrTom]

@dlech

An easy way to change the temperature is to turn all of the light matrix lights on or off.

That is very interesting (also the thread https://github.com/orgs/pybricks/discussions/675#discussioncomment-3341131). If the problem is indeed related to heating and the lights contribute it would mean that turning off the lights could alleviate our problems.

I actually thought about the LEDs but it seemed to me that the effect should be little compared to motor controls and battery drain. But maybe it is not. And also I have no idea where the IMU is located within the hub. If it is very close to the LEDs it could easily matter.

For the test I plan to use our kitchen convection oven; I assume about 50 or 60 deg Celsius should not cause (much) harm to the hub.

[laurensvalk]

Thanks for the additional input everyone! To help test this, I'll soon add an API option so you can do:

hub.imu.angular_velocity(calibrated=False)

In order to get the actual values from the sensor. Then we can better assess what those do, to eliminate builtin offset corrections or calculation errors we might have in the firmware.

if the drift is in the magnitude of 20 deg/min

I have not yet been able to reproduce this. If you find a reproducible case, that would be super helpful.

To double check, can you include the following in your code and share the result?

from pybricks import version
print(version)

Please also share the output of:

print(hub.imu.settings())

@DrTom , do you have any standard LEGO models built, such as the small or big drivebase? It would be super helpful if you are able to reproduce it on one of those, so we can compare.

---

There does seem to be some influence of the LEDS, though not major for the gyro at first glance. With the lights off, the average Z acceleration for my hub over one second is about 9681 mm/s^2. With the light on, it rises to about 9776, which is approximately 1% error. The effect on drift is very small, so doesn't seem to explain the higher than usual drift posted about above.

[laurensvalk]

@scatwang

I can still trigger a small drifiting (<0.1 deg/s) with a recent nightly build firmware. I will try to find the minimal reproduce steps.

This is indeed reproducible by putting it against something like a PC keyboard and then gently rotating it. This is because we use a stationary test where the angular velocity should stay under a threshold for a second in order to re-calibrate.

Due to some other recent developments where we will actually save the bias, I think we can choose to reduce the default threshold from 3 deg/s to 1 deg/s. This way it should be less common to accidentally move it slightly while calibrating.

Please experiment with the following script if you'd like to see the effect.

from pybricks.hubs import PrimeHub
from pybricks.parameters import Color, Axis
from pybricks.tools import wait

hub = PrimeHub()

#
# Default is 3. Reduce to 1 to make it more strict about calibration.
#
hub.imu.settings(angular_velocity_threshold=1)

while True:

    hub.light.on(Color.GREEN if hub.imu.stationary() else Color.RED)
    print(hub.imu.heading())
    wait(100)

[DrTom]

Results testing the LED impact on the drift. See the annotated log file and the python program for all details.

The hub was placed (inside the robot) on the floor in a room with about 23 deg Celsius temperature. The robot was not touched during the whole process. The room has wood floor glued to massive screed, so no shaking or something which could influence the result.

In the first test the hub was turned on, the LEDs were immediately turned off and than turned on after 5 Minutes. The test ran for 10 Minutes. We can see an ever increasing drift up to 13.5 deg/min towards the end of the test.

At first glance there seems to be something generally off and if there is an impact of the LEDs it is shadowed by that other thing. After some consideration I suspect that some heat inside the hub builds up until an equilibrium of generation and dissipation is reached.

So we repeat this test after the hub has been idle for a while, note that the center LEDs are on during that time.

In this second run we can see that the drift "recovers" towards the other direction up to -3.4 deg/min. After 5 minutes, when all the LEDs are turned on, the drift again shifts towards the other direction up to 1.1 deg/min at 9 minutes (the 10 minute mark is not present; a racing condition I took not into account, see the code). We could account for the positive drift by the fact that all LEDs are on now compared to only the 3x3 center LEDs when the hub is idle.

For now I would lean towards the following conclusion: drift is sensitive to heat produced by the hub itself; quite significantly so; even the LEDs can have a significant impact.

hub.log.txt stationary_drift_test.py.txt

[laurensvalk]

Thanks for checking! Can you share the script used to produce your results?

up to -3.4 deg/min

While not ideal, this still seems far less than your earlier finding:

then one or more runs following a stationary period (recalibration was prevented): we observed drift up to 20 deg/min

Was this using LEDs, or during certain runs?

I'll try building up some heat by stalling some motors.

[scatwang]

@scatwang

I can still trigger a small drifiting (<0.1 deg/s) with a recent nightly build firmware. I will try to find the minimal reproduce steps.

This is indeed reproducible by putting it against something like a PC keyboard and then gently rotating it. This is because we use a stationary test where the angular velocity should stay under a threshold for a second in order to re-calibrate.

Due to some other recent developments where we will actually save the bias, I think we can choose to reduce the default threshold from 3 deg/s to 1 deg/s. This way it should be less common to accidentally move it slightly while calibrating.

Please experiment with the following script if you'd like to see the effect.

from pybricks.hubs import PrimeHub
from pybricks.parameters import Color, Axis
from pybricks.tools import wait

hub = PrimeHub()

#
# Default is 3. Reduce to 1 to make it more strict about calibration.
#
hub.imu.settings(angular_velocity_threshold=1)

while True:

    hub.light.on(Color.GREEN if hub.imu.stationary() else Color.RED)
    print(hub.imu.heading())
    wait(100)

I know drifting can be triggered by simulating a slow and steady movement. But the issue I mentioned was triggered by running a program. The drivebase was intended to run fast and crash to the mission models. Anyway I will add if hub.imu.stationary() to the background to see if calibration was triggered.

[DrTom]

@laurensvalk

Thanks for checking! Can you share the script used to produce your results?

It is attached in my comment above. Somehow I had to add the .txt extension to upload it.

While not ideal, this still seems far less than your earlier finding: Was this using LEDs, or during certain runs?

The earlier findings were related to an actual run. In this case the robot was sitting still, completely stationary, no movement. For that it seems already significant. This was just to see if the LEDs do play a role.

[DrTom]

@laurensvalk

I'll soon add an API option ...

It seems that it would possible to read the temperature from the IMU:

https://github.com/STMicroelectronics/lsm6ds3tr-c-pid/blob/153864d2b44f50638dcb362198729919cb79a4bd/lsm6ds3tr-c_reg.h#L1907-L1908

Would it possible to expose that, too?

Also if we could access the drift setting for reading and writing together with the temperature it seems possible to first run some tests reading various values and then set them during a run in a separate tasks. This is probably not a solution for the long run but it could help us getting there.

[laurensvalk]

Would it possible to expose [temperature], too?

We currently do a round-read of the acceleration and gyro registers. We can have a look whether the temperature can be read as part of that.

This week I will try to push some of the existing changes so you can individually set and read:

• angular velocity stationary threshold
• acceleration stationary threshold
• angular velocity bias (plus the ability to read value without correction)
• angular velocity scale (to make 360 equate to an actual rotation)
• acceleration correction (plus the ability to read value without correction)

Thanks again for experimenting. It is much appreciated.

[laurensvalk]

Here is a short program that prints the heading every second and the drift since the last second. You can rotate the motor to turn the lights on and off. This has virtually no drift at all for me, whether the light is on or off.

from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Color, Port
from pybricks.tools import wait

hub = PrimeHub()
dial = Motor(Port.B)

DELAY = 1000

last = hub.imu.heading()

while True:

    wait(DELAY)

    # Get new heading and drift since last heading.
    new = hub.imu.heading()
    drift = (new - last)  / DELAY * 1000
    last = new

    print(f"H:{new:7.3f}     D:{drift:7.5f}")

    # Light on or off depending on motor angle.
    hub.display.on(100 if dial.angle() > 0 else 0)

    # Green = we are calibrating, red is not.
    hub.light.on(Color.GREEN if hub.imu.stationary() else Color.RED)

@DrTom - I see you've turned off calibration in your tests. Logging calibrated, time-integrating values while explicitly turning off calibration makes the data a bit difficult to interpret. A drift may seem large, but it could be the result of the not-fully calibrated data, which adds up to big numbers over the course of a few minutes.

I suppose it would be more helpful to do experiments with uncalibrated data.

When this build finishes building, you will be able to do:

data = hub.imu.angular_velocity(calibrated=False)

You will find that it is nonzero for stationary. This is what we try to average/estimate during calibration. And then we subtract it during normal use. And we integrate that result over time to produce a heading.

So the interesting part would be how fast the above measurement changes over time due to temperature and other factors. All the others (bias, heading) are derived from this.

[DrTom]

@laurensvalk

I see you've turned off calibration in your tests.

After initial calibration: yes. My impression was that this is the point. We let the system do calibration. The drift should be gone in this state (since it is calibrated). The we impose some condition onto the system - like turning on the LEDs. If those produce enough energy such that drift appears this will also be an issue during a run e.g. Since there can not be recalibration during a run we must otherwise compensate for that. So far my naive ideas ;-)

At any rate: I just flashed your new build. If repeat my tests and output your data as above it will be useful to you?