[Feature] Gyro bias calibration ideas

[laurensvalk]

Is your feature request related to a problem? Please describe. In many LEGO applications, the robot is stationary more often than not, including in between robot runs.

This knowledge could help us continuously calibrate the gyro when the hub is not moving.

There are all sorts of heuristics we could use to define "not moving", including by using information from the gyro itself.

Below is a simple example that uses just the accelerometer data, which is nice because it does not require calibration of its own. It works quite well, see video. We'd just have to pick the thresholds to work well across the range of hubs.

Demo of detecting if we're stationary. In the green state, we could continuously calibrate the gyro:

https://user-images.githubusercontent.com/12326241/216274171-20e6ce17-efa2-43cf-8147-5381248844c3.mp4

The calibration could run continuously. It would work more aggressively at first (i.e. the first stationary zones soon after boot), and then more gradually over time. It could also work more aggressively the longer it is stationary.

To further improve it, we can also buffer a few samples and discard those from the calibration with hindsight, to throw away the portion that could have happened just before the hub detected movement.

If we find that this works well enough, the user won't have to do anything and we don't need an API for it. They also don't need to do anything special on boot. They could just let it sit still anywhere to further stabilize it.

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

hub = PrimeHub()

acceleration_last = hub.imu.acceleration()

def is_stationary():

    global acceleration_last

    # Get the acceleration and change thereof.
    acceleration_now = hub.imu.acceleration()
    acceleration_change = acceleration_now - acceleration_last
    acceleration_last = acceleration_now

    # Anything larger than 9810 means we're not stationary,
    # but we add margin to account for sensor variability
    if abs(acceleration_now) > 11000:
        return False

    # Any change in the grvity vector means movement.
    if abs(acceleration_change) > 100:  # NOTE: this assumes a delta of 10 ms in the loop below.
        return False

    return True

stationary_timer = StopWatch()

while True:

    # Update state.
    stationary = is_stationary()

    # Reset timer on movement.
    if not stationary:
        stationary_timer.reset()

    # Green indicates stationary, red means not stable.
    if stationary_timer.time() > 500:
        hub.light.on(Color.GREEN)
    else:
        hub.light.on(Color.RED)

    wait(10)

[laurensvalk]

the user won't have to do anything and we don't need an API for it.

Instead, we could have something like hub.imu.is_ready() -> bool as a means to check that enough calibration has already happened for a reliable run. Anyone who still wants to do explicit calibration at the start of their code can just wait for it to become True if it isn't already.

[laurensvalk]

This can be done separately from the (future) 3D integration, and can already be done even just to improve the output of hub.imu.angular_velocity.

[dlech]

What are the semantics of is_ready()? does it ever change back to False once it is True?

