[Feature] User IMU calibration ideas

[laurensvalk]

Is your feature request related to a problem? Please describe.
When the hub is stationary and flat, the acceleration values are not perfectly aligned along the Z axis. This is possibly because the chip or its MEMS device is not perfectly aligned with the outer hub surfaces.

This can throw off the static tilt values by a degree or two.

If we used the accelerometer in our 3D orientation filter, this also propagates into that. Even if we didn't apply continuous fusion, we'd still need the accelerometer to set the initial orientation so the user doesn't have to worry about it.

Describe the solution you'd like
Create a one-off calibration routine that saves the offset/mapping in user storage.

It would guide the user through the steps to put the hub flat and then later on one of the sides.

This might fall in the same kind of category as motor sensor calibration (if we ever have that), so it could be worth thinking about where this belongs. Perhaps it could be a special program loaded from Pybricks Code.

The instructions could all happen through the terminal, so we don't have to invent a special UI for it. This way users can also do it in Pybricksdev, etc.

Describe alternatives you've considered
A static error of two degrees isn't terrible for many applications, so it's probably not critical if users skip the calibration routine. It's more intended for FLL teams who want to get the best performance out of their robot.

[laurensvalk]

This may also improve the X, Y gyro data being closer to zero when spinning along the z axis, which helps with 3D integration.

[laurensvalk]

It appears that the gyro scale is also a bit different for each axis. In the calibration routine, we could ask users to rotate the hub in increments of 90 or 360 on each axis to get the right values.

[Debenben]

I have good experience calibrating acceleration of my technic hubs with an offset. In my opinion, something like this is worth considering if a calibration routine is added. I took inspiration from #933 (comment) to create a proof of concept in python

from pybricks.hubs import ThisHub
from pybricks.parameters import Color
from pybricks.tools import wait, StopWatch
from pybricks.geometry import Matrix, vector, Axis

hub = ThisHub()

acceleration_last = hub.imu.acceleration()

# Different stationary positions used for calibration
# The calibration calculates an offset such that their magnitude is as equal 
# as possible
stationary_positions = []

def is_new_stationary():

    global acceleration_last, stationary_positions

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

    # Anything different from 9810 means we're not stationary,
    # but we add margin to account for sensor variability
    if abs(abs(acceleration_now) - 9810) > 1000:
        return False

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

    # Make sure the new stationary position is different from existing ones
    for v in stationary_positions:
        if abs(acceleration_now - v) < 5000:
            return False
    
    return True

stationary_timer = StopWatch()

stationary_counter = 0
stationary_average = vector(0, 0, 0)

# We need at least 3 different positions, lets take 6
while len(stationary_positions) < 6:

    # Update state.
    new_stationary = is_new_stationary()

    # Reset timer on movement.
    if not new_stationary:
        stationary_timer.reset()
        stationary_counter = 0
        stationary_average = vector(0, 0, 0)

    # Green indicates new stationary position, red means not stable
    # or position is already used for calibration
    if stationary_timer.time() > 500:
        hub.light.on(Color.GREEN)
        # to get more stable values we average over 100 measurements
        if stationary_counter < 100 :
            stationary_counter += 1
            stationary_average += acceleration_last*0.01
        else:
            stationary_positions.append(stationary_average)
            stationary_counter = 0
            stationary_average = vector(0, 0, 0)
        
    else:
        hub.light.on(Color.RED)

    wait(10)

# We have gathered enough data
# We want to find the vector x that approximately solves Ax = b
A = []
b = []

# We take two different stationary vectors v1 and v2 and demand that
# (v1 - x)**2 == (v2 - x)**2
# <=>
# 2*(v1 - v2)*x == v1**2 - v2**2
for v1 in stationary_positions:
    for v2 in stationary_positions:
        if abs(v1-v2) < 0.1:
            next
        row = 2*(v1 - v2).T
        A.append([row*Axis.X, row*Axis.Y, row*Axis.Z])
        b.append(v1.T*v1 - v2.T*v2)

# The least squares fit can be calculated with numpy.
# Copy the output of this program and execute on your PC
print("import numpy as np")
print("A = np.array(",A,")")
print("b = np.array(",b,")")
print("x = np.linalg.lstsq(A,b)[0]")
print("print(x)")


# The vector x is the offset that needs to be subtracted

#x = vector(166.89429952, -67.42990329, 236.65739606)
#def get_calibrated_acceleration():
#   return hub.imu.acceleration() - x

