I tried to run below MPC code in the implicit prediction form, but I got error (from the last line) saying: "All parameter arguments have to be simple variables (i.e., not expressions such a+b or 1+a)", could you tell me what went wrong?
Sorry I don't know why I cannot format what I typed to make it look nicer.
B = [1;2;3]; Const = [4;5;6];
N = 10; % prediction horizon
Q = eye(2); R = 0.5; P = 1;
options = sdpsettings('verbose', 2, 'debug', 1);
x_s = sdpvar(ones(1, N+1), ones(1, N+1)); y_s = x_s; phi_s = x_s;
u_s = sdpvar(ones(1, N), ones(1, N));
% Propagate Yalmip constraints and objectives
constraints = [];
objective = 0;
for j = 1 : N
objective = objective ...
+ R * u_s{j}^2 ... % control effort
+ P * (phi_s{j} - phi_s{j+1})^2; % heading angle stability
constraints = [constraints, ...
x_s{j+1} == x_s{j} + B(1)*u_s{j} + Const(1), ...
y_s{j+1} == y_s{j} + B(2)*u_s{j} + Const(2), ...
phi_s{j+1} == phi_s{j} + B(3)*u_s{j} + Const(3)]; % equality constraints
constraints = [constraints, -1 <= u_s{j} <= 1]; % inequality constraints
end
controller = optimizer(constraints, objective, options, {x_s{1}, y_s{1}, phi_s{1}}, [u_s{:}]);