Matrix index out-of-bounds (at row evaluation)

[controlstudent]

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

[controlstudent]

Also, I don't see any feedback in your library. Is this MPC entirely open-loop? Matrix z (plant output) is not used in the bUpdate()