Create DCM from q
        :param q: array q which represents a quaternion [w, x, y, z]
        :returns: 3x3 dcm array

def _q_to_dcm(self, q):
        """
        Create DCM from q
        :param q: array q which represents a quaternion [w, x, y, z]
        :returns: 3x3 dcm array
        """
        assert(len(q) == 4)
        assert(np.allclose(QuaternionBase.norm_array(q), 1))
        dcm = np.zeros([3, 3])
        a = q[0]
        b = q[1]
        c = q[2]
        d = q[3]
        a_sq = a * a
        b_sq = b * b
        c_sq = c * c
        d_sq = d * d
        dcm[0][0] = a_sq + b_sq - c_sq - d_sq
        dcm[0][1] = 2 * (b * c - a * d)
        dcm[0][2] = 2 * (a * c + b * d)
        dcm[1][0] = 2 * (b * c + a * d)
        dcm[1][1] = a_sq - b_sq + c_sq - d_sq
        dcm[1][2] = 2 * (c * d - a * b)
        dcm[2][0] = 2 * (b * d - a * c)
        dcm[2][1] = 2 * (a * b + c * d)
        dcm[2][2] = a_sq - b_sq - c_sq + d_sq
        return dcm