change_format.m

IMU姿态跟踪

function [a,w,a_ind,rot_3]=change_format(imu,vicon)
% load('train.mat');
% q=[];
% for i=1:size(vicon,2)
%     dcm=vicon(1,i).R;
%     qq=dcm2q(dcm);
%     q=[q;vicon(1,i).t,qq];
%     q_ind(i)=vicon(1,i).t;
% end
% q=[q_ind',q];
a=[];
for i=1:size(imu,2)
    ax=(imu(1,i).ax/1023*3.3-1.65)/0.8;
    ay=(imu(1,i).ay/1023*3.3-1.65)/0.8;
    az=(imu(1,i).az/1023*3.3-1.65)/0.8;
    a=[a;ax,ay,az];
    a_ind(i)=imu(1,i).t;
end
% mean_a=mean(a);
% a=[a_ind',a];


w=[];
for i=1:size(imu,2)
    wx=(imu(1,i).wx/1023*3.3-1.23)/0.002*pi/180;
    wy=(imu(1,i).wy/1023*3.3-1.23)/0.002*pi/180;
    wz=(imu(1,i).wz/1023*3.3-1.23)/0.002*pi/180;
    w=[w;wx,wy,wz];
end
% w=[a_ind',w];
% w=unique(w,'rows');
% for i=1:size(imu,2)
%     Rg(i,:)=(vicon(1,i).R*[0;0;1])';
% end
% b=(a(:,2:4)-Rg);
% mean_b=mean(b);
if nargin > 1
rot_3=[];
for i=1:size(imu,2)
    rot_mat=vicon(1,i).R;
    rot_x=atan2(rot_mat(3,2),rot_mat(3,3));
    rot_y=-asin(rot_mat(3,1));
    rot_z=atan2(rot_mat(2,1), rot_mat(1,1));
    rot_3=[rot_3;rot_x,rot_y,rot_z];
end
else
    rot_3=[];
end