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