mat2quat.m

来自「姿态定位完整的matlab算法」· M 代码 · 共 7 行

function qnb = Mat2Quat(M)  
%姿态矩阵转化为姿态四元数   
    qnb = [                     sqrt(abs(1.0 + M(1,1) + M(2,2) + M(3,3)))/2.0;
          sign(M(3,2)-M(2,3)) * sqrt(abs(1.0 + M(1,1) - M(2,2) - M(3,3)))/2.0;
          sign(M(1,3)-M(3,1)) * sqrt(abs(1.0 - M(1,1) + M(2,2) - M(3,3)))/2.0;
          sign(M(2,1)-M(1,2)) * sqrt(abs(1.0 - M(1,1) - M(2,2) + M(3,3)))/2.0 ];