Colimatror

Model Predictive Control in C++

The following article describes a simple control system approach where the Model Predictive Controller is applied. The article discusses the basic mechanism for this type of control which is willingly applicable in various engineering domains.

The MPC involves the prediction of the system behavior in the future (model described by a set of equations). The desired position of the physical model is achieved during the optimization process (cost function).

You can imagine that MPC having the dynamic model of the system (matrix A in linear time-invariant (LTI) system) simulates or predicts the model's position or behavior in the future. The controller then computes a sequence of control inputs that minimizes a predefined cost function, taking into account both the desired objectives and constraints.

For the article's purposes, I prepared simple simulations in C++. The intention was to display principles that can easily be replicated for more advanced model dynamics.

The source code you can find on my GitHub.

Standard Model Predictive Control (MPC) formulation, which I applied exactly in C++ can be defined as follows,

Consider a discrete-time linear system with the following state-space representation:

where,

• xk​ is the state vector at time step k,
• uk​ is the control input vector at time step k,
• yk​ is the output vector at time step k,
• A is the state transition matrix (model dynamics),
• B is the control input matrix,
• C is the output matrix.

The objective of Model Predictive Control (MPC) is to minimize a cost function over a finite prediction horizon N while satisfying system constraints. The cost function typically includes a quadratic term penalizing deviations from a reference trajectory and control inputs:

where,

• N is the prediction horizon,
• Q is the output weight matrix,
• R is the control input weight matrix,
• rk+i​ is the reference trajectory at time step k+i.

The MPC problem is formulated as an optimization problem,

​

Here, the control input u and state x are under lower and upper bounds.

This formulation outlines the mathematical structure of standard Model Predictive Control for a linear discrete-time system, which I applied in C++ simulations.
In practice (check the source code), we solve this optimization problem at each time step to compute the optimal control inputs over the prediction horizon, and then apply the first control input to the system.

MPC overview (by author)

We can break down the source and display crucial features of the algorithm,

1. Hessian Matrix (H): Generally, the Hessian matrix is a mathematical concept related to second-order derivatives that provides information about the curvature of the multivariate function.
   In the case of optimization, we use Hassian to analyze behaviors of the objective function near the critical point (minimum or maximum). In our particular case, the Hessian encodes the quadratic penalties on both the state and control input deviations.
2. Cost Vector (F): The vector is a combination of the predicted state deviations from the desired reference state and zero values for the control input deviations. This vector represents the linear terms of the cost function and guides the optimization process toward minimizing the cost.
3. Optimization Process: The optimization process aims to find the change in control input that minimizes the cost function. In the context of the quadratic optimization problem, this is achieved by solving a set of linear equations that arise from the optimality conditions of the optimization problem.
   We solve the linear equations formed by the optimality conditions. This operation computes the optimal change in control input that should be applied to the current control input in order to move the system states closer to the desired reference while minimizing the cost.

Notes for C++ implementation

Please make sure to review my C++ implementation carefully and take into consideration this line,

Eigen::VectorXd control_input_delta = H.colPivHouseholderQr().solve(-f);

This line of code is solving a linear equation of the form Hx = −f, where H is a matrix and f is a vector. The operation is performed using QR decomposition with column pivoting, a numerical method for solving linear systems. Here’s a step-by-step breakdown:

1. Matrix H and Vector f:

• H is a square matrix representing the system of linear equations.
• f is a vector, and the operation involves its negation (-f), which is used as the right-hand side of the linear equation.

2. Column Pivoted QR Decomposition:

colPivHouseholderQr() computes the QR decomposition of matrix H with column pivoting.
In QR decomposition, a matrix H is factorized into two matrices Q and R, where Q is an orthogonal matrix (meaning

where I is the identity matrix) and R is an upper triangular matrix.

• Column pivoting is a technique used to improve the numerical stability of the decomposition, especially for matrices that are close to singular or ill-conditioned.

3. Solving the Linear System:

• The operation .solve(-f) is used to solve the linear system Hx = −f using the QR decomposition of H.
• Mathematically, the solution x can be found by:

• Since Q is orthogonal,

simplifies the computation.

• The solution x is assigned to the variable control_input_delta, which is of type Eigen::VectorXd.

4. Mathematical Representation:

• The entire operation can be represented mathematically as solving for x in the equation Hx = −f using QR decomposition with column pivoting:

• Or more explicitly:

5. Q Matrix

1. Definition: The Q matrix is a square, symmetric, and typically positive semi-definite matrix used in the cost function of MPC to weigh the importance of tracking the reference state or setpoint.
2. Role in Cost Function: In the objective function of MPC, the Q matrix multiplies the state error (the difference between the predicted state and the desired state).
   This part of the cost function is often written as (x−x_ref​)TQ(x−x_ref​), where x is the predicted state, and x_ref​ is the desired state or reference.
3. Purpose: The Q matrix aims to penalize deviations from the desired state. By adjusting the weights in Q, you can specify which aspects of the state vector are more critical to regulate. For example, higher weights in Q for certain states mean that the controller will prioritize minimizing errors in those states.

6. R Matrix

1. Definition: The R matrix, like Q, is a square, symmetric, and usually positive definite matrix. It is used in the MPC cost function to weigh the control effort or control input.
2. Role in Cost Function: The R matrix is part of the objective function term involving the control input. This is typically written as uTRu, where u is the control input vector.
3. Purpose: The R matrix’s role is to penalize excessive control action. A higher weight in R implies a greater penalty for large control inputs, encouraging the controller to use smaller inputs and thus potentially leading to smoother control actions. This can be important for preventing excessive sets on actuators (motor drives) or for systems where large control inputs are undesirable or unsafe.

Balancing Q and R

The relative values in Q and R matrices define a trade-off between the importance of tracking the reference trajectory (state accuracy) and minimizing control effort. This trade-off is central to the design of an MPC controller:

• High Q, Low R: Prioritizes state tracking accuracy over control effort, leading to aggressive control actions.
• Low Q, High R: Prioritizes minimizing control effort, possibly at the expense of tracking performance.

Below, I provide the output from the simulations you can run on your machine.
The first simulation below provides information on how the MPC performs to approach the desired constant position (value).

MPC performance. Simulation 1

The second simulation you can run provides the MPC performance output while the desired trajectory has the shape of a sinus function. The correct choice of cost function (matrix value Q, R) affects the states and how the system stabilizes over time.

MPC performance. Simulation 2

Thank you for reading.