text: import numpy as np
  from sim.sim2d import sim_run
  
  # Simulator options.
  options = {}
  options['FIG_SIZE'] = [8, 8]
  options['OBSTACLES'] = False
  options['FULL_RECALCULATE'] = True
  
  class ModelPredictiveControl:
      def __init__(self):
          self.horizon = 15
          self.dt = 0.2
          # we are predicting dt*horizon seconds ahead
  
          # Reference or set point the controller will achieve.
          self.reference1 = [11, 11, 0]
          self.reference2 = [11, 1, 3*3.14/2]
  
      def plant_model(self, prev_state, dt, pedal, steering):
          x_t = prev_state[0]
          y_t = prev_state[1]
          psi_t = prev_state[2]
          v_t = prev_state[3]
  
          a_t = pedal
          steer_angle = steering
  
          x_t_1 = x_t + v_t*np.cos(psi_t)*dt
          y_t_1 = y_t + v_t*np.sin(psi_t)*dt
          v_t_1 = v_t + a_t*dt - v_t/25.0
          psi_t_1 = psi_t + v_t*dt*(np.tan(steer_angle)/2.5)  # length of car is 2.5
  
          return [x_t_1, y_t_1, psi_t_1, v_t_1]
  
      def cost_function(self, u, *args):
          state = args[0]
          ref = args[1]
          cost = 0.0
  
          for i in range(self.horizon):
              v_start = state[3]
  
              state = self.plant_model(state, self.dt, u[i*2], u[i*2+1])
              # u input vector is designed like = [(pedal for t1), (steering for t1), (pedal for t2), (steering for t2)...... (pedal for t(horizon)), (steering for t(horizon))]
  
              cost += ((state[0] - ref[0])**2)*1.5
              cost += ((state[1] - ref[1])**2)*1.5
              # position cost
  
              cost += ((state[2] - ref[2])**2)
              # angle cost
  
              # cost += ((state[3] - v_start)**2)
              # accelaration cost - cost for changing accelaration quickly so passengers don't get motion sickness
  
              # cost += (u[i*2+1]**2)*self.dt
              # steering input cost - cost for moving the steering wheel
  
          return cost
  
  sim_run(options, ModelPredictiveControl)
  