function addtheta_sh(tr,pt)
%This code adds orientation of body
trj = csvread(tr); pos = csvread(pt);

temp1 = pos(:,3); temp2 = pos(:,4);
pos(:,3) =temp2; pos(:,4) = temp1;
pos(:,5) = pos(:,4);
trj(:,5) = trj(:,4);

[~,itr,ipt] = intersect(trj(:,1:3), pos(:,1:3),'rows');

trj(itr,4) = pos(ipt,4);

temp3 = trj(:,3); temp4 = trj(:,4);

trj(:,3) = temp4; trj(:,4) = temp3;

csvwrite('results_tracking_angle.csv',trj)




end