Adds Mahony filter based IMU orientation estimation.

[CasperClarke]

This branch adds a Mahony filter that processes the gyro and accelerometer data to create a quaternion estimation of the hub's rotation.

This branch adds a few python bindings in order to read the heading of the vehicle, and its rotation quaternion, calibrate the gyro and modify the Mahony filter gains.

This branch also modifies the existing function that made use of the accelerometer to determine the pitch and roll of the hub.
Now the IMU estimation is used for the tilt function.

Over the span of 30 minutes, a drift of 0.5 degrees was observed with a good calibration of the gyro bias on a technichub.

The axis of roll and pitch and heading need to be validated, I expect they may be inverted from the intended direction (CCW Positive).

Another slight problem: If the start gyro calibration function is started and the program is then ended, it will continue running until the stop gyro calibration function is called. This could result in unintended behavior if a program has a start calibration and does not have a matching stop gyro calibration.

This is a side effect of the code being run beneath the python interpreter, the code running the gyro estimation does not stop between runs of python programs. This also has the beneficial side effect of gyro calibration being stored until a power cycle occurs or it is overwritten.

[laurensvalk]

Thanks for this contribution! It may take us a while to get back with feedback, but we will try to do so in the new year.

[dlech]

We've been looking at the IMU again recently, so I squashed, rebased and cleaned this up a bit in https://github.com/pybricks/pybricks-micropython/tree/pr135-rebase to try it out.

The program I used is:

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

hub = PrimeHub()

hub.imu.start_gyro_calibration()
wait(1000)
bias = hub.imu.stop_gyro_calibration()
hub.imu.set_gyro_bias(*bias)
hub.imu.reset_heading()

while True:
    hub.display.number(hub.imu.heading())
    print(hub.imu.heading(), *(hub.imu.angular_velocity() - bias))
    wait(500)

So hopefully I'm using it as intended.

This doesn't seem to get great results for me. If I have the hub laying flat on my desk and I rotate it 360 degrees, there is usually a 1 to 2 degree error in the heading accumulated with each rotation. Also, if the hub is left alone, the heading tends to drift one degree every few minutes. The direction and rate at which it drifts depends on the heading of the hub so maybe this is caused by random angle walk rather than gyro bias drift?

So it seems like there is more work to do here to improve this or we could consider a different approach entirely.

Also, FYI, pybricks/support#933 discusses some calibration ideas if you haven't seen that already.

[dlech]

Thanks again for the work on this. In the meantime, we've made some progress on implementing orientation estimation in a different way.