matlab/ins/AttiudeFromVectors.m

   1 function q = AttiudeFromVectors(mag,north,accel)
   2 % q = AttiudeFromVectors(mag,north,accel)
   3
   4 ge = [0 0 -9.81]';
   5 ge = ge / sqrt(ge' * ge);
   6 mag = mag / sqrt(mag' * mag);
   7 north = north / sqrt(north' * north);
   8 accel = accel / sqrt(accel' * accel);
   9
  10 Rib(1,:) = mag;
  11 Rib(2,:) = cross(mag,accel);
  12 Rib(3,:) = cross(Rib(1,:),Rib(2,:));
  13
  14 Rie(1,:) = north;
  15 Rie(2,:) = cross(north,ge);
  16 Rie(3,:) = cross(Rie(1,:),Rie(2,:));
  17
  18 R = (Rib' * Rie)';
  19
  20 m = [1 + R(1,1) + R(2,2) + R(3,3); ...
  21     1 + R(1,1) - R(2,2) - R(3,3); ...
  22     1 - R(1,1) + R(2,2) - R(3,3); ...
  23     1 - R(1,1) - R(2,2) + R(3,3)];
  24
  25 [mag,idx] = max(m);
  26 mag = 2 * sqrt(mag);
  27
  28 if (idx == 1)
  29     q = [mag/4; ...
  30         (R(2,3)-R(3,2))/mag; ...
  31         (R(3,1)-R(1,3))/mag; ...
  32         (R(1,2)-R(2,1))/mag];
  33 elseif (idx == 2)
  34     q(2) = mag/4;
  35     q(1) = (R(2,3)-R(3,2))/mag;
  36     q(3) = (R(1,2)+R(2,1))/mag;
  37     q(4) = (R(1,3)+R(3,1))/mag;
  38 elseif (idx == 3)
  39     q(3) = mag/4;
  40     q(1) = (R(3,1)-R(1,3))/mag;
  41     q(2) = (R(1,2)+R(2,1))/mag;
  42     q(4) = (R(2,3)+R(3,2))/mag;
  43 else
  44     q(4) = mag/4;
  45     q(1) = (R(1,2)-R(2,1))/mag;
  46     q(2) = (R(1,3)+R(3,1))/mag;
  47     q(3) = (R(2,3)+R(3,2))/mag;
  48 end
  49
  50 if (q(1) < 0)
  51     q = -q;
  52 end
  53
  54 q = reshape(q,[],1);