diff options
Diffstat (limited to 'templates/MPC_TUBE.m')
| -rw-r--r-- | templates/MPC_TUBE.m | 14 |
1 files changed, 9 insertions, 5 deletions
diff --git a/templates/MPC_TUBE.m b/templates/MPC_TUBE.m index 83b056b..9a630c1 100644 --- a/templates/MPC_TUBE.m +++ b/templates/MPC_TUBE.m @@ -15,7 +15,7 @@ classdef MPC_TUBE methods function obj = MPC_TUBE(Q,R,N,H_N,h_N,H_tube,h_tube,K_tube,params) obj.K_tube = K_tube; - + % YOUR CODE HERE nx = params.model.nx; nu = params.model.nu; @@ -42,14 +42,16 @@ classdef MPC_TUBE h_v = P_u_tightened.b; X0 = sdpvar(nx,1,'full'); - constraints = [H_tube*(Z{1}-X0) <= h_tube]; + constraints = [H_tube*(X0-Z{1}) <= h_tube]; objective = 0; for k = 1:N constraints = [ ... constraints, ... Z{k+1} == A*Z{k} + B*V{k} , ... - H_z * Z{k} <= h_z, ... - H_v * V{k} <= h_v ... +% H_z * Z{k} <= h_z, ... +% H_v * V{k} <= h_v ... + H_x * Z{k} <= h_x, ... + H_u * V{k} <= h_u ... ]; objective = objective + Z{k}'*Q*Z{k} + V{k}'*R*V{k}; end @@ -66,7 +68,9 @@ classdef MPC_TUBE [optimizer_out,errorcode] = obj.yalmip_optimizer(x); solvetime = toc; % YOUR CODE HERE - [u, objective] = optimizer_out{:}; + [v0, z0, objective] = optimizer_out{:}; + u = obj.K_tube*(x-z0)+v0; + feasible = true; if (errorcode ~= 0) |