任意旋转矢量到坐标变换矩阵的转换

function rotvec = CTMat2RotVec（CTMat）;
%
% Converts coordinate transformation matrix to rotation vector
% 
% M. S. Grewal L. R. Weill and A. P. Andrews
% Global Positioning Systems Inertial Navigation and Integration
% 2nd Edition Wiley 2006
%
% 1. Compute antisymmetric part of CTMat to get sine of theta
%
A          = （1/2）*（CTMat - CTMat‘）;
sine_theta = sqrt（A（32）^2+A（13）^2+A（21）^2）;
%
% 2. Solve for theta using Eq. C.125
%
theta = atan2（sine_theta（CTMat（11）+CTMat（22）+CTMat（33）-1）/2）;
%
% 3. Use truncated Taylor series approximation
%    near theta = 0 for theta/sin（theta）
%    and symmetric part of CTMat near theta = pi
%    in alternative formula to avoid computing theta/sin（theta） 
%
if sine_theta < .001
    if theta < .01
        thetaoversintheta = 1+1/6*theta^2+7/360*theta^4;
        rotvec = thetaoversintheta*[CTMat（32）;CTMat（13）;CTMat（21）];
    else
        %
        % Compute symmetric part of CTMat
        %
        S = （1/2）*（CTMat+CTMat‘）;
        %
        % Use Eq. C.133ff for solution
        %
        % This can still give different sign on rhovec if theta = pi
        % but rhovec and -rhovec represent the same rotation under
        % those conditions.  This is a problem with rotation vector
        % representation not with this function.