fun w/ kalman filters & motion sensors (& musings on the future of hardware objects)

[sneakers-the-rat]

Hey everyone, will have a more complete writeup on this in the docs soon, but having a bit of fun over in the parallax branch: https://github.com/wehr-lab/autopilot/tree/parallax

Really liking the combination of the transformation module with hardware objects: needed to estimate velocity in an absolute (geocentric) coordinate frame from a 9-axis IMU (xyz accelerometer, gyroscope, and magnetometer), this one over here: https://docs.auto-pi-lot.com/en/parallax/autopilot.hardware.i2c.html#autopilot.hardware.i2c.I2C_9DOF

So the accelerometer includes a vector for the force of gravity, which should let us estimate pitch and roll, which we can use to rotate the accelerometer readings to estimate absolute z-axis acceleration, a bit of simple, if naive trigonometry:

https://github.com/wehr-lab/autopilot/blob/bf47351264461224a6ba97c85ddea1d96692ce69/autopilot/transform/geometry.py#L145-L147

where accel[0:2] == accelerometer x,y,z readings.

Problems: accelerometer is noisy, and estimates will be bad when accelerometer not at rest (which is exactly what we care about). Luckily we have the ~ literal magic of kalman filters ~ that can rescue us. So after borrowing some code from the lovely filterpy library (https://github.com/rlabbe/filterpy), we now have a kalman filter transformation object (which still needs more documentation): https://docs.auto-pi-lot.com/en/parallax/autopilot.transform.timeseries.html#autopilot.transform.timeseries.Kalman

I've always heard about Kalman filters but have never taken the time before last week to sit down and learn about them, and i feel ashamed for not having done it sooner. I won't try and butcher the explanation as a newcomer here, but I found Roger Labbe's interactive book to be a really beautiful way to learn about them: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python . I adapted a sensor fusion strategy from this paper https://sci-hub.do/https://link.springer.com/chapter/10.1007%2F978-3-319-06773-5_41 where the roll and pitch are estimated from the noisy accelerometer readings and also incorporated with angular velocity measurements from the gyroscope.

So I implemented this whole thing as a transformation in the transform.geometry module: https://docs.auto-pi-lot.com/en/parallax/autopilot.transform.geometry.html#autopilot.transform.geometry.IMU_Orientation . I tried to make it general, you can use it just with accelerometer readings without the kalman filter (just the trig transformation),

from autopilot.transform.geometry import IMU_Orientation
orientation = IMU_Orientation(use_kalman=False)
roll, pitch = orientation.process(accel_reading)

with accelerometer readings with a kalman filter (just smoothing)

orientation = IMU_Orientation()
roll, pitch = orientation.process(accel_reading)

or with combined accelerometer/gyroscope readings

roll, pitch = orientation.process((accel_reading, gyro_reading))

For some reason I couldn't find any other libraries that implemented this transformation/sensor fusion, so now it's available for everyone, even if they don't want to use the rest of autopilot they just have to from autopilot.transform.geometry import IMU_Orientation and use it wherever!!

What's even cooler, in my opinion, is how easy it is to integrate these transforms into hardware objects for the best of both (implementational) worlds! Using the kalman_mode parameter, you can access the above three modes in the rotation property:

https://github.com/wehr-lab/autopilot/blob/bf47351264461224a6ba97c85ddea1d96692ce69/autopilot/hardware/i2c.py#L459-L491

So you just poll the object like this

from autopilot.hardware.i2c import I2C_9DOF
sensor = I2C_9DOF()
sensor.rotation

And then, with a few additional transforms I wrote, it's possible to extract z-axis velocity trivially (gotta love the recursive __add__ method ;)

from autopilot import transform as t

z_velocity   = t.geometry.Rotate('xy') # rotate accelerometer readings to geocentric coordinates
z_velocity += t.selection.Slice(slice(2,3)) # select the z component (array[2] == array[slice(2,3)])
z_velocity += t.math.Add(-9.8) # subtract gravity
z_velocity += t.timeseries.Integrate(decay=0.95, dt_scale = True) # integrate to velocity, scaling by time between measurements and a slight decay

rotation = sensor.rotation
z_vel = z_velocity.process((sensor._acceleration, rotation))

(the little bit of awkwardness in first needing to return rotation and then using sensor._acceleration is just to prevent a double read from the sensor with the acceleration property, want to figure out some rate-limiting so that's unnecessary).

I also noticed that the gyroscope was biased, and had nonzero reads at rest, but also this chip has a lot of slickness built into it! Turns out the sensor chip has a builtin filterbank for its sensors, so was able to expose that in the gyro_filter property

https://github.com/wehr-lab/autopilot/blob/bf47351264461224a6ba97c85ddea1d96692ce69/autopilot/hardware/i2c.py#L364-L387

Still need to implement full calibration routine, but it works for now! This is a general design practice I think i'm going to start carrying through to the rest of the objects in the hardware module -- expose measurements and controls for hardware as properties. Also want to make the control of i2c objects more uniform (eg. uniform way to list registries/etc.) but since in my experience each i2c device is controlled a little bit differently, it'll still likely be the case that we'll need to expose controls/readings explicitly in properties, but that's pretty easy and only took me an hour or so with the sensor datasheet!

Once that's in place, we can subclass the property decorator to make it possible to implement my dream API for connecting hardware objects together:

sensor.connect(sensor.rotation + z_velocity, "pilot_2.LEDS.LED_ID.brightness")

which would then stream the rotation measurement through the z_velocity transform from before to control the brightness of some LED (with id LED_ID ) on a different pilot. Still a bit of work to get there, but the dream is real and possible!

In the meantime, the code to use it to control the velocity of an array of stepper motors (another very cool little object: https://github.com/wehr-lab/autopilot/blob/bf47351264461224a6ba97c85ddea1d96692ce69/autopilot/hardware/esoteric.py#L18 but maybe a story for another time ;) is a little clunky --- being able to prototype easily using only parts of the library is something i've been working on! --- but still quite simple:

Code running on Pi with IMU:

import time
from autopilot.hardware.i2c import I2C_9DOF
from autopilot.transform.geometry import Rotate
from autopilot.transform.timeseries import Integrate
from autopilot.transform.selection import Slice
from autopilot.transform.math import Add
​
from autopilot.core.networking import Net_Node
​
# instantiate the sensor, inverting the x-axis of the gyroscope
sensor = I2C_9DOF(mag=False, invert_gyro=[0])
​
# create a transformation to take accelerometer readings & angles to z velocity
z_velocity  = Rotate('xy', inverse='y')
z_velocity += Slice(slice(2,3))
z_velocity += Add(-9.4)
z_velocity += Integrate(decay=0.95, dt_scale = True)
​
# set some sort of speed limit...
fs = 100
​
# networking object to send messages
node = Net_Node(id='IMU',
                 upstream='PLATFORM',
                 port=5560,
                 upstream_ip='192.168.0.101',
                 listens={},
                 instance=False)
​
last_read = time.time()
while True:
    if time.time()-last_read>1/fs:
        # read and process rotation
        rotation = sensor.rotation
        z_vel = z_velocity.process((sensor._acceleration, rotation))

        # send to platform
        node.send(to="PLATFORM", key="CONTINUOUS", value=z_vel)
        last_read = time.time()

Code running on pi with stepper array:

from autopilot.hardware.esoteric import Parallax_Platform
import numpy as np
from autopilot.core.networking import Net_Node
​
# lil callback function to update a global velocity from a message
vel = 0
def update_vel(input_vel):
   globals()['vel'] = input_vel[0]
​
# networking object to receive messages
node = Net_Node('PLATFORM',
   port=5565,
   route_port=5560,
   upstream='T',
   listens={'CONTINUOUS': update_vel},
   instance=False)
​
# instantiate platform
plat = Parallax_Platform()
plat.level()
​
# raise two rows of pillars
# create a mask to select pillars
mask = np.zeros((6,6), dtype=np.bool)
mask[2:4,:] = True
plat.mask = mask
# raise pillars in mask
plat.height = 5000
# wait for movement to complete (movements are asynchronous)
plat.join()
​
# put into velocity mode and let er rip
plat.move_mode = plat.Move_Modes.VELOCITY
​
velocity_scale = 200
last_vel = vel
try:
   while True:
       if vel != last_vel:
           plat.velocity = vel*velocity_scale
           last_vel = vel
except KeyboardInterrupt:
   plat.velocity = 0