def _dcm_to_q(self, dcm):
        """
        Create q from dcm
        Reference:
            - Shoemake, Quaternions,
            http://www.cs.ucr.edu/~vbz/resources/quatut.pdf

        :param dcm: 3x3 dcm array
        returns: quaternion array
        """
        assert(dcm.shape == (3, 3))
        q = np.zeros(4)

        tr = np.trace(dcm)
        if tr > 0:
            s = np.sqrt(tr + 1.0)
            q[0] = s * 0.5
            s = 0.5 / s
            q[1] = (dcm[2][1] - dcm[1][2]) * s
            q[2] = (dcm[0][2] - dcm[2][0]) * s
            q[3] = (dcm[1][0] - dcm[0][1]) * s
        else:
            dcm_i = np.argmax(np.diag(dcm))
            dcm_j = (dcm_i + 1) % 3
            dcm_k = (dcm_i + 2) % 3

            s = np.sqrt((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] -
                         dcm[dcm_k][dcm_k]) + 1.0)
            q[dcm_i + 1] = s * 0.5
            s = 0.5 / s
            q[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s
            q[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s
            q[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s

        return q
Create q from dcm
        Reference:
            - Shoemake, Quaternions,
            http://www.cs.ucr.edu/~vbz/resources/quatut.pdf

        :param dcm: 3x3 dcm array
        returns: quaternion array