code: def _euler_to_dcm(self, euler):
          
          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
docs: Create DCM from euler angles
          :param euler: array [roll, pitch, yaw] in rad
          :returns: 3x3 dcm array