# Details of the MPC for longitudinal control without a neural network

This post will introduce the details of the regular (without neural nets) MPCs which plan the longitudinal movement of a car.

We have three key functions for a controller:

- the setup_mpc() function initiates the mpc with the control gains we specify.
() function, which usually assigns the current vEgo and aEgo*set_cur_state*() function: after updating the current state, we call the mpc solver and get the solution.*update*

Now we look into the details of the MPC update step.

First, it sets the current MPC state, where we update the lead vehicle state.

# Setup current mpc state

self.cur_state[0].x_ego = 0.0if lead is not None and lead.status:

x_lead= lead.dRel

v_lead= max(0.0, lead.vLead)

a_lead= lead.aLeadK if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):

v_lead = 0.0

a_lead = 0.0 self.a_lead_tau= lead.aLeadTau ## lead accel time constant

self.new_lead = False

if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:

self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)

self.new_lead = True self.prev_lead_status = True

self.prev_lead_x = x_lead

self.cur_state[0].x_l = x_lead

self.cur_state[0].v_l = v_lead

else:

self.prev_lead_status = False # no leader

# Fake a fast lead car, so mpc keeps running

self.cur_state[0].x_l = 50.0

self.cur_state[0].v_l = v_ego + 10.0

a_lead = 0.0

self.a_lead_tau = _LEAD_ACCEL_TAU

Note that a_lead_tau is the lead vehicle acceleration time, it will be used for the mpc controller to predict the movement of the lead vehicle.

Calculate the mpc is quite simple, which is done by C++ code

`# Calculate mpc`

t = sec_since_boot()

self.n_its = self.libmpc.*run_mpc*(self.cur_state, self.mpc_solution, self.**a_lead_tau**, **a_lead**)

self.duration = int((sec_since_boot() - t) * 1e9)

** run_mpc** calculates the lead_vehicle predictions, like setting up the boundary constraint for the optimization problem, then uses the cost function to find the solution.

We can see, a_lead_tau is referred to as l in the following function, and used for estimating the acceleration of the lead vehicle by:

`a_l = a_l_0*exp(-l*t*t/2)`

Here the online data (acadoVariables.od) is the predicted lead vehicle speed and distance. As a comparison, for the mpc_model, the online data is the plan output by the neural network.

Get the solution from mpc:

`# Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed`

self.v_mpc = self.mpc_solution[0].v_ego[1]

self.a_mpc = self.mpc_solution[0].a_ego[1]

self.v_mpc_future = self.mpc_solution[0].v_ego[10]

The above code shows how to call the mpc solver, but how does the mpc work?

Quote from acado documentation, to set-up of a MPC in acado, it comprises six main steps:

Introducing all variables and constants.

Setting up the car ODE model.

Setting up a least-squares objective function by defining the five components of the

measurement functionandhan appropriate weighting matrix.Defining a complete optimal control problem (OCP) comprising the

dynamic model, the objective function as well as constraintson the input.Setting up a RealTimeAlgorithm defined by the OCP to be solved at each sampling instant together with a sampling time specifying the time lag between two sampling instants. Moreover, several options can be set and plot windows flushed.

Setting up a Controller by specifying a control law, i.e. the real-time algorithm solving our OCP in this case, and a reference trajectory to be tracked. In this example, the reference trajectory is read from a file where the value of all components are defined over time. (Note that the reference trajectory can be left away when calling the Controller constructor which is equivalent to all entries zero over the whole simulation horizon.)

The optimization problem for the mpc controller is probably defined in the *generator.cpp** *file*. *Note that here we can clearly see how the cost function is defined, including the desired distance our vehicle want to keep.

First, the mpc needs the dynamics of the system, which is just the ODE:

`// Equations of motion`

f << dot(**x_ego**) == v_ego;

f << dot(**v_ego**) == a_ego;

f << dot(**a_ego**) == j_ego;

Then, it defines the cost function using the following equations and some weights:

// Running cost

Function h;

h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) — 1; //66, ttc

h << (d_l — desired) / (0.05 * v_ego + 0.5); //66, distance

h << a_ego * (0.1 * v_ego + 1.0);

h << j_ego * (0.1 * v_ego + 1.0);// Weights are defined in mpc.

BMatrix Q(4,4); Q.setAll(true);

In file ** selfdrive.controls.lib.drive_helpers**, the MPC_COST_LONG are predefined as:

**TTC = 5.0 # time to collision**

DISTANCE = 0.1 # desired distance

ACCELERATION = 10.0 # acceleration

JERK = 20.0 # jerk

From the perspective of a CF model, we are especially interested in the desired distance to keep. Here is the detail of the** desired distance:**

#define G 9.81

#define TR 1.8#define RW(v_ego, v_l)(v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G))desired_distance =

3.6*v_ego - 1.8*v_l + v_ego*v_ego/(2*9.81) - v_l*v_l/(2*9.81)

Finally, the mpc is solved by minimizing the LSQ.