Результаты фильтра Мэджвика нестабильны при стационарных входных данных ⇐ Python

[Anonymous]

Я реализую фильтр Мэджвика, используя библиотеку `ahrs` в Python, но столкнулся с проблемой, из-за которой результаты нестабильны при вводе данных со стационарного датчика. Выходные углы со временем увеличиваются, даже если датчик не движется. Вот что я сделал:
1. Сначала корректирую входные данные до формата, используемого библиотекой `ahrs`:

Код: Выделить всё

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

• Затем я применяю фильтр Мэджвика:

Код: Выделить всё

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)

• Я использую эту функцию для преобразования кватернионов в углы Эйлера:

Код: Выделить всё

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)

Несмотря на то, что датчик неподвижен, выходные углы постоянно увеличиваются. Я ожидал, что они останутся относительно постоянными, учитывая отсутствие фактического движения.
Что может быть причиной такого отклонения на выходе фильтра Мэджвика? Существуют ли какие-либо распространенные ошибки или исправления, о которых мне следует знать при работе со стационарными данными?
Будем очень признательны за любые идеи или предложения!

Подробнее здесь: https://stackoverflow.com/questions/787 ... input-data