function q = qnormlz(q)
% 四元数归一化
    q = q/norm(q);
    if(q(1)<0)
        q(1) = -q(1);
        q(2) = -q(2);
        q(3) = -q(3);
        q(4) = -q(4);
    end