How can i draw a path relative to the room with orientation and acceleration relative to Sensor reference system?

Question

I have lead an experimente of human pedestrian dead-reckoning indoor . For this reaseon I chose to use a 6DOF IMU. the relative 3-axis acceleration respect to the sensor reference system and a 3-axis Angular-Velocity from gyroscope. Can you give me some tips on how I can get the position relative to the room in order to draw the displacement. I thought of proceding in this way: -compute the tangential velocity with tangetial motion velocity with costant jerk formula -compute the tangetial displacement -compute the angular displacement whit : |AngularVelocity|* deltaT (a sort of integration). -compute the rotation matrix from the angular displacement -compute the corrent position adding to the previous position the current displacemnt abtained pre-multiply the rotation matrix and tangetial displacement. What do you think about it?

vAng=gyroDataRaw;

a=[accDataRaw(:,1) accDataRaw(:,2) accDataRaw(:,3)];
vTang=zeros(size(a));
deltaT=Ts;
a0=[0 0 0];
vTang(1,:)=[0 0 0];

for i=2:N
    vTang(i,:)=vTang(i-1,:)+0.5*deltaT*(a(i-1,:)+a(i,:));
end

%% TANGENTIAL DISPLACEMENT

s=zeros(size(vTang));
for i=2:N
    s(i,:)=vTang(i-1,:)*deltaT+(1/3)*(deltaT^2)*(a(i-1,:)+(1/2)*a(i,:));
end


%% POSITION


totRot=eye(3);
position=ones(N,3);
for i=2:N
    rot=rotationMatrix(0,0,vAng(i,3));
    totRot=totRot*rot;
    position(i,:)=position(i-1,:)+(totRot*s(i-1,:)')';
end