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 /Submission files/MPC_TS_SC.m | |
parent | Update 23 (diff) | |
parent | update the new submission zip generated from npross branch (diff) | |
download | mpc_pe-d6b96c55ebb89ebd6e2d990ec89122799c68a230.tar.gz mpc_pe-d6b96c55ebb89ebd6e2d990ec89122799c68a230.zip |
Submission
Diffstat (limited to '')
-rw-r--r-- | Submission files/MPC_TS_SC.m | 72 |
1 files changed, 72 insertions, 0 deletions
diff --git a/Submission files/MPC_TS_SC.m b/Submission files/MPC_TS_SC.m new file mode 100644 index 0000000..fb1c720 --- /dev/null +++ b/Submission files/MPC_TS_SC.m @@ -0,0 +1,72 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +classdef MPC_TS_SC + properties + yalmip_optimizer + end + + 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 + tic; + [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 +end
\ No newline at end of file |