test_bias_a.m

IMU姿态跟踪

a=[];
for i=1:size(imu,2)
    diff_R=eye(3)-vicon(1,i).R;
    test_R=sum(sum(diff_R));
    a=[a;test_R];
end
[min_a,a_ind]=min(a);
bias_a=a_t(a_ind,:)-[0,0,1];