Advice on Estimating Velocity with Prime Hub's IMU Acceleration Data

[BY571]

Hey,
I'm currently working on a project where I need to accurately estimate the velocity of the Prime Hub based on its IMU acceleration data, specifically from imu.acceleration(Axis.X). Despite my efforts, I'm finding it challenging to obtain a reliable estimate due to the inherent noise and drift present in the acceleration data. While I've managed to mitigate the noise to some extent using a moving average filter, the drift remains a significant obstacle, leading to unreasonable and wrong results.

Does anyone have experience or advice on effectively compensating for the drift in IMU acceleration data or is there a direct method to obtain velocity data from the Prime Hub that I may have overlooked?
Any insights or advice would be greatly appreciated.
Thank you in advance for your help!

[laurensvalk]

drift present in the acceleration data

I wouldn't expect any drift in the acceleration data. Can you elaborate?

There may be a small nonzero stationary value due to imperfect alignment of the sensor in the casing, but I suppose you could subtract this.

is there a direct method to obtain velocity data from the Prime Hub

You can measure angular velocity (rotation), but not linear velocity (translation).

I need to accurately estimate the velocity of the Prime Hub based on its IMU acceleration data

Can you elaborate on the objective? Is it an independent homework problem? Or is it to improve driving a robot? Is it to estimate distance? Could the wheel speed be used to estimate the speed?

[BY571]

Thank you for your quick reply!

I wouldn't expect any drift in the acceleration data. Can you elaborate?

Here is a simple example with data I just gathered.
Situation: the Prime Hub is motionless on the table
I get the following measurements:
X-Acceleration 35.9046
X-Acceleration -2.39364
X-Acceleration -43.08552
X-Acceleration 33.51096
X-Acceleration -43.08552
X-Acceleration 2.39364
X-Acceleration 14.36184
X-Acceleration -4.78728
X-Acceleration -9.57456
X-Acceleration -7.18092

Now I move the Prime HUB a bit forward. Now again motionless I get the following measurements:
X-Acceleration 119.682
X-Acceleration 100.5329
X-Acceleration 100.5329
X-Acceleration 102.9265
X-Acceleration 131.6502
X-Acceleration 105.3202
X-Acceleration 117.2884
X-Acceleration 110.1074

I again move the Prime Hube and when motionless I read the following measurements:
X-Acceleration 368.6206
X-Acceleration 280.0559
X-Acceleration 289.6304
X-Acceleration 191.4912
X-Acceleration 215.4276
X-Acceleration 232.1831
X-Acceleration 272.875
X-Acceleration 234.5767

You can see there seems to be some increasing offset whenever I move it first having a mean acceleration of ~ 0, then ~ 100 and third time ~ 200.
Interestingly this happens also when I move the Prime Hub backwards. But normally the offset stays positive and in the range of [-300, 300] on rare events it might be higher than that like 500 or 600 range but only for a short amount of time and gets back in the 300 range.

Can you elaborate on the objective? Is it an independent homework problem? Or is it to improve driving a robot? Is it to estimate distance? Could the wheel speed be used to estimate the speed?

So I have a little robot with legs and want him to walk forward. The velocity is just one metric I would want to check but it could also be the distance walked. The issue is that the robot might stand still at some point or walk rather slowly and then the variable offset plays a crucial role. Maybe "drift" was wrongly chosen to describe the problem.

[Debenben]

To me it looks like your hub was tilted slightly different at each location, then those values are expected.

Some time ago I made a small calibration routine #943 for the accelerometer. I am happy to share a slightly improved version that calculates both an offset and a factor for each axis, feel free to give it a try:

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

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 = 0
# but are NOT interested in the trivial solution x = 0.
A = []