Would it be useful to make available in Python the time since the last successful calibration (e.g. I only want to calibrate now if there hasn't been a successful one for more than X minutes)?

[CasperClarke]

I have just now come back to check on how things were going and I found this through the comment that @dlech posted on my pull request.

For the purpose of user-independent gyro calibration, there are a lot of good methods.

Using the accelerometer to perform stationary detection is a beautiful idea and should perform reasonably well.

One other thing that can be done is using the accelerometer directly to calibrate the drift for the gyro. By detecting when the accelerometer values are stable the drift can be measured along the non-vertical directions. Some coordinate transforms would need to be done to do this, however when it's implemented all the user would need to do to get a good calibration is set the hub on its side for a few seconds to determine the drift rate along the "yaw" axis. This code would presumably be applied to all directions at the same time to make things easier.

I like the idea of having a confidence value for the gyro drift for an is_ready() flag but I am not sure of a good way of achieving that goal.

@dlech with your comment on my pull request, I have a few things to note: My initial tests used multiple minutes of calibration before I was able to get a low drift rate and my tests were done without moving the hub at any point, so active drift calibration is definitely a better option. When I was doing further testing I found that calibrating at the beginning of the code sadly did not yield a good estimation in any dynamic situations, so after moving the hub drift increased severely.

I should be able to create some better filtering and calibration code soon, I have started my classes at college again so I have been busy for a few weeks.

[laurensvalk]

Thanks for both of your comments!

What are the semantics of is_ready()? does it ever change back to False once it is True?

I think we could indicate that it is ready after a certain (accumulated) time of calibration. Or... we could return for how long it has been stationary as a measure of accuracy. Anyone who needs more accuracy could choose to wait longer.

We could let the value gradually expire, so that it goes down (or equivalently, back to False) if it hasn't been calibrated for a "long" time.

Likewise, it could go back to False or 0 for any conflicting / bad conditions, such as:

• Stationary acceleration but non-zero gyro rates given the current offsets
• I2C errors

[laurensvalk]

I made an experimental implementation in C, and found that stationary detection can be done in a much simpler way than the script above, without maintaining any data buffers or timers/processes. We can even include the gyro data, by requiring that it is constant (unknown bias drops out) while it is stationary. This makes it more reliable, because this includes rotation in the plane which we miss when looking at acceleration data only.

I started out experimenting in pbio, but I ended up with something so simple that maybe pbdrv isn't a bad place to do it. Then we could potentially save some resources by operating directly on the raw values as integers.

I'll try sharing an experimental implementation later this week.

EDIT: We can calibrate the accelerometer too. Probably not with a simple offset, but rather a mapping that accounts for the placement in the hub. Though this could only work if the user puts the hub in a fixed known orientation.

[laurensvalk]

I've updated the implementation and pushed it in https://github.com/pybricks/pybricks-micropython/tree/gyro-calibration. I've also added 1D heading integration as a test; I'll do 3D later.

• It has pretty much no drift at all and it can capture rotations and return to 0 accurately, so this is looking quite positive.
• No user calibration action is required.
• I left the sampling rate at 833, but it is closer to 820 in practice. I'd have to go and look at the clock setup to confirm.

You can try it as follows:

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

hub = PrimeHub()

while True:
    # Test stationary...
    if hub.imu.stationary():
        hub.light.on(Color.GREEN)
    else:
        hub.light.on(Color.RED)

    # This is a measure for number of seconds stationary.
    # There's no API for this yet, just this debug function.
    stationary_counter = hub.imu.debug()

    # Get heading
    heading = hub.imu.heading()

    # Display heading
    hub.display.number(round(heading))
    print("{:.1f}".format(heading))
    wait(10)

[dlech]

Nice work. I couldn't find anything in the datasheet about the clock, but since it doesn't have an external oscillator, I don't expect it to be very accurate.

[dlech]

It seems like this loses about 1 degree for every 5 rotations (turning the hub flat on my desk in a circle similar to how a drive base would drive in a circle).

[laurensvalk]

That's the clock/scaling issue above. This applies to gyros in general. As long as it goes back to 0 symmetrically, we should be good. The rest is just scaling.

[laurensvalk]

Mainly just the last commit with the 1D integration is an experiment/hack for now.

The commits stuff before it, we could start cleaning up and prepare for a pull request.

A few notes:

• https://github.com/pybricks/pybricks-micropython/commit/8ad5b77add6903de07fbfb39ae48f188e004673f and https://github.com/pybricks/pybricks-micropython/commit/bc3452ab874ba1d6855442292461296b56b1b494 are from last year. I ended up not using it. We could drop them, although the cleanups for Side aren't bad to have.
• I took some shortcuts:
  • Need to reconsider the design of pbdrv/pbio, who calls what and who sets callbacks where. As long as synchronous execution works about the same, I'm fine with any pattern
  • There is only one pbio gyro, and it's not really an "object" at this point. This could use some design and cleanups.
  • There are some 'REVISIT' notes here and there to get some values from appropriate config files.

[dlech]

Reversing does in deed gain back the lost degrees.

Idea: we could capture a timestamp in the INT1 gpio interrupt to get a reasonably accurate timestamp for each sample.

[laurensvalk]

1 degree for every 5 rotations

That seems to be about similar to setting 1639 to 1638.

To be sure, maybe we could count how many samples we get per e.g. 100 seconds.

And check if they are just more spread out or whether we occasionally miss one.

    // REVISIT: This should be 2 x 833, but it is slightly off. Need to
    // review actual sample rate.
    heading += (yaw_rate_last + gyro_rate[2]) / (1639);

Although, that is actually part of the last commit which isn't ready to be merged yet anyway.

[laurensvalk]

After seeing this comment it occurred to me to leave a related note here.

As you can see in the video, we could detect "taps" quite well, possibly even directionally.

We could do this based on the existing data without relying on the built in sensor filters, to reduce the complexity and overhead of the data transfer that we already have.

[michaelwareman]

Hello Laurens:

I tried your above code, but I get this error message: Traceback (most recent call last): File "imu_test2.py", line 9, in AttributeError: 'IMU' object has no attribute 'stationary'

The code in the first comment works for me.

You mentioned it being possible to tell the direction of the tap. Is there a way to get the pitch, roll and yaw values? Mike,

[dlech]

You need to install this firmware to use the experimental code: https://github.com/pybricks/pybricks-micropython/suites/10826302396/artifacts/545075700

[dlech]

pitch and roll are available via the tilt function. Yaw hasn't been implemented yet, see #912.

[CasperClarke]

Can we save values to the EEPROM? If so I can implement a standard Kalman filter and create an accelerometer calibration user function in python. The Kalman filter should automatically handle the gyro bias to a certain extent. Additionally, a Kalman filter would give us a much better orientation estimate than most other methods. If we can save the accelerometer calibration to the EEPROM the user would only have to calibrate the accelerometer once by putting the hub on each side.

[laurensvalk]

See https://github.com/pybricks/support/issues/943 for putting the hub on a few side and storing those values :smile:

[laurensvalk]

Offset calibration is done. I'll open a new issue dedicated to magnitude calibration.