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.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-- | Submission files/MPC_TS.m | 62 |
1 files changed, 62 insertions, 0 deletions
diff --git a/Submission files/MPC_TS.m b/Submission files/MPC_TS.m new file mode 100644 index 0000000..cbe7b45 --- /dev/null +++ b/Submission files/MPC_TS.m @@ -0,0 +1,62 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 + properties + yalmip_optimizer + end + + methods + function obj = MPC_TS(Q,R,N,H,h,params) + % YOUR CODE HERE + opts = sdpsettings('verbose',1,'solver','quadprog'); + + % Create yalmip optimization variables + U = sdpvar(repmat(params.model.nu,N,1),ones(1,N)); + X = sdpvar(repmat(params.model.nx,N+1,1),ones(1,N+1)); + + % Build cost function and constraints + Pinf = idare(params.model.A, params.model.B, Q, R); + objective = X{N+1}' * Pinf * X{N+1}; + constraints = []; + for k=1:N+1 + if k <= N + % These constraints are for the index in 1, ..., N + objective = objective + X{k}' * Q * X{k} + U{k}' * R * U{k}; + constraints = [constraints, ... + X{k+1} == params.model.A * X{k} + params.model.B * U{k}, ... + params.constraints.InputMatrix * U{k} <= params.constraints.InputRHS, ... + ]; + end + + % These contrains are for 1, ..., N+1 + constraints = [constraints, ... + params.constraints.StateMatrix * X{k} <= params.constraints.StateRHS, ... + ]; + end + % Terminal constraint + constraints = [constraints, H*X{N+1} <= h]; + + obj.yalmip_optimizer = optimizer(constraints,objective,opts,X{1},{U{1} objective}); end + + function [u, ctrl_info] = eval(obj,x) + %% evaluate control action by solving MPC problem, e.g. + 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 |