data = xlsread('data.xlsx');
t = data(:,1);
x = data(:,2);
y = data(:,3);
z = data(:,4);
smoothOrder = 8; %%change the smooth order to fit better
t1 = t(1:end-smoothOrder);
t2 = t(smoothOrder+1:end);
x1 = x(1:end-smoothOrder);
x2 = x(smoothOrder+1:end);
y1 = y(1:end-smoothOrder);
y2 = y(smoothOrder+1:end);
z1 = z(1:end-smoothOrder);
z2 = z(smoothOrder+1:end);
dx = (x2-x1)./(t2-t1); %central difference (velocity)
dy = (y2-y1)./(t2-t1); %central difference (velocity)
dz = (z2-z1)./(t2-t1); %central difference (velocity)
tt = (t2+t1)./2;
tt1 = tt(1:end-smoothOrder);
tt2 = tt(smoothOrder+1:end);
dx1 = dx(1:end-smoothOrder);
dx2 = dx(smoothOrder+1:end);
dy1 = dy(1:end-smoothOrder);
dy2 = dy(smoothOrder+1:end);
dz1 = dz(1:end-smoothOrder);
dz2 = dz(smoothOrder+1:end);
ddx = (dx2-dx1)./(tt2-tt1); %central difference (acceleration)
ddy = (dy2-dy1)./(tt2-tt1); %central difference (acceleration)
ddz = (dz2-dz1)./(tt2-tt1); %central difference (acceleration)
ttt = (tt2+tt1)./2;
subplot(3,3,1), plot(t,x,'k'), title('x data');
axis([min(t) max(t) min(x) max(x)]);
subplot(3,3,2), plot(t,y,'k'), title('y data');
axis([min(t) max(t) min(y) max(y)]);
subplot(3,3,3), plot(t,z,'k'), title('z data');
axis([min(t) max(t) min(z) max(z)]);
subplot(3,3,4), plot(t(1:end-1),diff(x)./diff(t),'r.:',tt,dx,'b');
axis([min(t) max(t) min(dx) max(dx)]);
title('dx data'), legend('backward diff','central diff');
subplot(3,3,5), plot(t(1:end-1),diff(y)./diff(t),'r.:',tt,dy,'b');
axis([min(t) max(t) min(dy) max(dy)]);
title('dy data'), legend('backward diff','central diff');
subplot(3,3,6), plot(t(1:end-1),diff(z)./diff(t),'r.:',tt,dz,'b');
axis([min(t) max(t) min(dz) max(dz)]);
title('dz data'), legend('backward diff','central diff');
subplot(3,3,7), plot(t(1:end-2),diff(x,2)./diff(t,2),'r.:',ttt,ddx,'b');
axis([min(t) max(t) min(ddx) max(ddx)]);
title('d^2x data'), legend('backward diff','central diff');
subplot(3,3,8), plot(t(1:end-2),diff(y,2)./diff(t,2),'r.:',ttt,ddy,'b');
axis([min(t) max(t) min(ddy) max(ddy)]);
title('d^2y data'), legend('backward diff','central diff');
subplot(3,3,9), plot(t(1:end-2),diff(z,2)./diff(t,2),'r.:',ttt,ddz,'b');
axis([min(t) max(t) min(ddz) max(ddz)]);
title('d^2z data'), legend('backward diff','central diff');
