Integration of accelerations to produce spatial displacement

I am doubly integrating accelerations from a device in order to determine the displacement graph. I have written this code;

A1 = load('datat.txt')
A = length(A1)
time = (length(A1)/40); % the amount of time the device was switched on for
t=linspace(0,time,length(A1));
Az2=(A1*9.81);%converting the data into m/s^2

Az3 = padarray(Az2,[0 3],'post');

vz(length(Az3),:)=0; % setting out a vector containing zero's to then fill

i=1;

while (i<A)
vz(i)=1/2*((t(i+1)-t(i))*((Az3(i)+Az3(i+1))));
i=i+1;
end

Bz1=vz;
B = length(Bz1);
time = (length(Bz1)/40); %the amount of time the device was switched on for
t=linspace(0,time,B); % seperating time properly

Bz3 = padarray(Bz1,[0 3],'post');
sz(length(Bz1),:)=0; % setting out a vector containing zero's to then fill

i=1;

while (i<B)
sz(i)=1/2*((t(i+1)-t(i))*((Bz3(i)+Bz3(i+1))/2));
i=i+1;
end
figure(2)
plot(sz)

However my displacement graph suggests that the strides of the subject wearing the device are very small. Is my approach correct. Is there a function in matlab that can do this in a better way?