[laurensvalk]

Thanks @Debenben, this is interesting. So far I have been working by the assumption that the errors might be due to the chip placement on the PCB (or mounting of PCB in the plastic) being off by a few degrees. If this is the case, a static rotation map could take care of this.

Yours seems to take a more direct approach to subtracting the same offset always. Do you get improved results if you put the hub on all six sides?

As a means of verification, I have used something like this:

# Verification. Put the hub on any flat side:
while True:

    up = hub.imu.up()
    if up == Side.TOP:
        ideal = Axis.Z
    if up == Side.BOTTOM:
        ideal = -Axis.Z
    if up == Side.LEFT:
        ideal = Axis.Y
    if up == Side.RIGHT:
        ideal = -Axis.Y
    if up == Side.FRONT:
        ideal = Axis.X
    if up == Side.BACK:
        ideal = -Axis.X
    
    # Original measurement
    g = hub.imu.acceleration()
    g = g / abs(g)
    error_before = abs(g - ideal)

    # After correction
    c =         # <<---------- Your corrected acceleration vector goes here
    error_after = abs(c - ideal)

    print("Error. Before: {:.4f} After: {:.4f}".format(error_before, error_after))

    wait(100)

There's a line where you could add the corrected version of your method (hub.imu.acceleration() - x).

I'm not fully happy with my static map approach, so I'll share that part of it later (i.e. the part of filling the blank line in my variant).

---

And thanks for generously sponsoring Pybricks once more --- we really appreciate it!

[Debenben]

@laurensvalk I finally came around to testing verification you suggested. In case of my hubs (1xInventorHub, 6xTechnicHub) the mounting of the PCBs is quite accurate. The errors I see with your suggested verification method are not larger than the deformations of the plastic casings of my hubs which make it hard to define what the proper six side orientations are. I tested with and without my offset calibration and adding the offset does not significantly change this.

This is not unexpected: When calibrating an hub where the PCB is rotated with my offset calibration method you get the same offsets rotated accordingly and it should not correct the rotation itself like a static rotation map would.

The easiest way to see the effect of the offset calibration is checking the magnitude of the acceleration. With the offset calibration you can use the magnitude to determine if the hub is accelerating or not, i.e. you get approximately the same magnitude regardless of the rotation. This is an impression what the magnitude looks like when placing the hub in different orientations and moving it between those positions:

The other way to see improvements with the offset calibration is when measuring angles. I checked this by putting the hub a known distance (two meters) in front of a wall with a measuring ruler attached to the wall. I attached a cross-line laserpointer to the hub pointing to the wall. The inclination angle from the position of the laserdot can then be compare to the accelerometer measurement.

Unfortunately I have not found the time yet to repeat the measurements which I did some time ago and do a clean documentation, I also don't think it is a must-have feature for pybricks. I just wanted to share the method and in case people are looking for an easy calibration routine they know how they could do it themselves. The more accurate way to calibrate would be to calibrate both offset and gain or even additionally the absolute magnitude e.g. like this: https://www.nxp.com/docs/en/application-note/AN4399.pdf but I think for this you need to place the hubs in defined angles, therefore it requires more effort.

[laurensvalk]

Thanks for that graph! That does indeed bring it nicely close to 1G.

[laurensvalk]

Now that #1622 is implemented, we can potentially make some progress on this.

As a simple case to get started, we could store a 1D correction factor for gyro heading, along with a command or program to help you find the right value.

And then get to the more interesting 3D or 6D correction in a future release as well.

[laurensvalk]

@Debenben Thanks again, this is great. I was now able to replicate your results. What follows is a slightly simpler variant that achieves about the same, but does not require an external computer with numpy.

In addition to offset, I have also added scale. The user has to place the hub on all 6 sizes, which would be reasonably easy to instruct to the user.

from pybricks.hubs import ThisHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop, Axis
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, vector

COUNT = 500

def wait_for_side(side):
    while not hub.imu.up() == side or not hub.imu.stationary():
        wait(10)

def get_avg_accel(axis):
    total = 0
    wait(1000)
    for c in range(COUNT):
        if not hub.imu.stationary():
            raise ValueError("Moved it!")
        total += hub.imu.acceleration(axis)
        wait(10)
    return total / COUNT


hub = ThisHub()

