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!