function [ rv ] = q2rv( q )
%% **************************************************************
%名称：Quaternion to Rotation Vector
%功能：将姿态四元数转化为等效旋转矢量
%________________________________________________________________________
% 输入：
%       q: 
% 输出：
%       rv: 
%_________________________________________________________________________
%作者：哈尔滨工程大学 自动化学院 张峥
%日期：2020年10月2日
% ************************************************************************
%%
% 原理@ 捷联惯导算法与组合导航原理讲义 P233
% 首先将四元数转化为标量非负四元数
if q(1) < 0
    q = -q;
end

% 等效旋转矢量模值的一半
nmhalf = acos(q(1));

if nmhalf > 1e-20
    b = 2*nmhalf/sin(nmhalf);
else
    b = 2;
end

rv = b*q(2:4);

end