Below is my code and I am having problems running the StdMPC function that I am trying to define. Running my code gives me the error that the matrix A and Xopt((i)) are not of compatible shapes for matrix multiplication.
OptControlVector is a custom function I have defined in another package that gives me a list. In my code, I want to get the output from OptControlVector, take its first ‘m’ elements and store in a row of a matrix Uopt, calculate and assign the next row of Xopt (using the previous row of Xopt and same row no. of Uopt) and do this iteratively T number of times. So, I use the Do loop.
StdMPC(A_,B_,Ax_,Au_,Af_,bx_,bu_,bf_,Q_,Qf_,R_,initcond_,N_,T_):=
Module({Uopt=ConstantArray(0,{T,1}),Uopttemp,m=Dimensions(B)((2)),Xopt=ConstantArray(0,{T+1,1})},
Xopt((1))=initcond;
Do(Uopttemp=OptControlVector(A,B,Ax,Au,Af,bx,bu,bf,Q,Qf,R,Xopt((i)),N);
Uopt((i))=Uopttemp((1;;m));Xopt((i+1))=A.Xopt((i))+B.Uopt((i)),{i,T});Uopt)
This error made me suspicious that all the expressions in my Do loop are not being evaluated and that Xopt((i+1)) was not being evaluated which resulted in Xopt((2)) and so on being {0}. I then did this: In first case, I inserted Print(i) as the first expression in the Do loop and then in another case, put it as the second or third expression in the Do loop. The below code shows the first case I tried.
StdMPC(A_,B_,Ax_,Au_,Af_,bx_,bu_,bf_,Q_,Qf_,R_,initcond_,N_,T_):=
Module({Uopt=ConstantArray(0,{T,1}),Uopttemp,m=Dimensions(B)((2)),Xopt=ConstantArray(0,{T+1,1})},
Xopt((1))=initcond;
Do(Print(i);Uopttemp=OptControlVector(A,B,Ax,Au,Af,bx,bu,bf,Q,Qf,R,Xopt((i)),N);
Uopt((i))=Uopttemp((1;;m));Xopt((i+1))=A.Xopt((i))+B.Uopt((i)),{i,T});Uopt)
The output for this printed i=1,2,3 for T=3, as expected. However, for the second case, all I got was i=2 printed three times for T=3. The code for the second case is
StdMPC(A_,B_,Ax_,Au_,Af_,bx_,bu_,bf_,Q_,Qf_,R_,initcond_,N_,T_):=
Module({Uopt=ConstantArray(0,{T,1}),Uopttemp,m=Dimensions(B)((2)),Xopt=ConstantArray(0,{T+1,1})},
Xopt((1))=initcond;
Do(Uopttemp=OptControlVector(A,B,Ax,Au,Af,bx,bu,bf,Q,Qf,R,Xopt((i)),N);Print(i);
Uopt((i))=Uopttemp((1;;m));Xopt((i+1))=A.Xopt((i))+B.Uopt((i)),{i,T});Uopt)
I have been trying to debug this for the past 3-4 hours now and I would be extremely grateful is someone could point out any mistakes here.