diff options
| -rw-r--r-- | templates/MPC.m | 89 | ||||
| -rw-r--r-- | templates/generate_constraints.m | 8 | ||||
| -rw-r--r-- | templates/traj_cost.m | 6 |
3 files changed, 93 insertions, 10 deletions
diff --git a/templates/MPC.m b/templates/MPC.m index 3e9d2f1..b2d411d 100644 --- a/templates/MPC.m +++ b/templates/MPC.m @@ -6,6 +6,44 @@ % 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; +% +% % define optimization variables +% U = sdpvar(repmat(nu,1,N),ones(1,N),'full'); +% X0 = sdpvar(nx,1,'full'); +% +% % YOUR CODE HERE +% +% 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, 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 + classdef MPC properties yalmip_optimizer @@ -15,31 +53,64 @@ classdef MPC function obj = MPC(Q,R,N,params) nu = params.model.nu; nx = params.model.nx; - + % YOUR CODE HERE % define optimization variables + 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); + + % define constraints +% 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 ... + ]; - % YOUR CODE HERE - + 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, 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; + % extract optimal control action and objective function value + u = optimizer_out{1}; + objective = optimizer_out{2}; + + % check feasibility of optimization problem + feasible = ~isnan(objective) && ~isinf(objective); if (errorcode ~= 0) feasible = false; end + % create control info struct ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime); end end -end
\ No newline at end of file +end diff --git a/templates/generate_constraints.m b/templates/generate_constraints.m index f87ac26..008f50c 100644 --- a/templates/generate_constraints.m +++ b/templates/generate_constraints.m @@ -15,5 +15,11 @@ function [H_u, h_u, H_x, h_x] = generate_constraints(params) s_max = params.constraints.MaxAbsPositionXZ; y_max = params.constraints.MaxAbsPositionY; h_x = [s_max;y_max;s_max;s_max;y_max;s_max]; - H_x = [eye(3);-eye(3)]; +% H_x = [eye(3);-eye(3)]; + H_x=[1,0,0,0,0,0; + 0,1,0,0,0,0; + 0,0,1,0,0,0; + -1,0,0,0,0,0; + 0,-1,0,0,0,0; + 0,0,-1,0,0,0]; end
\ No newline at end of file diff --git a/templates/traj_cost.m b/templates/traj_cost.m index 7e0b443..57566c9 100644 --- a/templates/traj_cost.m +++ b/templates/traj_cost.m @@ -8,4 +8,10 @@ function J_Nt = traj_cost(Xt,Ut,Q,R) % YOUR CODE HERE + J_Nt=0; + for i=1:length(Xt(1,:))-1 + a= Xt(:,i)'*Q*Xt(:,i); + b= Ut(:,i)'*R*Ut(:,i); + J_Nt=J_Nt+a+b; + end end
\ No newline at end of file |