diff options
author | Yuan Xu <yuanxu@student.ethz.ch> | 2023-05-24 17:35:46 +0200 |
---|---|---|
committer | Yuan Xu <yuanxu@student.ethz.ch> | 2023-05-24 17:35:46 +0200 |
commit | a23ba49056a36aff221d0c23e228b981222c3576 (patch) | |
tree | 742c18cbe13c1603909b55ee82188ce6360e8fa0 /Submission files/MPC.m | |
parent | ADD: declaration of originality (diff) | |
download | mpc_pe-a23ba49056a36aff221d0c23e228b981222c3576.tar.gz mpc_pe-a23ba49056a36aff221d0c23e228b981222c3576.zip |
add submission zipsubmission
Diffstat (limited to '')
-rw-r--r-- | Submission files/MPC.m | 71 |
1 files changed, 71 insertions, 0 deletions
diff --git a/Submission files/MPC.m b/Submission files/MPC.m new file mode 100644 index 0000000..355eb81 --- /dev/null +++ b/Submission files/MPC.m @@ -0,0 +1,71 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 + properties + yalmip_optimizer + end + + methods + function obj = MPC(Q,R,N,params) + nu = params.model.nu; + nx = params.model.nx; + % YOUR CODE HERE + A=params.model.A; + B=params.model.B; + U = sdpvar(repmat(nu,1,N),ones(1,N),'full'); + X = sdpvar(repmat(nx,1,N+1),ones(1,N+1),'full'); + + [K,P,~] = dlqr(A,B,Q,R); + +% s_max=params.constraints.MaxAbsPositionXZ; +% y_max=params.constraints.MaxAbsPositionY; +% u_max = params.constraints.MaxAbsThrust; + H_x = params.constraints.StateMatrix; + h_x = params.constraints.StateRHS; + H_u = params.constraints.InputMatrix; + h_u = params.constraints.InputRHS; + X0 = sdpvar(nx,1,'full'); + objective = 0; + constraints = X{1} == X0; + for k = 1:N + constraints = [ ... + constraints, ... + X{k+1} == A*X{k} + B*U{k} , ... + H_x * X{k} <= h_x, ... + H_u * U{k} <= h_u ... + ]; + + objective = objective + X{k}'*Q*X{k} + U{k}'*R*U{k}; + end + objective=objective+X{N+1}'*P*X{N+1}; + % terminal constraint +% constraints = [ ... +% constraints, ... +% X{N+1} == zeros(nx,1) +% ]; + opts = sdpsettings('verbose',1,'solver','quadprog'); + 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 |