ahrs icon indicating copy to clipboard operation
ahrs copied to clipboard

Weird behavior in to_DCM and from_DCM in Quaternion class

Open Floabsolut opened this issue 5 months ago • 2 comments

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:

  1. 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]).

  2. 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.

Floabsolut avatar Jan 19 '24 20:01 Floabsolut

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)

Floabsolut avatar Jan 19 '24 20:01 Floabsolut

I have the impression to_DCM is giving the Transpose of what DCM should be.

Floabsolut avatar Jan 19 '24 23:01 Floabsolut