MPC implementation in C++

I was really curious about how fast can we calculate the control signal using MPC. Because of this, first I wanted to write a short C code, but finally I end up with C++. because it is easy to overload the operations and more flexible and easier to implement the Matrix operations. Also there are disadvantages just to mention that the std::vector is slower than the C array and C++ can increase the size of the compiled code significantly.

I have to mention that this is my first code in C++, but I already have experience with OOP(Matlab, Python, Java, PHP) and at the University I got the basics of the C. 

For the first test I used the Eigen library which is really straight forward, but where is the challenge if I'm using a libraries instead of writing my own code and learning new OP language. With the Eigen I could achieve around 6e-5s which is really good, but this package is optimized for big matrices, take a look on the benchmark. In my case, the biggest matrix is a 4x4 matrix, which is really small.

In the second test I wrote my own library(you can fork from this on the github), I'm using arrays instead of std::vectors, because it's faster as I mentioned before.
The result of the test:

Runtime

My next step was to test on my Raspberry Pi using ChibiOS/RT(ARM  CPU). In this platform to calculate one control signal took around 2ms which is not really good. In the mean time I also install Arch Linux on the RPI where the runtime was 0.4ms which is a bit better, so I will try to compile the Linux kernel with real time capability.

Finally this is the test code

int main(int argc, char* argv[]) {

 /*
  * Defining the variables
  */
 //Control horizon
 int nc = 4;
 // Prediction horizon
 int np = 4;
 //Control signal 
 Matrix u(1, 1);
 // Sampling time
 double Ts = 5e-5;

 MPC mySysMat(2, 1, 1);
 //Error matrix, just for testing; 

 Matrix DeltaU(np, 1);

 Matrix R = 1e-5
   * Matrix::Identity(mySysMat.C.rows() * nc,
     mySysMat.C.rows() * nc);
 Matrix Q = 1e4 * Matrix::Identity(np, np);

 Matrix x0(2, 1);
 //Reference signal
 Matrix y_ref(100000, 1);
 y_ref.rblock(0, 0, 50000, 1) = 100 * Matrix::Ones(50000, 1);
 y_ref.rblock(50000, 0, 100000, 1) = -100 * Matrix::Ones(50000, 1);

 double start = now();

 //DC motor parameters
 double Rm = 0.35;
 double Km = 0.0296;
 double Ke = 0.0296;
 double b = 6.7 * 10e-5;
 double J = 2.9 * 10e-6;
 double Fc = 0.02;
 double L = 25 * 10e-6;

 /*
  Parsing the input parameters
  */
 for (int i = 1; i < argc; i++) {
  if (i + 1 != argc) // Check that we haven't finished parsing already
   if (!strcmp(argv[i], "-s")) {
    Ts = atof(argv[i + 1]);
   }
  if (!strcmp(argv[i], "-nc")) {
   nc = atoi(argv[i + 1]);
  }
  if (!strcmp(argv[i], "-np")) {
   np = atoi(argv[i + 1]);
  }

  if (!strcmp(argv[i], "-h")) {
#ifdef CMD
   std::cout << "Coming soon..." << std::endl;
#endif

  }
 }

 //Initializing the system matrices
 mySysMat.Fi << 1 - Ts * (Rm / L), -Ts * (Ke / L), Ts * (Km / J), 1
   - Ts * (b / J);

 mySysMat.Ga << Ts * 1 / L, 0;

 mySysMat.C << 0, 1;

 mySysMat.calcMPCFi(np);
 mySysMat.calcMPCGa(np);
 mySysMat.calcMPCGy(np, nc);
#ifdef DEBUG
 std::cout << "Calculation Fi,Ga,Gy took : " << (double) (now() - start)
   << std::endl;
#endif

 Matrix calcT(1, y_ref.rows());
 Matrix u_hist(1, y_ref.rows());
 Matrix w_hist(1, y_ref.rows());

 for (int i = 0; i < y_ref.rows() - np; i++) {

  start = now();
  //Calculating the error

  DeltaU = mySysMat.calcContSig(
    mySysMat.calcError(y_ref.block(i, 0, i + np, 1), x0, u), Q, R);

  u(0, 0) += DeltaU(0, 0);

  //We are not including this in the time measurement
  if (abs(u(0, 0)) > 5)
   u(0, 0) = sgn(u(0, 0)) * 5;

  u_hist(0, i) = u(0, 0);

  w_hist(0, i) = x0(1, 0);
  //Simulating the system
  x0 = mySysMat.Fi * x0 + mySysMat.Ga * u;

  //Storing the calculation time
  calcT(0, i) = (double) (now() - start);

 }
#ifdef DEBUG
 std::cout << "Minimum:" << calcT.min() << std::endl;
 std::cout << "Maximum:" << calcT.max() << std::endl;
#endif

#ifdef PLOT
 Gnuplot g3("lines");
 plot_x(y_ref, Ts, &g3);
 plot_x(w_hist, Ts, &g3);

 Gnuplot g2("lines");
 plot_x(u_hist, Ts, &g2);

 Gnuplot g1("lines");
 plot_x(calcT, Ts, &g1);
 std::cin.get();
#endif

Model predictive control of brushed DC motor

Motor model

The state space model of a brushed DC motor as follow:
$$A =\left[ {\begin{array}{cc} -\frac{R_{a}}{L_{a}} & -\frac{K_{e}}{L_{a}} &  0 \\ \frac{K_{m}}{J} & -\frac{b}{J} & 0 \\ 0 & 1 & 0 \end{array} } \right] B=\left[ {\begin{array}{cc} \frac{1}{L_{a}} \\ 0 \\ 0 \end{array} } \right] C=\left[ {\begin{array}{cc} 0 \\ 1 \\ 0 \end{array} } \right] D=\left[ {\begin{array}{cc} 0 \end{array} } \right]\tag{1}$$
As you can see, I assumed that the torque is zero. 
Where the states are:
$$\dot{x(t)} =\left[ {\begin{array}{cc} \frac{di}{dt} \\ \frac{d\omega}{dt} \\ \frac{d\theta}{dt} \end{array} } \right]\tag{2}$$

Controller design

I already introduced the MPC controller, please read that first before you continue to read this blog. 

The first step is to determine the Toeplitz matrix:

$$\begin{bmatrix}x_{k+1|k} \\ \vdots \\ x_{k+N_c|k} \\ x_{k+N_c+1|k} \\ \vdots \\ x_{k+N|k} \end{bmatrix}=\begin{bmatrix}\Phi \\ \vdots \\ \Phi^{N_c} \\ \Phi^{N_c+1} \\ \vdots \\ \Phi^N \end{bmatrix}x_k + \begin{bmatrix}\Gamma \\ \vdots \\ \sum_{i=0}^{N_c-1} \Phi^{i} \Gamma\\ \sum_{i=0}^{N_c}\Phi^{i} \Gamma\\ \vdots \\ \sum_{i=0}^{N}\Phi^i \Gamma \end{bmatrix}u_{k-1}\\+ \begin{bmatrix}\Gamma & \cdots & 0 \\ \vdots & \ddots & \vdots \\ \sum_{i=0}^{N_c}\Phi^{i} \Gamma & \cdots & \Phi \Gamma+\Gamma \\ \vdots & \ddots & \vdots \\ \sum_{i=0}^{N-1}\Phi^i \Gamma & \cdots & \sum_{i=0}^{N-N_c}\Phi^i \Gamma \end{bmatrix} \begin{bmatrix}\Delta u_{k+1|k} \\ \vdots \\ \Delta u_{k+N_c-1|k} \end{bmatrix}\tag{1}$$ 

I wrote the following Matlab function to build up the  $\Phi^*$, $\Gamma^*$, $G_y$:

function [FiFi,FiGam,Gy] = getFiFi_FiGa(Fi,Ga,C,N,Nc,n)
    
    FiFi=[]; %ennek a merete N*n x n kell legyen
    
    for i=1:N,    
        FiFi=[FiFi; C*Fi^i]; 
    end
    % FiGam szamitasa
    FiGam=[];
    for i=1:N,
        S=0;
        for j=0:i-1,       
            S=S+C*Fi^j*Ga;    
        end
        FiGam=[FiGam; S];
    end
    
    ng=size(FiGam,1);
    mg=size(FiGam,2);
    % Gy szamitasa
    Gy=zeros(ng,mg*Nc);
    % Gy szamitasa
    for j=1:mg:Nc*mg   % oszlopok         
        Gy(j:end, j:j+mg-1)=FiGam(1:end-j+1,:);
    end

end

To find the optimal control signal we have to solve the following optimization problem $J_k(u,x_k)=\frac{1}{2}\Delta u_k^TH\Delta u_k+f^T\Delta u_k+const$. I already presented the solution of this problem (see Model predictive control). I am going to  implement the calculation of the $\Delta u_k$ optimal control signal in the following example:

for v=1:n-N
    %Error vector
    E=Yref(v:v+N-1)-FiFi*x0-FiGam*u;
    
     deltau=inv(Gy'*Q*Gy+R)*Gy'*Q*E;
     u=u+deltau(1:2);
     for k=1:length(u)
         if(abs(u(k))>lim)
             u(k) = sign(u(k))*lim;
         end
     end
   
    x0 = Fi*x0+Ga*u;
    yt = C*x0;
    
    
    um=[um u];
    ym=[ym;yt];
end;

In the example above I also tried to constrain the control signal by choosing the $lim$ variable to 38. After choosing the values of the noise matrices as follows

• $Q=eye(N*Nro)*1e4$ where $N$ is the number of the horizon and $Nro$ is the number of the outputs.
• $R=eye(Nc*Nrin)*1e-5$ where $Nc$ is the number of the control horizon and $Nrin$ is the number of the inputs.

I got these results:

Figure 1. Rotor speed

Figure 2. Control signal

Figure 3. Error between the reference and measured rotor speed

We can reduce the error by increasing or eliminating the constrain of control signal as you can see in $Figure 4$

Figure 4. Error between the reference and measured rotor speed without limiting the control signal

The other solution  of the optimal control signal calculation in Matlab is the built in $quadprog$ function. You can find a simple example below, also you can read the description of this function on the Matlab help page:

H=2*(Gy'*Q*Gy+R);
for v=1:n-N
    %Hiba vektor    
    E=Yref(v:v+N-1)-FiFi*x0-FiGam*u;
    
    f=-2*E'*Q*Gy;        
    
    b = [abs(lim-u(1));
        abs(lim+u(1))];
    A = [1 0 0 0 0 0 0 0;
        -1 0 0 0 0 0 0 0;
        ];
     
    Aeq=[];beq=[];
    
    [deltau,FVAL,EXITFLAG]=quadprog(H,f,A,b,Aeq,beq);
    u=u+deltau(1:2);
    
   
    x0 = Fi*x0+Ga*u;
    yt = C*x0;    
    
    um=[um u0];
    ym=[ym;yt];
    Em = [Em;E];
end;

Appendix

Motor parameters:

    Rm = 0.35;
    Km=0.0296;
    Ke = 0.0296;
    b = 6.7*10e-5;
    J = 2.9*10e-6;
    Fc = 0.02;
    L = 25 *10e-6;
    Ts = 1e-4;

Model predictive control

Introduction

The Model Predictive Controller (MPC) theory was developed after 1960, being a special case of the optimal control theory. In the MPC theory the dynamic optimization problem will be rewritten as  a static optimization problem. In the MPC theory they are using the varying control horizon theorem. The static optimization problem will be solved at each time step.

The MPC has the following properties:

• possibility to constrain the control signal
• adopts the time delays in the system 
• possibility to compensate the sensors error
• simple implementations of MIMO and LTI systems

We consider the discrete LTI sytem:

$$x_{k+1}=\Phi_kx_k+\Gamma_ku_k\\y_k=Cx_k\tag{1}$$

The control objective at time $k$ to minimize the following quadratic cost function:

$$J_k(u,x_k)=\sum_{j=1}^N||e_{k+j|k}||^2_{Q_j}+||\Delta u_{k+j-1|k}||^2_{R_j}\tag{2}$$
where

$e_{k}=y_k-y^{ref}_{k}$ is the error between the reference trajectory and the measured trajectory,

$\Delta u_{k+j|k}=u_{k+j|k}-u_{k+j-1|k}$ is the change of the input signal between two steps.  

In the $k$ step we already know the $u_{k-1}$, hence we can write
$$u_{k|k}=\Delta u_{k|k}+u_{k-1}\\u_{k+1|k}=\Delta u_{k+1|k}+\Delta u_{k|k}+u_{k-1} \\ \vdots \\ u_{k+N_c-1|k}=\Delta u_{k+N_c-1|k}+\cdots+\Delta u_{k|k}+u_{k-1} \tag{3}$$
The states predictions will be as follows:
$$x_{k+1|k}=\Phi x_{k|k}+\Gamma \left(\Delta u_{k|k}+u_{k-1}\right) \\ x_{k+2|k}=\Phi^2 x_{k|k}+\left(\Phi+I\right)\Gamma \Delta u_{k|k}+\Gamma u_{k+1|k}+\left(\Phi+I\right)\Gamma u_{k-1|k}\\ \vdots \\x_{k+N_c|k}=\Phi^{N_c} x_{k|k}+\left(\Phi^{N_c-1}+\cdots+\Phi+I\right)\Gamma \Delta u_{k|k}\\+\cdots+\Gamma u_{k+N_c-1|k}+\left(\Phi^{N_c-1}+\cdots+\Phi+I\right)\Gamma u_{k-1|k}\tag{4}$$

After some algebraic operations we can write the equations from above in matrix form:

$$\begin{bmatrix}x_{k+1|k} \\ \vdots \\ x_{k+N_c|k} \\ x_{k+N_c+1|k} \\ \vdots \\ x_{k+N|k} \end{bmatrix}=\begin{bmatrix}\Phi \\ \vdots \\ \Phi^{N_c} \\ \Phi^{N_c+1} \\ \vdots \\ \Phi^N \end{bmatrix}x_k + \begin{bmatrix}\Gamma \\ \vdots \\ \sum_{i=0}^{N_c-1} \Phi^{i} \Gamma\\ \sum_{i=0}^{N_c}\Phi^{i} \Gamma\\ \vdots \\ \sum_{i=0}^{N}\Phi^i \Gamma \end{bmatrix}u_{k-1}\\+ \begin{bmatrix}\Gamma & \cdots & 0 \\ \vdots & \ddots & \vdots \\ \sum_{i=0}^{N_c}\Phi^{i} \Gamma & \cdots & \Phi \Gamma+\Gamma \\ \vdots & \ddots & \vdots \\ \sum_{i=0}^{N-1}\Phi^i \Gamma & \cdots & \sum_{i=0}^{N-N_c}\Phi^i \Gamma \end{bmatrix} \begin{bmatrix}\Delta u_{k+1|k} \\ \vdots \\ \Delta u_{k+N_c-1|k} \end{bmatrix}\tag{5}$$
$$y_{k+j|k}=C x_{k+j|k}\tag{6}$$

From the $5^{th}$ and $6^{th}$ equation we can write the following:
$$y_k=\Phi^*x_k+\Gamma^*u_{k-1}+G_y\Delta u_k\tag{7}$$
And the error is
$$e_k=y^{ref}_k-\Phi^*x_k-\Gamma^*u_{k-1}\tag{8}$$

The idea behind the MPC is to rewrite the dynamic optimisation problem as a static optimization problem:
$$J_k(u,x_k)=||y_k-y^{ref}_k||^2_{Q_j}+||\Delta u_{k}||^2_{R_j}\tag{9}$$
where
$$e_k=\begin{bmatrix}e_{k+1|k} \\ \vdots \\ e_{k+N|k} \end{bmatrix}, \Delta u_k=\begin{bmatrix}\Delta u_{k+1|k} \\ \vdots \\ \Delta u_{k+N_c-1|k} \end{bmatrix}\tag{10}$$
$$Q=\begin{bmatrix}Q_1 & 0 & \cdots & 0 \\ 0 & Q_2 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & Q_N \end{bmatrix}, R=\begin{bmatrix}R_1 & 0 & \cdots & 0 \\ 0 & R_2 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & R_{N_c-1} \end{bmatrix}\tag{11}$$ 
From the $8-9$ equations we can deduce
$$J_k(u,x_k)=||G_y\Delta u_k-E_k||^2_{Q_j}+||\Delta u_{k}||^2_{R_j}\\=\left[\Delta u_k^TG_y^T-E_k^T\right]Q\left[\Delta u_kG_y-E_k\right]+\Delta u_k^TR\Delta u_k\\=\Delta u_k^T\left[G^T_yQG_y+R\right]\Delta u_k-2E^T_kQG_y\Delta u_k+E_k^TQE_k\\=\frac{1}{2}\Delta u_k^TH\Delta u_k+f^T\Delta u_k+const\tag{12}$$
where
$$H= 2\left[G^T_yQG_y+R\right]\tag{13}$$
$$f=-2G_y^TQE_k\tag{14}$$

To get the optimal control signal with the inequality conditions, we have to solve the  $min_{\Delta u_k}\left\{J_k(u,x_k)\right\}$. There is a well know solution of this optimal problem called quadratic programming. In Matlab there is the $quadprog$ function to solve the optimization problem. There is also some C/C++ implementations, one that I know is the ACADO Toolkit. One of the disadvantages of the solution is the time consumption, because of this the sampling time will be bigger therefore it could increase the instability of the system.  

Differentiating (12) the decisions $u_k$ and setting the result equal to zero the optimal control sequence is obtained as seen below:

$$\Delta u_k =\left[G^T_yQG_y+R\right]^{-1}G_y^TQE_k\tag{15}$$

Note that the optimal control deviation ($\Delta u_k$ ) gives offset free control in steady state. The expression above is true if we assume that the control signal changes between two steps is zero. 

Stability check

Hence stability can easily be established a posteriori by checking that the eigenvalues of the closed loop system matrix $\Phi+\Gamma K$ are strictly inside the unit circle, where

$$K=\left[G^T_yQG_y+R\right]^{-1}G_y^TQ\tag{16}$$

$N_c$ is the control horizon
$N$ is the prediction horizon