grav_est.m

IMU姿态跟踪

function a=grav_est(rot_3)
for i=1:size(rot_3,1)
    rx=[1,0,0;
        0,cos(rot_3(i,1)),-sin(rot_3(i,1));
        0,sin(rot_3(i,1)),cos(rot_3(i,1))];
    ry=[cos(rot_3(i,2)),0,sin(rot_3(i,2));
        0,1,0;
        -sin(rot_3(i,2)),0,cos(rot_3(i,2))];
    rz=[cos(rot_3(i,3)),-sin(rot_3(i,3)),0;
        sin(rot_3(i,3)),cos(rot_3(i,3)),0;
        0,0,1];
    r=rz*ry*rx;
    a(i,:)=(inv(r)*[0;0;1])';
      a(i,2)=-a(i,2);
end