1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-05 13:24:11 +01:00
LibrePilot/matlab/ins/AttiudeFromVectors.m
2012-04-03 03:42:20 -05:00

54 lines
1.2 KiB
Matlab

function q = AttiudeFromVectors(mag,north,accel)
% q = AttiudeFromVectors(mag,north,accel)
ge = [0 0 -9.81]';
ge = ge / sqrt(ge' * ge);
mag = mag / sqrt(mag' * mag);
north = north / sqrt(north' * north);
accel = accel / sqrt(accel' * accel);
Rib(1,:) = mag;
Rib(2,:) = cross(mag,accel);
Rib(3,:) = cross(Rib(1,:),Rib(2,:));
Rie(1,:) = north;
Rie(2,:) = cross(north,ge);
Rie(3,:) = cross(Rie(1,:),Rie(2,:));
R = (Rib' * Rie)';
m = [1 + R(1,1) + R(2,2) + R(3,3); ...
1 + R(1,1) - R(2,2) - R(3,3); ...
1 - R(1,1) + R(2,2) - R(3,3); ...
1 - R(1,1) - R(2,2) + R(3,3)];
[mag,idx] = max(m);
mag = 2 * sqrt(mag);
if (idx == 1)
q = [mag/4; ...
(R(2,3)-R(3,2))/mag; ...
(R(3,1)-R(1,3))/mag; ...
(R(1,2)-R(2,1))/mag];
elseif (idx == 2)
q(2) = mag/4;
q(1) = (R(2,3)-R(3,2))/mag;
q(3) = (R(1,2)+R(2,1))/mag;
q(4) = (R(1,3)+R(3,1))/mag;
elseif (idx == 3)
q(3) = mag/4;
q(1) = (R(3,1)-R(1,3))/mag;
q(2) = (R(1,2)+R(2,1))/mag;
q(4) = (R(2,3)+R(3,2))/mag;
else
q(4) = mag/4;
q(1) = (R(1,2)-R(2,1))/mag;
q(2) = (R(1,3)+R(3,1))/mag;
q(3) = (R(2,3)+R(3,2))/mag;
end
if (q(1) < 0)
q = -q;
end
q = reshape(q,[],1);