Model Predictive Control for cars in C++

In the 10’th project from the Self-Driving Car engineer program designed by Udacity, we implemented a Model Predictive Control to drive the car around the track. This time however we’re not given the crosstrack error, we had to calculate that ourself. Additionally, there’s a 100 millisecond latency between actuations commands on top of the connection latency.

This post builds up starting with very simple P, PD, and PID implementations, to a complex Model Predictive Control:

  • P, PD, and PID controllers implemented in Python
  • Model Predictive Control implemented in C++

P, PD, and PID controllers implemented in Python

A proportional–integral–derivative controller (PID controller or three term controller) is a control loop feedback mechanism widely used in industrial control systems and a variety of other applications requiring continuously modulated control. A PID controller continuously calculates an error value e ( t ) as the difference between a desired setpoint (SP) and a measured process variable (PV) and applies a correction based on proportional, integral, and derivative terms (denoted P, I, and D respectively) which give their name to the controller. In practical terms it automatically applies accurate and responsive correction to a control function.

P-Controller implementation

PD-Controller implementation

The PD controller is very similar to the P controller. Was added the prev_cte variable which is assigned to the previous CTE and diff_cte, the difference between the current CTE and previous CTE. We then put it all together with the new tau_d parameter to calculate the new steering value, -tau_p * cte - tau_d * diff_cte.

PID-Controller implementation

With the integral term we’re keeping track of all the previous CTEs, initially we set int_cte to 0 and then add the current cte term to the count int_cte += cte. Finally we update the steering value, -tau_p * cte - tau_d * diff_cte - tau_i * int_cte with the new tau_i parameter.

This may not seem all that impressive. PID seems to do worse than the PD controller. The purpose of the I term is to compensate for biases, and the current robot has no bias.


Model Predictive Control implemented in C++

Model Predictive Control reframes the task of following a trajectory as an optimization problem. The solution to the optimization problem is the optimal trajectory. Model Predictive Control involves simulating different actuators inputs, predicting the resulting trajectory and selecting that trajectory with a minimum cost. The current state and the reference trajectory are known. At each step in time the actuator inputs are optimized in order to minimize the cost of the predicted trajectory. Once the lowest cost trajectory is found the first set of actuator commands are executed and the rest are thrown away, and the repeats at each time step when a new optimal trajectory is calculated.

A good start to the cost function is to think of the error that we would like to minimize. For example, measuring the offset from the center of the lane, where the center of the lane can be called the reference, or desired, state. As well, if the goal is to move between 2 locations we would like to penalize slow or stopped cars. Additionally we want the car to change the lanes as smooth as possible.

Model Predictive Control uses an optimizer to find the control inputs and minimize the cost function. We actually execute the very first set of control inputs which brings the vehicle to a new state and then the process is repeated.

In the image below by executing the first step (first control inputs) will move the car to the next yellow dot. After that a completely new trajectory is generated inclusive new dots.


First, we set up everything required for the Model Predictive Control loop. This consists of defining the duration of the trajectory, T, by choosing N and dt. Next, we define the vehicle model and constraints such as actual limitations. Finally, we define the cost function.


With the model setup complete, we begin to state feedback loop. First, we pass the current state to the Model Predictive Controller.

Next, the optimization solver is called. The solver uses the initial state, the model constraints and the cost function to return a vector of control inputs that minimize the cost function.

The solver used is IPOPT.


We first apply the first control input to the vehicle and repeat the loop.

In a real car, an actuation command won’t execute instantly – there will be a delay as the command propagates through the system. A realistic delay might be on the order of 100 milliseconds.

This is a problem called “latency”, and it’s a difficult challenge for some controllers – like a PID controller – to overcome. But a Model Predictive Controller can adapt quite well because we can model this latency in the system.

PID controllers will calculate the error with respect to the present state, but the actuation will be performed when the vehicle is in a future (and likely different) state. This can sometimes lead to instability.

The PID controller could try to compute a control input based on a future error, but without a vehicle model it’s unlikely this will be accurate.

A contributing factor to latency is actuator dynamics. For example the time elapsed between when you command a steering angle to when that angle is actually achieved. This could easily be modeled by a simple dynamic system and incorporated into the vehicle model. One approach would be running a simulation using the vehicle model starting from the current state for the duration of the latency. The resulting state from the simulation is the new initial state for MPC.

Thus, MPC can deal with latency much more effectively, by explicitly taking it into account, than a PID controller.

The full source code can be found here.

Video result



Leave a Reply

Your email address will not be published.