summaryrefslogtreecommitdiffstats
path: root/templates
diff options
context:
space:
mode:
authorYanzhenXiangRobotics <54230111+YanzhenXiangRobotics@users.noreply.github.com>2023-05-15 13:13:37 +0200
committerYanzhenXiangRobotics <54230111+YanzhenXiangRobotics@users.noreply.github.com>2023-05-15 13:13:37 +0200
commit657966bf73fdb50fd1c7b55b15766e3eadfb7d4a (patch)
tree2f5d9d0a822737e6aac5686576a1c4f0c639ea85 /templates
parentADD: pass 28 29 (diff)
downloadmpc_pe-657966bf73fdb50fd1c7b55b15766e3eadfb7d4a.tar.gz
mpc_pe-657966bf73fdb50fd1c7b55b15766e3eadfb7d4a.zip
ADD: raw 30
Diffstat (limited to 'templates')
-rw-r--r--templates/MPC_TUBE.m41
1 files changed, 40 insertions, 1 deletions
diff --git a/templates/MPC_TUBE.m b/templates/MPC_TUBE.m
index bd62044..83b056b 100644
--- a/templates/MPC_TUBE.m
+++ b/templates/MPC_TUBE.m
@@ -17,6 +17,44 @@ classdef MPC_TUBE
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*(Z{1}-X0) <= 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 ...
+ ];
+ 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 +66,8 @@ classdef MPC_TUBE
[optimizer_out,errorcode] = obj.yalmip_optimizer(x);
solvetime = toc;
% YOUR CODE HERE
-
+ [u, objective] = optimizer_out{:};
+
feasible = true;
if (errorcode ~= 0)
feasible = false;