function q = rot2cuat(R)
    eta = sqrt(1+R(1,1)+R(2,2)+R(3,3))/2;
    eps = (1/(4*eta)) * [R(3,2)-R(2,3); R(1,3)-R(3,1); R(2,1)-R(1,2)];
    q = [eta; eps];
end