dcm2qua.m

惯性导航系统传递对准系统模型程序

function qua_vec = dcm2qua(DCMbn)
%四元数计算公式。输入导航系和载体系间的方向余弦阵DCMbn，输出四元数的a/b/c/d。
  if nargin<1,error('insufficient number of input arguments'),end
  
  a = 0.5*sqrt(1+DCMbn(1,1)+DCMbn(2,2)+DCMbn(3,3));
  tmp = inv(4*a);
  b = tmp*(DCMbn(3,2)-DCMbn(2,3));
  c = tmp*(DCMbn(1,3)-DCMbn(3,1));
  d = tmp*(DCMbn(2,1)-DCMbn(1,2));
  
  qua_vec = [a b c d];