Offline AHRS Use for Yaw / Pitch / Roll estimation with Tobii Glasses 3

[nomisch]

Hello !

First, thanks for this nice repository, very cool to hve all algos on the same place !

In my project, I am trying to obtain the Yaw Pitch Roll from the Accelerometer, Gyroscope and Magnetometers collected from the IMU of Tobii Glasses 3.
I am trying to obtain the YPR a posteriori, once the recording has been completed and the data has been stored in text files.

I am having some difficulties understanding exactly how to use the AHRS example.
Assuming the recording starts with the glasses lying on a flat table, I want to be able to know when the user is looking up/down (the angle at whcih he is looking) -> So I want to know the Pitch value of the glasses over-time.

Unfortunately, my current code does not seem to produce consistent results, according to my tests.

Let me describe my current Python code below:

1. Read and process data
2. Use AHRS to compute YPR
3. Plot YPR on a graph

Imports

import ahrs
import matplotlib.pyplot as plt
import pandas as pd
import scipy.signal
from scipy.spatial.transform import Rotation as Rot

1. Read and process data

        df = pd.read_csv(filepath, delimiter='\t', decimal=',')

        acc_data = df[['Accelerometer X', 'Accelerometer Y', 'Accelerometer Z']].astype(float)
        acc_data = acc_data.dropna(axis=0).to_numpy()

        gyro_data = df[['Gyro X', 'Gyro Y', 'Gyro Z']].astype(float)
        gyro_data = gyro_data.dropna(axis=0)
        gyro_data = gyro_data.mul(math.pi/180.0) #From Tobii : Gyro data is in degree/sec, we need to convert it in rad/sec
        gyro_data = gyro_data.to_numpy()

        # We need to resample magnetometer data as it was processed at 10Hz and not 100hz
        mag_data = df[['Magnetometer X', 'Magnetometer Y', 'Magnetometer Z']].astype(float)
        mag_data = mag_data.dropna(axis=0).to_numpy()
        mag_data = scipy.signal.resample(mag_data, gyro_data.shape[0])

2. Process data: using either the Madwick algorithm or the EKF algorithm

attitudes = ahrs.filters.Madgwick(acc=acc_data, gyr=gyro_data, mag=mag_data, frequency=100.0)

or

attitudes = ahrs.filters.EKF(acc=acc_data, gyr=gyro_data, mag=mag_data, frequency=100.0),

3. Plot data: I suspect I do something wrong here ...

    # Compute Yaw Pitch Roll over time and plot
    rpy = []
    for q in attitudes.Q:
        r = Rot.from_quat(q)
        rpy.append(r.as_euler('xyz', degrees=True))
    df = pd.DataFrame.from_records(rpy)
    sns.lineplot(data=df)
    plt.title("InputData:" + inDataType + "  Algorithm: " +algoName)
    plt.legend(["Roll", "Pitch", "Yaw"])
    plt.show()

What makes me think I have an error ?

• When I do some static tests (keeping the glasses static): I have a lot of drift.
• When I do some dynamic tests (tilting the the glasses up/down): I don't seem to see the corresponding orientation on the plots
• When I compare the results from Madgwick and EKF, they don't look the same at all

I attached two sample plot for static conditions below

Thanks for your help !