diff options
author | YanzhenXiang <54230111+YanzhenXiangRobotics@users.noreply.github.com> | 2023-11-06 16:49:26 +0100 |
---|---|---|
committer | GitHub <noreply@github.com> | 2023-11-06 16:49:26 +0100 |
commit | d6b96c55ebb89ebd6e2d990ec89122799c68a230 (patch) | |
tree | 7a837438da9e07a5e77658c6d882577fff8bcae1 /templates/MPC_TUBE.m | |
parent | Update 23 (diff) | |
parent | update the new submission zip generated from npross branch (diff) | |
download | mpc_pe-master.tar.gz mpc_pe-master.zip |
Submission
Diffstat (limited to '')
-rw-r--r-- | templates/MPC_TUBE.m | 47 |
1 files changed, 45 insertions, 2 deletions
diff --git a/templates/MPC_TUBE.m b/templates/MPC_TUBE.m index bd62044..9a630c1 100644 --- a/templates/MPC_TUBE.m +++ b/templates/MPC_TUBE.m @@ -15,8 +15,48 @@ 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; + + A = params.model.A; + B = params.model.B; + + [~,P,~] = dlqr(A,B,Q,R); + + V = sdpvar(repmat(nu,1,N),ones(1,N),'full'); + Z = sdpvar(repmat(nx,1,N+1),ones(1,N+1),'full'); + + H_x = params.constraints.StateMatrix; + h_x = params.constraints.StateRHS; + H_u = params.constraints.InputMatrix; + h_u = params.constraints.InputRHS; + + P_x_tightened = Polyhedron('A',H_x,'b',h_x).minus(Polyhedron('A',H_tube,'b',h_tube)); + H_z = P_x_tightened.A; + h_z = P_x_tightened.b; + + P_u_tightened = Polyhedron('A',H_u,'b',h_u).minus(K_tube*Polyhedron('A',H_tube,'b',h_tube)); + H_v = P_u_tightened.A; + h_v = P_u_tightened.b; + + X0 = sdpvar(nx,1,'full'); + 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_x * Z{k} <= h_x, ... + H_u * V{k} <= h_u ... + ]; + objective = objective + Z{k}'*Q*Z{k} + V{k}'*R*V{k}; + end + constraints = [constraints, H_N * Z{end} <= h_N]; + objective = objective + Z{end}'*P*Z{end}; opts = sdpsettings('verbose',1,'solver','quadprog'); obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{V{1} Z{1} objective}); @@ -28,7 +68,10 @@ classdef MPC_TUBE [optimizer_out,errorcode] = obj.yalmip_optimizer(x); solvetime = toc; % YOUR CODE HERE - + [v0, z0, objective] = optimizer_out{:}; + u = obj.K_tube*(x-z0)+v0; + + feasible = true; if (errorcode ~= 0) feasible = false; |