Main library for implementation of Model Predictive Control system.
mpc_lib
├── include
│ ├── cppad
│ ├── Eigen-3.3
│ ├── Ipopt-3.12.7
│ └── mpc_lib
│ └── MPC.h
└── src
└── MPC.cppcppad
A C++ Algorithmic Differentiation package
Eigen-3.3
Eigen is a high-level C++ library of template headers for linear algebra, matrix and vector operations, geometrical transformations, numerical solvers and related algorithms.
Ipopt-3.12.7
IPOPT, short for "Interior Point OPTimizer, pronounced I-P-Opt", is a software library for large scale nonlinear optimization of continuous systems.
- Install cppad
sudo apt-get install cppad- Install IPOPT
Warnings maybe ignored.
sudo apt-get install gfortran
sudo apt-get install unzip
wget https://www.coin-or.org/download/source/Ipopt/Ipopt-3.12.7.zip && unzip Ipopt-3.12.7.zip && rm Ipopt-3.12.7.zip
sudo bash install_ipopt.sh ./Ipopt-3.12.7/
Setup :
1. Define length of prediction horizon, N, and duration of each timestep, dt.
2. Define vehicle dynamics and actuator limitations along with other constraints.
3. Define cost function
Loop :
1. Pass current state as initial state to model predictive controller.
2. Call the optimization solver (we used Ipopt). It will return a vector of control inputs that minimizes the cost function.
3. Apply first control input to vehicle.
4. Back to 1
Dynamics
Constraints
Cost function
Preprocessing of setpoints is done in the main controller node
The setpoints are passed to the controller with respect to an arbitrary global coordinate system by the path planning module. This is then transformed into the vehicle's local coordinate system.
for (int i = 0; i < ptsx.size(); i++)
{
// translation
double shift_x = ptsx[i] - px;
double shift_y = ptsy[i] - py;
// rotation
ptsx[i] = shift_x * cos(-theta) - shift_y * sin(-theta);
ptsy[i] = shift_x * sin(-theta) + shift_y * cos(-theta);
}This is then used to get a 3rd order polynomial as an estimate of the current road curve ahead. Using a smaller order polynomial runs the risk of underfitting and likewise using a higher order polynomial would be prone to overfitting or an inefficient unnecessary added complexity.
Function used to get fitted curve - GitHub
The current Cross track error and Orientation error is calculated from the polynomial coefficients as follows:
double cte = coeffs[0];
double etheta = -atan(coeffs[1]);
Starter code : Udacity Model Predictive Control (MPC) Project