clc;
clear all;
close all;
VarInput = load('Hw3_quadData.mat');
qDat = VarInput.quadrotor;
[n,~] = size(qDat);
Xi_0 = [qDat(1,2:4)'; qDat(1,8:10)'];
v = qDat(:,5:7);
omega = qDat(:,11:13);
t = qDat(:,1);
dt = qDat(2,1) - qDat(1,1);
T_init = qDat(1,1);
T_final = qDat(end,1);
pose_dot(1,Xi_0,{dt,v,omega});
[t_out,Xi] = ode45(@pose_dot,[T_init T_final],Xi_0);
figure(1)
plot(t_out,Xi(:,4:6)*(180/pi),':');
hold on
plot(t,qDat(:,8:10)*(180/pi),'--');
legend('\phi_{calc}','\theta_{calc}','\psi_{calc}','\phi_{true}','\theta_{true}','\psi_{true}');
grid on
title('Euler Angles vs. Time');
ylabel('Euler Angles (Degrees)');
xlabel('Time (Seconds)');
figure(2)
plot(t_out,Xi(:,1:3),':');
hold on
plot(t,qDat(:,2:4),'--')
legend('x_{calc}','y_{calc}','z_{calc}','x_{true}','y_{true}','z_{true}');
grid on
title('CoM Position vs Time');
ylabel('Position (meters)');
xlabel('Time (seconds)');
function Xi_dot1 = pose_dot(t,Xi,prstVars)
if nargin > 2
dt = prstVars{1};
v = prstVars{2};
omega = prstVars{3};
end
ind = floor(t/dt) + 1;
nu = [v(ind,:)'; omega(ind,:)'];
cPsi = cos(Xi(6));
sPsi = sin(Xi(6));
cPhi = cos(Xi(4));
sPhi = sin(Xi(4));
cTh = cos(Xi(5));
sTh = sin(Xi(5));
tTh = tan(Xi(5));
Xi_dot1 = [cPsi*cTh cPsi*sTh*sPhi-sPsi*cPhi sPsi*sPhi+cPsi*sTh*cPhi 0 0 0;...
sPsi*cTh cPsi*cPhi+sPsi*sTh*sPhi sPsi*sTh*cPhi-cPsi*sPhi 0 0 0;...
-sTh cTh*sPhi cTh*cPhi 0 0 0;...
0 0 0 1 sPhi*tTh cPhi*tTh;...
0 0 0 0 cPhi -sPhi;...
0 0 0 0 sPhi/cTh cPhi/cTh]*nu;
end