diff options
| -rw-r--r-- | templates/MPC_TS_SC.m | 42 | ||||
| -rw-r--r-- | templates/MPC_TS_SC_params.mat | bin | 0 -> 223 bytes | |||
| -rw-r--r-- | templates/MPC_TS_SC_script.m | 14 |
3 files changed, 53 insertions, 3 deletions
diff --git a/templates/MPC_TS_SC.m b/templates/MPC_TS_SC.m index 0e64767..fb1c720 100644 --- a/templates/MPC_TS_SC.m +++ b/templates/MPC_TS_SC.m @@ -14,22 +14,58 @@ classdef MPC_TS_SC methods function obj = MPC_TS_SC(Q,R,N,H,h,S,v,params) % YOUR CODE HERE + % initialize parameters + nu = params.model.nu; + nx = params.model.nx; + A=params.model.A; + B=params.model.B; + H_x = params.constraints.StateMatrix; + h_x = params.constraints.StateRHS; + H_u = params.constraints.InputMatrix; + h_u = params.constraints.InputRHS; + [~,P,~] = dlqr(A,B,Q,R); + + U = sdpvar(repmat(nu,1,N),ones(1,N),'full'); + X = sdpvar(repmat(nx,1,N+1),ones(1,N+1),'full'); + sv = sdpvar(repmat(length(S(1,:)),1,N+1),ones(1,N+1),'full'); + + X0 = sdpvar(nx,1,'full'); + objective = 0; + constraints = X{1} == X0; + % items for 1 to N + for k=1:N + objective = objective + X{k}' * Q * X{k} + U{k}' * R * U{k} + sv{k}' * S * sv{k} + v * max(sv{k}); + constraints = [constraints, ... + X{k+1} == A * X{k} + B * U{k}, ... + H_x * X{k} <= h_x+sv{k}, ... + H_u * U{k} <= h_u, ... + sv{k} >= 0 ... + ]; + end + % items for N+1 + objective = objective + X{N+1}'*P*X{N+1} + sv{N+1}'*S*sv{N+1} + v*max(sv{N+1}); + constraints = [constraints, ... + sv{N+1} >= 0, ... + H_x * X{N+1} <= h_x + sv{N+1} ... + ]; + % maximum positively invariant set constraint + constraints = [constraints, H * X{N+1} <= h]; opts = sdpsettings('verbose',1,'solver','quadprog','quadprog.TolFun',1e-8); obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective}); end function [u, ctrl_info] = eval(obj,x) - %% evaluate control action by solving MPC problem, e.g. + % evaluate control action by solving MPC problem tic; - [optimizer_out,errorcode] = obj.yalmip_optimizer(x); + [optimizer_out,errorcode,~] = obj.yalmip_optimizer{x}; solvetime = toc; + [u, objective] = optimizer_out{:}; feasible = true; if (errorcode ~= 0) feasible = false; end - ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime); end end diff --git a/templates/MPC_TS_SC_params.mat b/templates/MPC_TS_SC_params.mat Binary files differnew file mode 100644 index 0000000..b8cc020 --- /dev/null +++ b/templates/MPC_TS_SC_params.mat diff --git a/templates/MPC_TS_SC_script.m b/templates/MPC_TS_SC_script.m new file mode 100644 index 0000000..9499cf1 --- /dev/null +++ b/templates/MPC_TS_SC_script.m @@ -0,0 +1,14 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +S=100000000*eye(6); +v=100000000; +matfile=struct("S",S,"v",v); +save("MPC_TS_SC_params.mat","-struct","matfile") + + |