Fixees and closes #149 (bug in matrix_to_quaternion)