Depending on the literature, I sometime find
q1 = (C_23 - C_32) / (4 * q_c)and sometimes the other sign around, and I'm not sure why, which would explain this conjugate issue. Could you help me please?
Part of your math is following the convention for selecting the positive root of q_c, and part of your math is following the convention of selecting the negative root of q_c.
The paper Computation of the Quaternion from a Rotation Matrix has a good explanation of the process for finding a quaternion from a rotation matrix. There are multiple formulas you can use, and some have better numerical stability than others, but the one you are using is "0.1.2 Solution of diagonal for b1."
(Note that Farrell follows a scalar first convention, and that q_c in your code corresponds to b1 in the paper.)
In this step, he solves the equation 4*b1**2 = 1 + R_11 + R_22 + R_33 by taking the square root, to obtain b1 = np.sqrt(1 + R_11 + R_22 + R_33) / 2. In this step, he is taking the positive root. However, it is equally valid to say that b1 = -np.sqrt(1 + R_11 + R_22 + R_33) / 2 is a solution.
He addresses this choice in the summary:
Each of the quaternions involves a sign ambiguity due to the fact that either the positive or negative square root could have been selected. This document has selected the positive square root throughout. If the negative square root is selected, then the direction of the vector portion of the quaternion will also be reversed. This results in the same rotation matrix.
I am guessing that that is where your confusion stems from: you are combining code from a source that uses the positive root to obtain the scalar component with code from a source that uses the negative root to obtain the imaginary components.
The simplest fix is to swap the signs here:
q_vec = np.array([[C_32 - C_23], [C_13 - C_31], [C_21 - C_12]]) / (4*q_c)
import numpy as np
import scipy.spatial
a_quat = np.array([0.1967, 0.5692, 0.5163, 0.6089])
print("Original quaternion", a_quat)
a_rotation = scipy.spatial.transform.Rotation.from_quat(a_quat)
a_matrix = a_rotation.as_matrix()
print("Matrix")
print(a_matrix)
def convert_dcm_to_quaternion(dcm):
"""
Convert DCM to a quaternion
"""
C_11 = dcm[0,0] #angle between vector 1 of initial frame and vector 1 of rotated frame
C_12 = dcm[0,1] #angle between vector 2 of initial frame and vector 1 of rotated frame
C_13 = dcm[0,2]
C_21 = dcm[1,0] #angle between vector 1 of initial frame and vector 2 of rotated frame
C_22 = dcm[1,1]
C_23 = dcm[1,2]
C_31 = dcm[2,0]
C_32 = dcm[2,1]
C_33 = dcm[2,2]
q_c = 1/2 * np.sqrt(C_11 + C_22 + C_33 + 1) #consider that scalar value != 0, i.e. not at a singularity. Use Markley or Shepperd methods otherwise.
q_vec = np.array([[C_32 - C_23], [C_13 - C_31], [C_21 - C_12]]) / (4*q_c)
q = np.vstack((q_vec,q_c ))
q = q.flatten()
return q
print("converting back")
print(convert_dcm_to_quaternion(a_matrix))
print()