Implement a Non Linear MPC with Trignometric constraint

[James Mathew]

Hi,

I am trying to solve a non linear MPC problem using YALMIP. But I am getting the following error.

Parameters are currently only allowed to enter function such as exp, sin etc as exp(a),
sin(b) etc.

Following is the code that I am using,

input_N =2;
state_N = 5;
horizon_N = 30;
Q = eye(state_N);
R = eye(input_N);


%% Constraints
u1_min = -8;
u1_max = 8;
u2_min = -10;
u2_max = 10;




M_1 = [1;-1];
M_2 = [1;-1];
m_1 = [u1_max;u1_min];
m_2 = [u2_max;u2_min];


%% YALMIP
x = sdpvar(state_N,N_horizon);
u = sdpvar(input_N,N_horizon);


con_mpc = [];
obj_mpc = 0;
for i = 1:N_horizon-1
    x_theta(1) = u(2,i);
    x_theta(2) = x(3,i);
    x_theta(3) = u(1,i);
    x_theta(4) = x(5,i);
    x_theta(5) = u(1,i)*sin(x(1,i));
   con_mpc =[con_mpc, x(:,i+1) == [sin(x_mpc(2,i));sin(u_mpc(2,i));sin(u_mpc(1,i));sin(x_mpc(5,i));cos(x_mpc(1,i))]];
   con_mpc = [con_mpc, M_2*u(1,i) <= m_2 ];
   con_mpc = [con_mpc, M_1*u(2,i) <= m_1 ];
   obj_mpc = obj_mpc + x(:,i)'*Q*x(:,i) + u(:,i)'*R*u(:,i); % Cost function
end
% con_mpc = [con_mpc ,Ff*x_mpc(:,N_horizon) <= ff];
% obj_mpc = obj_mpc + x_mpc(:,N_horizon)'*Qf*x_mpc(:,N_horizon);
opt = sdpsettings;
opt.solver = 'quadprog';
opt.quadprog.TolCon = 1e-9;
innerController = optimizer(con_mpc, obj_mpc, opt, x(:,1), u(:,1));

I am getting the error even though I am only using the sine and cosine functions which it says is allowed.

[Johan Löfberg]

Weird code. In the loop to set up the prediction model, you temporarily crate a vector x_theta which you don't use, and you reference a variable x_mpc and u_mpc which you haven't defined in the code you show. Hence, this is not the code you are using. Also, I hope you know you can simply write -1 <= u(1,i) <= 1

[James Mathew]

Hi,

Thank you for the reply

You were right. I had edited the code slightly before posting. Since I was getting the error, I was trying out a different way to implement the same using x_theta. Below is the updated code

input_N =2;
state_N = 5;
horizon_N = 30;
Q = eye(state_N);
R = eye(input_N);




%% Constraints
u1_min = -8;
u1_max = 8;
u2_min = -10;
u2_max = 10;

%% YALMIP
x_mpc = sdpvar(state_N,horizon_N);
u_mpc = sdpvar(input_N,horizon_N);

con_mpc = [];
obj_mpc = 0;

for i = 1:horizon_N-1
   con_mpc =[con_mpc, x_mpc(:,i+1) == [sin(x_mpc(2,i));sin(u_mpc(2,i));sin(u_mpc(1,i));sin(x_mpc(5,i));cos(x_mpc(1,i))]];
   con_mpc = [con_mpc, u1_min<= u_mpc(1,i) <= u1_max ];
   con_mpc = [con_mpc, u2_min<= u_mpc(2,i) <= u2_max ];
   obj_mpc = obj_mpc + x_mpc(:,i)'*Q*x_mpc(:,i) + u_mpc(:,i)'*R*u_mpc(:,i); % Cost function
end


opt = sdpsettings;

opt.solver = 'quadprog';
opt.quadprog.TolCon = 1e-9;

innerController = optimizer(con_mpc, obj_mpc, opt, x_mpc(:,1), u_mpc(:,1));

But I am still getting the same error

Error using optimizer (line 297)

[Johan Löfberg]

For fixed parameter, i.e. x_mpc(:,1), this model is not linear as you have sin/cos on future x_mpc and u_mpc too. Hence, it makes no sense to use quadprog to try to solve this

Hence, as a first step you must use a nonlinear solver, and until you actually debugged and confirmed that you can solve this using some nonlinear solver, you should use optimize instead of optimizer.

Once you have something working, you can start to create a compiled solver for simulation (i.e. optimizer) where x1 is the parameter. The problem then is that you cannot use cos(x1) i.e nonlinearfunction(parameter). Instead, you define variables x1, cosx1 and sinx1 as new parameters in the model, and then you call optimizer object with currentx ,cos(currentx) and sin(currentx) as data for the parameters. But that far into the future for you. First you have to be able to actually solve this nonlinear MPC problem, which won't be easy.

[James Mathew]

Hi,

Thank you again for your reply. I will try to do what you said.