print("Align +Z, Display up: ", end="")
wait_for_side(Side.TOP)
z_up = get_avg_accel(Axis.Z)
print(z_up)

print("Align -Z, Display down: ", end="")
wait_for_side(Side.BOTTOM)
z_down = get_avg_accel(Axis.Z)
print(z_down)

print("Align +X, USB up: ", end="")
wait_for_side(Side.FRONT)
x_up = get_avg_accel(Axis.X)
print(x_up)

print("Align -X, USB down: ", end="")
wait_for_side(Side.BACK)
x_down = get_avg_accel(Axis.X)
print(x_down)

print("Align +Y, ACE up: ", end="")
wait_for_side(Side.LEFT)
y_up = get_avg_accel(Axis.Y)
print(y_up)

print("Align -Y, BDF up: ", end="")
wait_for_side(Side.RIGHT)
y_down = get_avg_accel(Axis.Y)
print(y_down)

# my technic hub
# x_up = 9810.152
# x_down = -9809.669
# y_up = 9984.96
# y_down = -9738.044
# z_up = 10227.69
# z_down = -9500.274

# my spike prime hub
# x_up = 9890.628
# x_down = -9973.075
# y_up = 9845.006
# y_down = -9939.991
# z_up = 9701.395
# z_down = -10080.28

scale_x = (x_up - x_down) / 2
scale_y = (y_up - y_down) / 2
scale_z = (z_up - z_down) / 2

print(scale_x)
print(scale_y)
print(scale_z)

off_x = (x_up + x_down) / 2
off_y = (y_up + y_down) / 2
off_z = (z_up + z_down) / 2
print(off_x)
print(off_y)
print(off_z)

while True:
    a = hub.imu.acceleration()
    x, y, z = a

    v = vector((x - off_x)/scale_x, (y - off_y)/scale_y, (z - off_z)/scale_z)

    # Compare new (left) to unadjusted (right)
    print(f"{abs(v) * 9810:10.3f}", f"{abs(a):10.3f}")

    wait(250)

This results in 6 floating point values that we can store persistently on the hub, so you'd have to do it only once (until you upgrade the firmware).

I have been working on a 3D attitude estimation algorithm, and having accurate acceleration data will help with this.

[BertLindeman]

For the Technic hub add some side support as the hub will bobble on the holes on its sides.

[laurensvalk]

Yes. When we add interactive instructions for this with a visual, we can include suggestions for a basic frame on all sides. Because even the flat sides aren't quite flat.

For example:

[Debenben]

@laurensvalk I have not tested it, but your variant looks good and seems like a good compromise of ease-of-use and accuracy. Thank you for your awesome work!

[laurensvalk]

Here (scroll down) is an experimental firmware that lets you call:

hub.imu.settings(acceleration_correction=(x_up, x_down, y_up, y_down, z_up, z_down))

Where x_up is the average positive X gravity acceleration for your hub, and so on. Below is a script to find it and verify the results.

Big thanks to @Debenben for the inspiration for this method and the generous contributions via GitHub sponsors!

In my hub, the biggest improvement comes when the hub is flat, upside down. This shows the original magnitude error away from 1 on the left and the result after calibration on the right.

    -0.029      0.001
    -0.027      0.002
    -0.028      0.001
    -0.029      0.000
    -0.030     -0.001
    -0.031     -0.002
    -0.030     -0.000
    -0.029      0.000
    -0.030     -0.000
    -0.026      0.003

---

from pybricks.hubs import ThisHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop, Axis
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, vector

COUNT = 300

def wait_for_side(side):
    while not hub.imu.up(calibrated=False) == side or not hub.imu.stationary():
        wait(10)

def get_avg_accel(axis):
    total = 0
    wait(500)
    for c in range(COUNT):
        if not hub.imu.stationary():
            raise ValueError("Moved it!")
        total += hub.imu.acceleration(axis, calibrated=False)
        wait(10)
    return total / COUNT

hub = ThisHub()

print("Align +Z, Display up: ", end="")
wait_for_side(Side.TOP)
z_up = get_avg_accel(Axis.Z)
print(z_up)

print("Align -Z, Display down: ", end="")
wait_for_side(Side.BOTTOM)
z_down = get_avg_accel(Axis.Z)
print(z_down)

print("Align +X, USB up: ", end="")
wait_for_side(Side.FRONT)
x_up = get_avg_accel(Axis.X)
print(x_up)

