An example MPC in C++

A full implementation of MPC is beyond the scope of this chapter but you can review this example implementation written in C++ at https://github.com/Krishtof-Korda/CarND-MPC-Project-Submission/blob/master/src/MPC.cpp.

The following example will walk you through the implementation of an MPC module that you can use in place of a PID controller for both lateral and longitudinal control. Recall that MPC is a MIMO system, meaning you can control multiple outputs.

The following example shows all the basic components and code you'll need to build an MPC controller:

1. First, use the following code to fit a polynomial to your prediction horizon waypoints:

   Main.cpp --> polyfit()

   Use the following code to calculate cross-tracking errors:

   Main.cpp --> polyeval()
   double cte = polyeval(coeffs, px) - py;

   Use the following code to calculate orientation errors:

   double epsi = psi - atan(coeffs[1] + 2*coeffs[2]*px + 3*coeffs[3]*px*px) ;

2. Now, we use MPC.cpp...