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