Mayitzin / ahrs

Attitude and Heading Reference Systems in Python

Home Page:https://ahrs.readthedocs.io/

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

Weird behavior in to_DCM and from_DCM in Quaternion class

Floabsolut opened this issue · 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.

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.