plot_contrast.m

IMU姿态跟踪

function plot_contrast(vicon,mean_rot,sample_rot,R,i)

figure(1);
subplot(1,3,1);
rotplot(R,i);
% title('true');

subplot(1,3,2);

rx=[1,0,0;
        0,cos(mean_rot(1)),-sin(mean_rot(1));
        0,sin(mean_rot(1)),cos(mean_rot(1))];
    ry=[cos(mean_rot(2)),0,sin(mean_rot(2));
        0,1,0;
        -sin(mean_rot(2)),0,cos(mean_rot(2))];
    rz=[cos(mean_rot(3)),-sin(mean_rot(3)),0;
        sin(mean_rot(3)),cos(mean_rot(3)),0;
        0,0,1];
    r=rz*ry*rx;
    rotplot(r,i);
%     title('estimation');
    
    subplot(1,3,3);
    temp=sprintf('iteration:%d',i);
    
    scatter3(sample_rot(1,:),sample_rot(2,:),sample_rot(3,:),'b+');
    hold on
    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));
    scatter3(rot_x,rot_y,rot_z,'ro');
    scatter3(mean_rot(1),mean_rot(2),mean_rot(3),'go');
    hold off
    axis([-1 1 -1 1 -1 1]);
    title(temp);