matlab - 如何绘制相对于房间的路径，其方向和加速度相对于传感器参考系统？

Question

我领导了一个人类行人推算室内的实验。出于这个原因，我选择使用 6DOF IMU。相对于传感器参考系统的相对 3 轴加速度和来自陀螺仪的 3 轴角速度。你能给我一些关于如何获得相对于房间的位置以绘制位移的提示吗？我想以这种方式进行： - 使用常数加加速度公式计算切向运动速度的切向速度 - 计算切向位移 - 计算角位移：|AngularVelocity|* deltaT（一种积分）。-从角位移计算旋转矩阵 -计算当前位置加上当前位移获得的前一个位置，预乘旋转​​矩阵和切向位移。你怎么看待这件事？

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