%% Read OptiTrack File
clear all
filename = 'me133a_6nov_wand_250_270m.csv';
fileraw = csvread(filename,7,0);
time = fileraw(:,2);
rb_q = fileraw(:,3:6)';
rb_xyz = (fileraw(:,7:9))';
nmarkers = 5;
marker = cell(nmarkers,1);
for i=1:nmarkers
   marker{i} =  fileraw(:,7+4*i:9+4*i);
end

%% Plot Data

% 2D
figure
plot(time,rb_q')
legend('q_x','q_y','q_z','q_w')
xlabel('Time(s)')
ylabel('Quaternion components')


figure
plot(time,rb_xyz)
xlabel('Time(s)')
ylabel('World Position')
legend('x','y','z')


% 3D Path
figure
plot3(rb_xyz(1,:),rb_xyz(2,:),rb_xyz(3,:))
axis equal
grid on


