# Model Predictive Control for driving a Self-Driving Car

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.

In the kinematic model, the front wheels of the SDC are lumped into a unique wheel located at the center of the front axle. The state of the system is defined in terms of four state variables: $x$ (x Position), $y$ (y Position), $v$ (Velocity) and $\psi$(Orientation). Further, the system is considered to have two control inputs (actuators): $a$ (acceleration: which is controlled by throttle/braking) and $\alpha$ (steering angle).  The kinematic model can then be written as:
$\dot{x} = v \cos(\phi)$
$\dot{y} = v \sin(\phi)$
$\dot{v} = a$
$\dot{\phi} = \frac{v}{L_f}\alpha$
Here $L_f$ is the distance between SDC’s center of mass and its front wheels. The SDC is considered to have to outputs $CTE$ (Cross Track Error) and $e\psi$ (Orientation Error).
The update equation for the model can be formulated as:
$x_{t+1} = x_{t} + v_t \cos(\phi_t) dt$
$y_{t+1} = y_{t} + v_t \sin(\phi_t) dt$
$v_{t+1} = v_{t} + a_t dt$
$\phi_{t+1} = \phi_{t} + \frac{v_t}{L_f} \alpha_t dt$
$CTE_{t+1} =f(x_t) - y_t + v_t \sin(e\psi_t) dt$
$e\psi_{t+1} = \psi_{t} - \psi^{des}_{t} + \frac{v_t}{L_f} \alpha_t dt$
$\psi^{des}$ is the desired orientation angle.

### 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 ($N$) and duration of each time-step duration ($dt$). Then we use that to find the cost of certain predicted trajectory.  The cost ($C$) of a certain trajectory can be formulated as:

$C_{ref} = \sum_{t=1}^{N}(K_1(e\psi_{t})^2 + K_2(cte_{t})^2 + K_3(v_t - v^{ref}_t)^2)$
$C_{act} = \sum_{t=1}^{N-1}(K_4\alpha^2_t + K_5a^2_t)$
$C_{\Delta act} = \sum_{t=1}^{N-2}(K_6(\alpha_{t+1} - \alpha_{t})^2 + K_7(a_{t+1} - a_{t})^2)$
$C = C_{ref} + C_{act} + C_{\Delta act}$

Here $C_{ref}$ is the cost of deviation from the reference trajectory. $C_{act}$ is the cost of using actuators (Control Signals) and $C_{\Delta act}$ is the value gap between sequential actuation. $K_1$$K_7$ 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 $N$ 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 ($N$) was taken as $10$. For the larger value of $N$, the Ipopt solver was overloaded with the larger number of computations causing inaccuracies due to degradation in real-time performance. The time-step duration ($dt$) was taken as $0.1$ 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.

Check my Github page for complete implementation.