0

I'm implementing a Madgwick filter using the `ahrs` library in Python, but I'm encountering an issue where the results are not stable when I input data from a stationary sensor. The output angles are increasing over time, even though the sensor is not moving. Here's what I've done:

1. First, I correct the input data to the format used by the `ahrs` library:

# Correct dimension of the data
for col in df.columns:
    if col.startswith('ACC'):
        df[col] *= 0.001 * 9.81  # Convert to m/s^2
    elif col.startswith('GYRO'):
        df[col] *= 0.001
        df[col] = np.deg2rad(df[col])  # Convert to radians/s
    elif col.startswith('MAG'):
        continue  # Already in correct format (uT)
  1. Then I apply the Madgwick filter:
madgwick = Madgwick(frequency=4)
Q = np.tile([1., 0., 0., 0.], (num_samples, 1))  # Allocate for quaternions
for t, row in df.iterrows():
    if t == 0:
        continue

    acc1_data = row.values[:3]
    gyro_data = row.values[3:6]
    acc2_data = row.values[6:9]
    mag_data = row.values[9:]
    
    # Select accelerometer
    acc_data = acc1_data

    Q[t] = madgwick.updateMARG(Q[t-1], gyr=gyro_data, acc=acc_data, mag=mag_data)

    orientation = quaternion_to_angle(Q[t])
    print(orientation)
  1. I'm using this function to convert quaternions to Euler angles:
def quaternion_to_angle(quaternion):
    w, x, y, z = quaternion

    roll = np.arctan2(2 * (w*x + y*z), 1 - 2 * (x**2 + y**2))
    pitch = np.arcsin(2 * (w*y - z*x))
    yaw = np.arctan2(2 * (w*z + x*y), 1 - 2 * (y**2 + z**2))

    return np.rad2deg(roll), np.rad2deg(pitch), np.rad2deg(yaw)

Despite the sensor being stationary, the output angles are continuously increasing. I expected them to remain relatively constant given that there's no actual movement.

What could be causing this drift in the Madgwick filter output? Are there any common pitfalls or corrections I should be aware of when dealing with stationary data?

Any insights or suggestions would be greatly appreciated!

0

Browse other questions tagged or ask your own question.