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)
- 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)
- 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!