print("Align -X, USB down: ", end="")
wait_for_side(Side.BACK)
x_down = get_avg_accel(Axis.X)
print(x_down)

print("Align +Y, ACE up: ", end="")
wait_for_side(Side.LEFT)
y_up = get_avg_accel(Axis.Y)
print(y_up)

print("Align -Y, BDF up: ", end="")
wait_for_side(Side.RIGHT)
y_down = get_avg_accel(Axis.Y)
print(y_down)

hub.imu.settings(acceleration_correction=(x_up, x_down, y_up, y_down, z_up, z_down))
print(hub.imu.settings())

GRAVITY = 9806.65

while True:
    old = hub.imu.acceleration(calibrated=False) / 9806.65
    new = hub.imu.acceleration() / 9806.65  # Method will default to using calibration values if available

    print(f"{1-abs(old):10.3f}", f"{1-abs(new):10.3f}")

    wait(250)

[BertLindeman]

Great program Laurens.

You defined GRAVITY but do not use it 😉

I take it calibration is not intended for the movehub as

hub.imu.up(calibrated=False)

and

hub.imu.stationary()

are refused for the movehub.

The cityhub has no IMU so that is clear.

Do you want me to update this program with UP and DOWN texts related to the hub that is connected?
I do not have an EssentialHub, and did not see enough pictures to know what side is which.

[laurensvalk]

Thanks for giving this a try Bert!

This script is just a quick test to try out the new calibration feature. Don't worry too much about the text in this end-user script.

The plan is to eventually make a decent front-end interface for it, which will have short videos or animations to show how you have to rotate the hub.

But first we need to actually make it all work, and this one step towards that objective.

I'm currently working on the next step, which is 3D gyro integration with fusion from the now-improved accelerometer data.

[vanczakp]

Hello @laurensvalk ,

Is there good and incorrect orientation for your sequence? If I understand it correctly your program calculates the 3D misplacement of sensor compared to the technic hub and gives you the proper transformation. Therefore it matters which orientation is used for measuring first then only one is correct for each direction. Here is what I used and I guess some of it is incorrect. Test was done on glass table, the rug is for better representation)

Could you tell me which is incorrect and what would be the correct way?

[laurensvalk]

@vanczakp, you shouldn't have to do anything for the moment to use the new firmware. It is only to improve accuracy slightly for certain yaw turns, which is important in FLL.

[laurensvalk]

@Debenben and @BertLindeman : In case you want to try an early version of the new procedure, you can do so in #1907

[vanczakp]

@vanczakp, you shouldn't have to do anything for the moment to use the new firmware. It is only to improve accuracy slightly for certain yaw turns, which is important in FLL.

@laurensvalk I did not use the new firmware, I used your code to get the offset values because the one what Debenben suggested does not work (requires some numpy or whatever). I used your code partly (until offstet is printed). Anyway my question still applies: Are the 6 orientation I used correct or are some of them incorrect? Thanks in advance!

[laurensvalk]

I think we may be talking about different things here. These settings are only relevant for the upcoming firmware changes. This is still an experimental feature and subject to change, so I would say don't worry about these details for now.

[vanczakp]

I think we may be talking about different things here. These settings are only relevant for the upcoming firmware changes. This is still an experimental feature and subject to change, so I would say don't worry about these details for now.

sorry, I asked a stupid question. Anyway I did 20-30 run with your code to measure the offset. Requires 5-6 minutes to get the hub heated up and have stable results. Mostly the Z needs time to get stable

[Debenben]

@laurensvalk I was able to do a test with the new firmware and your calibration script. I only tested one of my technic hubs and only a couple of tilt values and everything seems to be working perfectly:

• Tilt from uncalibrated acceleration is off by more than a degree.
• I recon my quick measurements with a folding rule have an average error of about 2 cm which is 0.33 degree at 3.5 m distance.
• Standard deviation of pitch when averaging over 100 acceleration values is up to 0.028 degrees (in calm environment without vibration damping).
• In the same situation the pitch from the inbuilt tilt function (using fusion with gyro by default) is more stable with a standard deviation of 0.0063 degrees. It deviates by less than 0.005 degrees from acceleration-only value.

measurement.zip

[laurensvalk]

Thank you @Debenben , this looks great! I'm sure there are better calibration algorithms available, but this is a nice step forwards.

The instructions for trying out the latest version are given in #1907. I'm going to close this issue to avoid confusion.