# We take two different stationary vectors v1 and v2 and demand that
#   (diag(a)*v1 - b)^2 == (diag(a)*v2 - b)^2
#    <=>
#   diag(a)*(diag(v1)*v1 - diag(v2)*v2)*a + 2*diag(a)*(v1 - v2)*b == 0
#    <=>:
#   A*x == 0
# with x = (b[0]*a[0], a[0]*a[0], b[1]*a[1], a[1]*a[1], b[2]*a[2], a[2]*a[2])

for v1 in stationary_positions:
    for v2 in stationary_positions:
        if abs(v1-v2) < 0.1:
            continue
        row = []
        for i in range(3):
            row.append(2*(v1[i] - v2[i]))
            row.append(v1[i]**2 - v2[i]**2)
        A.append(row)

# 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("eigen_values, eigen_vectors = np.linalg.eig(np.dot(A.T, A))")
print("x = eigen_vectors[:, np.argmin(eigen_values)]")
print("a2 = np.array([x[1], x[3], x[5]])")
print("a = np.sqrt(a2/np.average(a2))")
print("b = np.array([x[0]/a[0], x[2]/a[1], x[4]/a[2]])/np.average(a2)")
print("print(a, b)")


# The vector a is a multiplicative factor and b the offset that needs to be added
#a = Matrix([[1.00529377, 0, 0], [0, 0.99839546, 0], [0, 0, 0.99628858]])
#b = vector(-163.90077065, 74.68607559, -302.52333711)
#def get_calibrated_acceleration():
#   return a*hub.imu.acceleration() + b

Assuming you do not rotate the hub, move only for short amounts of time and start at a standstill you could calculate velocity like this:

from pybricks.hubs import ThisHub
from pybricks.geometry import Matrix, vector

hub = ThisHub()
#replace these values with your calibration results:
a = Matrix([[1.00302423, 0, 0], [0, 0.99240993, 0], [0, 0, 1.00452224]])
b = vector(8.5508267, -38.28099919, 19.20580666)

def get_calibrated_acceleration():
   return a*hub.imu.acceleration() + b

#keep history of acceleration values for averaging
accelerations = []
for i in range(100):
    accelerations.append(get_calibrated_acceleration())


while(True):
    #update acceleration history
    accelerations.pop(0)
    accelerations.append(get_calibrated_acceleration())

    #assume moving average is earth gravity
    gravity = vector(0, 0, 0)
    for x in accelerations:
        gravity += (x/len(accelerations))

    #current coordinate acceleration vector:
    #print(get_calibrated_acceleration() - gravity)

    #velocity obtained from integrating over last 10 acceleration values:
    velocity = vector(0, 0, 0)
    for i in range(1, 11):
        velocity += (accelerations[len(accelerations) - i] - gravity)
    print(velocity[0], velocity[1], velocity[2])

[laurensvalk]

So I have a little robot with legs and want him to walk forward. The velocity is just one metric I would want to check

Thanks for the context.

If you know the joint angles at a given time, you might be able to find the instantaneous point of rotation and the distance to the hub's center, and compute the forward velocity from the angular velocity, which you can measure.

I would anticipate this to be be much less noisy than integrating the acceleration measurement.

---

And thanks @Debenben for chiming in! Always much appreciated.

[BY571]

Thanks a lot, @Debenben and @laurensvalk for your feedback and input!

@Debenben thank you for sharing your script for calibration. I was also trying to do something similar a while ago but it was much less sophisticated and only subtracted the offset when noticing that the hub was stationary. I will give your version a try and let you know!

@laurensvalk This is a really good idea I will try it as well!

[Debenben]

Since we have better imu functions and calibration routines in pybricks now it should be a lot easier to do this integration more accurately now. In case anyone wants to give it a try, this is what I came up with:

"""
Proof-of-Concept for estimating movement along X-Axis from acceleration measurements.

Concept and notation based on en.wikipedia.org/wiki/Kalman_filter.
Time is measured in Seconds, distance in Millimenters.
State x = (coordinate position, coordinate velocity, coordinate acceleration).
"""

