-[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;