So i want to compute the euler angles and a rotation matrix to adjust the axis of the accelerometer. I am using the accelerometer (420 Hz), gyroscope (420 Hz) and magnetometer (100Hz) sensors of my phone. A Samsung Galaxy FE 21. The phone is just standing still and still the quaternions and therefore the euler angles and rotation matrix are changing. Changing means at the beginning adjusted acceleration axis y is near the g value -9.81 and at the end the adjusted acceleration axis z is near the g value. Can anyone explain why?
Here is the output of the quaternions, where already the mistake is probably happening.
array([[ 0.69195007, 0.02116878, 0.01067919, 0.72155592],
[ 0.69207007, 0.02128627, 0.01067362, 0.72143744],
[ 0.69219179, 0.02140226, 0.0106742 , 0.72131722],
...,
[ 0.99924922, 0.02342693, -0.00675266, 0.03010937],
[ 0.99926311, 0.02316672, -0.0062859 , 0.02995027],
[ 0.99926311, 0.02316672, -0.0062859 , 0.02995027]])
import pandas as pd
import numpy as np
from ahrs.filters import Madgwick
from ahrs.common.quaternion import Quaternion
freq="0.01S"
hz=100
start=30.0
# begin by adjusting the frequency of the different sensors through resampling. I do this for every sensor.
acc["Time_s"]=0
acc.Time_s=pd.to_datetime(acc.Time, unit="s")
acc['time_str'] = acc['Time_s'].dt.strftime('%S.%f')
acc.set_index('Time_s', inplace=True)
acc_down=acc.resample(freq).mean()
acc_loc=acc_down.loc[acc_down["Time"]>=start]
min_sample_size=min(len(gyro_loc),len(mag_loc), len(acc_loc))
gyro_loc=gyro_loc.tail(min_sample_size)
acc_loc=acc_loc.tail(min_sample_size)
mag_loc=mag_loc.tail(min_sample_size)
```
# I then use the outputs for computing the quaternion, euler angles and rotation matrix
```
madgwick = Madgwick(gyr=gyro_array, acc=acc_array, mag=mag_array, frequency=hz)
#https://automaticaddison.com/how-to-convert-a-quaternion-into-euler-angles-in-python/
q = np.array([0.0, 0.0, 0.707, 0.707])
# Compute the Euler angles
def euler_from_quaternion(q):
r0=2*(q[3]*q[0]+q[1]*q[2])
r1=1-2*(q[0]**2+q[1]**2)
roll = np.arctan2(r0,r1)
roll_deg = np.degrees(roll)
p1=2*(q[3] * q[1] - q[2]*q[0])
pitch=np.arcsin(p1)
pitch_deg=np.degrees(pitch)
y1=2*(q[3] * q[2]+q[0]*q[1])
y2=1-2*(q[1]**2+q[2]**2)
yaw=np.arctan2(y1,y2)
yaw_deg=np.degrees(yaw)
return(roll_deg, pitch_deg, yaw_deg)
#compute rotation matrix
def compute_r_matrix(roll, pitch, yaw):
roll=np.radians(roll)
pitch=np.radians(pitch)
yaw=np.radians(yaw)
roll_mat=[[1,0,0],[0,np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]
pitch_mat=[[np.cos(pitch), 0, np.sin(pitch)], [0,1,0], [-np.sin(pitch), 0, np.cos(pitch)]]
yaw_mat=[[np.cos(yaw), -np.sin(yaw), 0],
[np.sin(yaw), np.cos(yaw), 0],
[0,0,1]]
rot_mat = np.dot(yaw_mat, np.dot(pitch_mat, roll_mat))
#print(rot_mat)
return(rot_mat)
qs=madgwick.Q
i=0
adjust_x_list=list()
adjust_y_list=list()
adjust_z_list=list()
for q in qs:
roll_deg, pitch_deg, yaw_deg=euler_from_quaternion(q)
rot_matrix=compute_r_matrix(roll_deg, pitch_deg, yaw_deg)
adjust_a=np.dot(rot_matrix, acc_array[i])
i = i + 1
adjust_x_list.append(adjust_a[0])
adjust_y_list.append(adjust_a[1])
adjust_z_list.append(adjust_a[2])