How to convert the quaternion to euler angles in EKF filter?

July 9, 2023

I am using the extended kalman filter and trying to find the orientation from the mag, accel and gyr. I get the quaternion, but I want to convert it to euler angles. How should I do that? should I import something as well?
this is the code:

import numpy as np
from ahrs.filters import QUEST
from ahrs.filters import EKF
from ahrs.common.orientation import acc2q
acc_data=np.array([[1,3,5.8]],dtype=float)
gyr_data=np.array([[2,4,8]],dtype=float)
mag_data=np.array([[1,2,3]],dtype=float)
ekf = EKF(gyr=gyr_data, acc=acc_data, mag=mag_data)
num_samples = 1             # Assuming sensors have 1000 samples each
Q = np.zeros((num_samples, 4))  # Allocate array for quaternions
Q[0] = acc2q(acc_data[0])       # First sample of tri-axial accelerometer
for t in range(1, num_samples):
    
    Q[t] = ekf.update(Q[t-1], gyr_data[t], acc_data[t])

>Solution :

import numpy as np
from math import atan2, asin

def quaternion_to_euler(q):
    roll = atan2(2*(q[0]*q[1] + q[2]*q[3]), 1 - 2*(q[1]**2 + q[2]**2))
    pitch = asin(2*(q[0]*q[2] - q[3]*q[1]))
    yaw = atan2(2*(q[0]*q[3] + q[1]*q[2]), 1 - 2*(q[2]**2 + q[3]**2))
    return np.array([roll, pitch, yaw])

Use the quaternion_to_euler function to convert the quaternion to Euler angles: