clc
GRAVITY = 9.80665;
+RAD2DEG = 57.2957795;
Max_Range_Accel = 39.203407; Min_Range_Accel = -39.204006; Res_Accel = 0.000598;
Max_Range_Gyro = 1146.862549; Min_Range_Gyro = -1146.880005; Res_Gyro = 0.017500;
Max_Range_Magnetic = 1200; Min_Range_Magnetic = -1200; Res_Magnetic = 1;
+PITCH_PHASE_CORRECTION = -1;
+ROLL_PHASE_CORRECTION = -1;
+YAW_PHASE_CORRECTION = -1;
+
Bias_Ax = 0.098586;
Bias_Ay = 0.18385;
Bias_Az = 10.084 - GRAVITY;
OR_aid = zeros(3,BUFFER_SIZE);
OR_err = zeros(3,BUFFER_SIZE);
+euler = zeros(BUFFER_SIZE,3);
+
% Sensor Data simulating orientation motions
% get accel x,y,z axis data from stored file
Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,4))')(1:BUFFER_SIZE);
% estimate orientation
-[OR_driv, OR_aid, OR_err] = estimate_orientation(Accel_data, Gyro_data, Mag_data);
+[Quat_driv, Quat_aid, Quat_err] = estimate_orientation(Accel_data, Gyro_data, Mag_data);
+
+euler = quat2euler(Quat_aid);
+OR_aid(1,:) = euler(:,2)' * RAD2DEG;
+OR_aid(2,:) = euler(:,1)' * RAD2DEG;
+OR_aid(3,:) = euler(:,3)' * RAD2DEG;
+
+euler = quat2euler(Quat_driv);
+OR_driv(1,:) = ROLL_PHASE_CORRECTION * euler(:,2)' * RAD2DEG;
+OR_driv(2,:) = PITCH_PHASE_CORRECTION * euler(:,1)' * RAD2DEG;
+OR_driv(3,:) = YAW_PHASE_CORRECTION * euler(:,3)' * RAD2DEG;
+
+euler = quat2euler(Quat_err);
+OR_err(1,:) = euler(:,2)' * RAD2DEG;
+OR_err(2,:) = euler(:,1)' * RAD2DEG;
+OR_err(3,:) = euler(:,3)' * RAD2DEG;
% Rotation Plot Results
hfig=(figure);