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

def _euler_to_dcm(self, euler):
        """
        Create DCM from euler angles
        :param euler: array [roll, pitch, yaw] in rad
        :returns: 3x3 dcm array
        """
        assert(len(euler) == 3)
        phi = euler[0]
        theta = euler[1]
        psi = euler[2]
        dcm = np.zeros([3, 3])
        c_phi = np.cos(phi)
        s_phi = np.sin(phi)
        c_theta = np.cos(theta)
        s_theta = np.sin(theta)
        c_psi = np.cos(psi)
        s_psi = np.sin(psi)

        dcm[0][0] = c_theta * c_psi
        dcm[0][1] = -c_phi * s_psi + s_phi * s_theta * c_psi
        dcm[0][2] = s_phi * s_psi + c_phi * s_theta * c_psi

        dcm[1][0] = c_theta * s_psi
        dcm[1][1] = c_phi * c_psi + s_phi * s_theta * s_psi
        dcm[1][2] = -s_phi * c_psi + c_phi * s_theta * s_psi

        dcm[2][0] = -s_theta
        dcm[2][1] = s_phi * c_theta
        dcm[2][2] = c_phi * c_theta
        return dcm