I get different yaw angle with different estimator when using the same data
ISOEhy opened this issue · comments
orientation = Mahony(gyr=gyr,
acc=acc,
mag=mag,
frequency=50.0,
frame='ENU') # Using IMU
orientation_EKF = EKF(gyr=gyr,
acc=acc,
mag=mag,
frequency=50.0,
frame='ENU') # Using IMU
orientation_Comp = Complementary(gyr=gyr,
acc=acc,
mag=mag,
frequency=50.0,
frame='ENU') # Using IMU
orientation_Mad = Madgwick(gyr=gyr,
acc=acc,
mag=mag,
frequency=50.0,
frame='ENU') # Using IMU
orientation_AQUA = AQUA(gyr=gyr,
acc=acc,
mag=mag,
frequency=50.0,
frame='ENU') # Using IMU
return orientation.Q, orientation_EKF.Q, orientation_Comp.Q, orientation_Mad.Q, orientation_AQUA.Q
Qs = trip_quaternions(fp)
from scipy.spatial.transform import Rotation as R
import matplotlib.pyplot as plt
fd = pd.read_csv(fp) #.columns
tows = fd['tow']
fts = ['Mahony', 'EKF', 'Complementary', 'Madgwick', 'AQUA']
for Q in Qs:
yaws = []
for Q0 in Q:
q = R.from_quat(Q0)
p, r, y = q.as_euler('zyx', degrees=True)
yaws.append(y)
plt.plot(tows, yaws)
plt.legend(fts)
plt.title('Yaw angle')