Matrix index out-of-bounds (at row evaluation) #2
Description
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