from pybricks.hubs import ThisHub
from pybricks.parameters import Axis
from pybricks.tools import Matrix, vector

hub = ThisHub() # use TechnicHub or Prime/InventorHub
if hub.imu.settings()[4] == (9806.65, -9806.65, 9806.65, -9806.65, 9806.65, -9806.65):
    print("Hub is not calibrated; Starting calibration!")
    import _imu_calibrate

def invert2x2(M): # returns inverse of matrix M
    a = M[0,0]
    b = M[0,1]
    c = M[1,0]
    d = M[1,1]
    return 1/(a*d - b*c)*Matrix([[d, -b], [-c, a]])

# parameters:
dt = 0.0011 # time between measurements = duration of while loop iteration. 0.0011 for TechnicHub 0.0008 for Prime/InventorHub.
sigma = 63 # uncertainty of measured coordinate acceleration z
variance = 1617 # variance of change of z
a_g = 9806.65 # gravity
a_thresh = 60 # acceleration change threshold for stationary detection
a_count = 10 # acceleration number threshold for stationary detection
I = Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # unit matrix
F = Matrix([[1, dt, 0.5*dt**2], [0, 1, dt], [0, 0, 1]]) # state transistion matrix
Q = variance*dt*I # uncorrelated noise
#Q = variance*Matrix([[0.25*dt**4, 0.5*dt**3, 0.5*dt**2], [0.5*dt**3, dt**2, dt], [0.5*dt**2, dt, 1]]) # piecewise white noise

# initialize values:
x_post = vector(0, 0, 0) # initial state
P_post = sigma*I # initial covariance
a_last = vector(0, 0, 0) # last measured acceleration value
stationary = 0 # count accelerations below threshold
count = 0 # count loop iterations

while True:
    count += 1

    # prediction:
    x_pred = F*x_post
    P_pred = F*P_post*F.T + Q

    # measurement:
    a = hub.imu.acceleration()
    O = hub.imu.orientation()
    if abs(a - a_last) < a_thresh:
        stationary += 1
    else:
        stationary = 0
    a_last = a

    if stationary > a_count:
        # hub is stationary -> coordinate acceleration and velocity measured as 0
        H = Matrix([[0, 0, 1], [0, 1, 0]])
        z = vector(0, 0)
        S = H*P_pred*H.T + sigma*Matrix([[1, 0], [0, 1]])
        K = P_pred*H.T*invert2x2(S)
    else:
        # hub is moving -> coordinate acceleration measured as z
        H = vector(0, 0, 1).T
        z = (H*O*a_g - a.T)[0]
        S = H*P_pred*H.T + sigma
        K = P_pred*H.T*S**(-1)

    # correction:
    x_post = x_pred + K*(z - H*x_pred)
    P_post = (I - K*H)*P_pred

    if count%100 == 0:
        # coordinate position, coordinate velocity, coordinate acceleration
        print(f"{x_post[0]:+09.2f}, {x_post[1]:+09.2f}, {x_post[2]:+09.2f}")

[Debenben]

@dlech I saw your comment in #2444 regarding a real-wold usecase that is limited by CPU time and would benefit from a sub-millisecond timer. I think the above is an example for both. To speed it up I would want to pre-allocate all memory and use integers instead of floats. Can you think of some low-hanging fruits that could be done in micropython?

[dlech]

I have doubts that the accelerometers have a low enough signal to noise ratio to actually be useful for this, but always interesting to measure it and see what it can do. 😄

To be as accurate as possible, this would need to be done in C code so that it could handle each sensor reading as it comes in so that it doesn't miss any.

But for the Python code, there are quite a few object constructors with constant values that could be pulled out of the loop to avoid having to create a new object on each loop. This could possibly improve performance. There are also some operations that only involve constants, like F.T that could be stored in a new constant.

Any matrix operation that returns a new matrix is also creating new objects. But I'm not sure that replacing it all with Python integer math would actually be a performance improvement since it would have to perform many more Python operations.