pronenewbits / arduino_unconstrained_mpc_library Goto Github PK

In the third implementation (the numerically robust one), R_L matrix is defined with a size of {(MPC_HP_LEN*SS_Z_LEN + MPC_HU_LEN*SS_U_LEN), MPC_HP_LEN*SS_Z_LEN}.
But R_L is multiplied with dU(k)_optimal during the problem formulation. As dU(k)_optimal has a size of {MPC_HU_LEN*SS_U_LEN,1}, the R_L matrix should have a size of {(MPC_HP_LEN*SS_Z_LEN + MPC_HU_LEN*SS_U_LEN), MPC_HU_LEN*SS_U_LEN}.

This size is confirmed by the fact that GammaLeft is (SQ*CTHETA) over SR :

• SQ has a size of {MPC_HP_LEN*SS_Z_LEN, MPC_HP_LEN*SS_Z_LEN}
• CTHETA has a size of {MPC_HP_LEN*SS_Z_LEN, MPC_HU_LEN*SS_U_LEN}
• Thus, SQ*CTHETA has a size of {MPC_HP_LEN*SS_Z_LEN, MPC_HU_LEN*SS_U_LEN}
• SR has a size of {MPC_HU_LEN*SS_U_LEN, MPC_HU_LEN*SS_U_LEN}
• Therefore (SQ*CTHETA) over SR has a size of {(MPC_HP_LEN*SS_Z_LEN + MPC_HU_LEN*SS_U_LEN), MPC_HU_LEN*SS_U_LEN}
• Q_L * R_L = GammaLeft, so the number of columns of R_L is the number of columns of GammaLeft, which is MPC_HU_LEN*SS_U_LEN and not MPC_HP_LEN*SS_Z_LEN

In your model, it does not seem to be an issue because MPC_HU_LEN*SS_U_LEN is 8 and MPC_HP_LEN*SS_Z_LEN is 14, i.e. MPC_HU_LEN*SS_U_LEN < MPC_HP_LEN*SS_Z_LEN.

Nonetheless, thank you for this amazing open source code
Best regards

Thank you very much for your project. I am trying to adapt your project to my 4-state SISO.

In the setup() routine, I am describing the plant:

A[0][0] = -0.2399;
A[0][1] = -0.3451;
A[0][2] = -0.171;
A[0][3] = 0.0;
A[1][0] = 0.3825;
A[1][1] = 0.2083;
A[1][2] = -0.505;
A[1][3] = 0.0;
A[2][0] = 0.2824;
A[2][1] = 0.4265;
A[2][2] = 0.8546;
A[2][3] = 0.0;
A[3][0] = 0.002541;
A[3][1] = 0.005183;
A[3][2] = 0.01914;
A[3][3] = 1.0;

B[0][0] = 0.3825;
B[1][0] = 1.13;
B[2][0] = 0.3252;
B[3][0] = 0.001914;

C[0][0] = -1.29;
C[0][1] = 1.512;
C[0][2] = -2.954;
C[0][3] = 73.84;

And modified the input (SP_NEXT) accordingly in the loop():

/* ================================ Updating Set Point ================================= */
    Matrix SP_NEXT(SS_Z_LEN, 1);
    if (i32iterSP < 100 - MPC_HP_LEN + 1) {
      SP_NEXT[0][0] = 10.;
    } else if (i32iterSP < 200 - MPC_HP_LEN + 1) {
      SP_NEXT[0][0] = 20.;
    } else {
      SP_NEXT[0][0] = 30.;
    }
    if (i32iterSP < 300 - MPC_HP_LEN + 1) {
      i32iterSP++;
    } else {
      i32iterSP = 0;
    }
    for (int32_t _i = 0; _i < (MPC_HP_LEN - 1); _i++) {
      SP = SP.InsertSubMatrix(SP, (_i * SS_Z_LEN), 0, ((_i + 1) * SS_Z_LEN), 0, SS_Z_LEN, 1);
    }
    SP = SP.InsertSubMatrix(SP_NEXT, ((MPC_HP_LEN - 1) * SS_Z_LEN), 0, 0, 0, SS_Z_LEN, 1);
    /* -------------------------------- Updating Set Point --------------------------------- */

In the konfig.h file, I've changed the dimensions accordingly.

/* State Space dimension */
#define SS_X_LEN    (4)
#define SS_Z_LEN    (1)
#define SS_U_LEN    (1)
#define SS_DT_MILIS (20)                        
#define SS_DT       float_prec(SS_DT_MILIS/1000.)   /* Sampling time */

In the terminal I get:
Matrix index out-of-bounds (at row evaluation)

I cannot figure out how to address this. I am using Teensy4.1, Arduino IDE 2.2.1.

Thank you