Create DCM from euler angles
        :param dcm: 3x3 dcm array
        :returns: array [roll, pitch, yaw] in rad

def _dcm_to_euler(self, dcm):
        """
        Create DCM from euler angles
        :param dcm: 3x3 dcm array
        :returns: array [roll, pitch, yaw] in rad
        """
        assert(dcm.shape == (3, 3))
        theta = np.arcsin(min(1, max(-1, -dcm[2][0])))

        if abs(theta - np.pi/2) < 1.0e-3:
            phi = 0.0
            psi = (np.arctan2(dcm[1][2] - dcm[0][1],
                              dcm[0][2] + dcm[1][1]) + phi)
        elif abs(theta + np.pi/2) < 1.0e-3:
            phi = 0.0
            psi = np.arctan2(dcm[1][2] - dcm[0][1],
                             dcm[0][2] + dcm[1][1] - phi)
        else:
            phi = np.arctan2(dcm[2][1], dcm[2][2])
            psi = np.arctan2(dcm[1][0], dcm[0][0])

        return np.array([phi, theta, psi])