In the previous project, I discussed how we can implement a PID controller for a self-driving car (SDC). Although the PID controller was able to move the SDC around the track, due to its purely reactive nature it resulted in a lot of oscillations during the drive. In this project, the Model Predictive control solves this problem by estimating the future states of the SDC. But to implement the control first we need to derive a state model of the SDC.

**Kinematic Model of a Self-Driving Car**

Similar to any other system, we can design a variety of state-space models for capturing the behavioral dynamics of an SDC. One such model called kinematic model provides a mathematical description of the vehicle motion without considering the forces that affect the motion. The equations of motion are based purely on geometric relationships governing the system.

### Model Predictive Control: Cost Formulation and Implementation

Given a reference trajectory (found out by path planning), the Model Predictive Control (MPC) predicts the optimal actuator (control) inputs to minimize the difference between reference and actual trajectory. The performance of the MPC depends on how we formulate the cost function to be minimized. First step is to define number of time steps of the trajectory () and duration of each time-step duration (). Then we use that to find the cost of certain predicted trajectory. The cost () of a certain trajectory can be formulated as:

Here is the cost of deviation from the reference trajectory. is the cost of using actuators (Control Signals) and is the value gap between sequential actuation. – are cost coefficients or weights for different types of cost. The following code shows the implementation of the cost function in C++.

fg[0] = 0; // Cost size_t i; // Cost based on error due to deviation from reference for( i = 0; i < N; i++ ) { fg[0] += CppAD::pow(vars[cte_start + i], 2); // Cost of Cross Track Error fg[0] += CppAD::pow(vars[epsi_start + i], 2); // Cost of Orientation Error fg[0] += 0.001*CppAD::pow(vars[v_start + i] - v_ref, 2); // Cost of deviation from Reference Velocity (v_ref) } // Cost of using actuators for (i = 0; i< N - 1 ; i++) { fg[0] += 0.05*CppAD::pow(vars[delta_start + i], 2); // Cost of Steering fg[0] += 0.05*CppAD::pow(vars[a_start + i], 2); // Cost of Acceleration (Throttle) } // Cost of value gap between Sequential Actuation (to smooth the actuation) for (i = 0; i < N - 2; i++) { fg[0] += 250*CppAD::pow(vars[delta_start + i + 1] - vars[delta_start + i], 2); // Steering fg[0] += 5*CppAD::pow(vars[a_start + i + 1] - vars[a_start + i], 2); // Acceleration (Throttle) }

For every time step, we try to find the optimal trajectory for next steps such that the optimized trajectory closely matched the reference trajectory. Optimal trajectory is found by minimizing the cost function using Ipopt solver. The number of time steps () was taken as . For the larger value of , the Ipopt solver was overloaded with the larger number of computations causing inaccuracies due to degradation in real-time performance. The time-step duration () was taken as seconds. Taking a smaller value resulted in increased oscillations and larger value resulted in inaccuracies around the curve causing the SDC to go off the road.

**Results**

Clearly, the SDC is able to drive through the track closely following the reference trajectory and unlike the PID controller, the Model Predictive Control is able to steer the SDC along a smoother path.

## One comment