ahrs
ahrs copied to clipboard
Weird behavior in to_DCM and from_DCM in Quaternion class
First of all, Thanks for making this package available!
While using the Quaternion class I experienced weird behavior to go to / from DCM.
For example:
-
if your satellite is at lat=0, lon=0 and velocity is [0,0,1] (Going north). Then the DCM in ECEF is defined as dcm_ecef2platform = np.array([x_platform, y_platform, z_platform]) with y_platform = velocity = [0,0,1] z_platform = pointing = [-1, 0, 0] x_platform = cross(y_platform, z_platform) = [0, -1, 0] so DCM = array([[ 0., -1., 0.], [ 0., 0., 1.], [-1., 0., 0.]]) If I use the method from_DCM I have different quaternions according to the method parameter I choose. The answer could also be obtained with the roll pitch yaw angles: Quaternion().from_rpy(np.array([-90, 0, 90]) * DEG2RAD) which gives the correct quaternion: Quaternion([0.5, 0.5, -0.5, -0.5]).
-
The method to_DCM gives unexpected output some times. for Quaternion([ 0.5 0.5 -0.5 -0.5]) the to_DCM gives [[ 0. 0. -1.] [-1. 0. 0.] [ 0. 1. 0.]] instead of [[ 0. -1. 0.] [ 0. 0. 1.] [-1. 0. 0.]]
Maybe I am missing something, I would be more than happy to help there.
Maybe from_DCM could be :
@staticmethod
def from_DCM(mat):
""" Returns the quaternion corresponding to the unitary DCM matrix. """
mat = np.array(mat)
tr = np.trace(mat)
d = 1 + 2 * mat.diagonal() - tr
qsquare = 1 / 4 * np.array([1 + tr, d[0], d[1], d[2]])
qsquare = qsquare.clip(0, None) # avoid numerical errors
# compute signs matrix
signs = np.sign([mat[1, 2] - mat[2, 1], mat[2, 0] - mat[0, 2], mat[0, 1] - mat[1, 0],
mat[0, 1] + mat[1, 0], mat[2, 0] + mat[0, 2], mat[1, 2] + mat[2, 1]])
signs_m = np.zeros((4, 4))
signs_m[np.triu_indices(4, 1)] = signs
signs_m += signs_m.T
signs_m[np.diag_indices(4)] = 1.
# choose appropriate signs
max_idx = qsquare.argmax()
coords = np.sqrt(qsquare) * signs_m[max_idx]
return Quaternion(*coords)
I have the impression to_DCM is giving the Transpose of what DCM should be.