close all
clear all

addpath('SpinCalc');

dt = 1/50;  
ts = 1; %Time at which step change occurs
tf = 10;  %Final Time


QQ = SpinCalc('EA321toQ',[0 0 0],.001,1);

for i = 1:ts/dt
  q(i,:) = QQ;
end

QQ = SpinCalc('EA321toQ',[90 0 0],.001,1);
for i = ts/dt+1:tf/dt
  q(i,:) = QQ;
end


q = q(1:1:end,:);


tau = .1;

[InstAxisOfRotation, AngularRate] = ComputeAngularDisplacement(q,dt,tau);

t = dt*(0:length(AngularRate)-1);


figure('Position',[  1058         480        1160         992]);
ah1 = subplot(3,1,1);
plot(t,InstAxisOfRotation);
title(['Quat Rotations Between Timesteps - Low Pass Filter tau = ' num2str(tau) ' s']);
ylabel('Quat Components');
legend('X','Y','Z');
ah2 = subplot(3,1,2);
plot(t,AngularRate);
ylabel('Angular Rotation Rate (degrees/second)');
ah3 = subplot(3,1,3);
plot(t,dt*cumtrapz(AngularRate));
ylabel('Cumulative Rotation (degrees)');
grid on;
linkaxes([ah1 ah2 ah3],'x');



