diff options
Diffstat (limited to 'templates')
29 files changed, 826 insertions, 34 deletions
diff --git a/templates/LQR.m b/templates/LQR.m index 2a79da9..873546a 100644 --- a/templates/LQR.m +++ b/templates/LQR.m @@ -15,13 +15,17 @@ classdef LQR %constructor function obj = LQR(Q,R,params) % YOUR CODE HERE - % obj.K = ... (save feedback matrix for use in eval function) + E = zeros(params.model.nx); + S = zeros(params.model.nx, params.model.nu); + Pinf = idare(params.model.A, params.model.B, Q, R); + obj.K = -inv(R + params.model.B' * Pinf * params.model.B) ... + * params.model.B' * Pinf * params.model.A; end function [u, ctrl_info] = eval(obj,x) % YOUR CODE HERE - % u = ... + u = obj.K * x; ctrl_info = struct('ctrl_feas',true); end end -end
\ No newline at end of file +end diff --git a/templates/MPC.m b/templates/MPC.m index 3e9d2f1..355eb81 100644 --- a/templates/MPC.m +++ b/templates/MPC.m @@ -15,31 +15,57 @@ classdef MPC function obj = MPC(Q,R,N,params) nu = params.model.nu; nx = params.model.nx; - - % define optimization variables + % 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 ... + ]; - % 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; 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 +end diff --git a/templates/MPC_TE.m b/templates/MPC_TE.m index e0b55d2..747c7ee 100644 --- a/templates/MPC_TE.m +++ b/templates/MPC_TE.m @@ -15,7 +15,33 @@ classdef MPC_TE function obj = MPC_TE(Q,R,N,params) % YOUR CODE HERE opts = sdpsettings('verbose',1,'solver','quadprog'); - obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective}); + + % Create yalmip 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 + objective = 0; + 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, X{N+1} == zeros(size(X{N+1}))]; + + obj.yalmip_optimizer = optimizer(constraints,objective,opts,X{1},{U{1} objective}); end function [u, ctrl_info] = eval(obj,x) diff --git a/templates/MPC_TS.m b/templates/MPC_TS.m index 05f92ff..cbe7b45 100644 --- a/templates/MPC_TS.m +++ b/templates/MPC_TS.m @@ -15,8 +15,34 @@ classdef MPC_TS function obj = MPC_TS(Q,R,N,H,h,params) % YOUR CODE HERE opts = sdpsettings('verbose',1,'solver','quadprog'); - obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective}); - end + + % 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. 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") + + diff --git a/templates/MPC_TUBE.m b/templates/MPC_TUBE.m index bd62044..9a630c1 100644 --- a/templates/MPC_TUBE.m +++ b/templates/MPC_TUBE.m @@ -15,8 +15,48 @@ classdef MPC_TUBE methods function obj = MPC_TUBE(Q,R,N,H_N,h_N,H_tube,h_tube,K_tube,params) obj.K_tube = K_tube; - + % YOUR CODE HERE + nx = params.model.nx; + nu = params.model.nu; + + A = params.model.A; + B = params.model.B; + + [~,P,~] = dlqr(A,B,Q,R); + + V = sdpvar(repmat(nu,1,N),ones(1,N),'full'); + Z = sdpvar(repmat(nx,1,N+1),ones(1,N+1),'full'); + + H_x = params.constraints.StateMatrix; + h_x = params.constraints.StateRHS; + H_u = params.constraints.InputMatrix; + h_u = params.constraints.InputRHS; + + P_x_tightened = Polyhedron('A',H_x,'b',h_x).minus(Polyhedron('A',H_tube,'b',h_tube)); + H_z = P_x_tightened.A; + h_z = P_x_tightened.b; + + P_u_tightened = Polyhedron('A',H_u,'b',h_u).minus(K_tube*Polyhedron('A',H_tube,'b',h_tube)); + H_v = P_u_tightened.A; + h_v = P_u_tightened.b; + + X0 = sdpvar(nx,1,'full'); + constraints = [H_tube*(X0-Z{1}) <= h_tube]; + objective = 0; + for k = 1:N + constraints = [ ... + constraints, ... + Z{k+1} == A*Z{k} + B*V{k} , ... +% H_z * Z{k} <= h_z, ... +% H_v * V{k} <= h_v ... + H_x * Z{k} <= h_x, ... + H_u * V{k} <= h_u ... + ]; + objective = objective + Z{k}'*Q*Z{k} + V{k}'*R*V{k}; + end + constraints = [constraints, H_N * Z{end} <= h_N]; + objective = objective + Z{end}'*P*Z{end}; opts = sdpsettings('verbose',1,'solver','quadprog'); obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{V{1} Z{1} objective}); @@ -28,7 +68,10 @@ classdef MPC_TUBE [optimizer_out,errorcode] = obj.yalmip_optimizer(x); solvetime = toc; % YOUR CODE HERE - + [v0, z0, objective] = optimizer_out{:}; + u = obj.K_tube*(x-z0)+v0; + + feasible = true; if (errorcode ~= 0) feasible = false; diff --git a/templates/MPC_TUBE_params.mat b/templates/MPC_TUBE_params.mat Binary files differnew file mode 100644 index 0000000..9db7ef4 --- /dev/null +++ b/templates/MPC_TUBE_params.mat diff --git a/templates/MPC_TUBE_script.m b/templates/MPC_TUBE_script.m new file mode 100644 index 0000000..a021cd0 --- /dev/null +++ b/templates/MPC_TUBE_script.m @@ -0,0 +1,15 @@ +p = [0.05, 0.1]; +params = generate_params(); +params = generate_params_z(params); +K_tube = compute_tube_controller(p,params); +[H_tube, h_tube, n_iter] = compute_minRPI(K_tube, params); +params = compute_tightening(K_tube, H_tube, h_tube, params); + +Q = diag([300,0.1]);R = 1;N = 50; +[H_N, h_N] = lqr_maxPI(Q, R, params); + +% MPC_TUBE_obj = MPC_TUBE(Q,R,N,H_N,h_N,H_tube,h_tube,K_tube,params); +matfile = struct("p", p, "K_tube", K_tube, "H_tube", H_tube, "h_tube", h_tube,... + "H_N", H_N, "h_N", h_N, "params_z_tube", params); +save("MPC_TUBE_params.mat", "-struct", "matfile"); + diff --git a/templates/chebycenter.m b/templates/chebycenter.m new file mode 100755 index 0000000..cbdce1c --- /dev/null +++ b/templates/chebycenter.m @@ -0,0 +1,20 @@ +function [c,r] = chebycenter(A,b) +%CHEBYCENTER Compute Chebyshev center of polytope Ax <= b. +% The Chebyshev center of a polytope is the center of the largest +% hypersphere enclosed by the polytope. +% Requires optimization toolbox. + +[n,p] = size(A); +an = sqrt(sum(A.^2,2)); +A1 = zeros(n,p+1); +A1(:,1:p) = A; +A1(:,p+1) = an; +f = zeros(p+1,1); +f(p+1) = -1; + +options = optimset; +options = optimset(options,'Display', 'off'); +c = linprog(f,A1,b,[],[],[],[],[],options); +r = c(p+1); +c = c(1:p); +end diff --git a/templates/compute_minRPI.m b/templates/compute_minRPI.m index 0b4ffdb..19df517 100644 --- a/templates/compute_minRPI.m +++ b/templates/compute_minRPI.m @@ -8,4 +8,32 @@ function [H_tube,h_tube,n_iter] = compute_minRPI(K_tube,params) % YOUR CODE HERE + A = params.model.A+params.model.B*K_tube; + % A = params.model.A; +% display(A) + nx = params.model.nx; + omega = Polyhedron('A',[eye(nx);-eye(nx)],'b',zeros(1,2*nx)'); + % display(omega.A) + % display(omega.b) + n_iter = 0; + Hw = params.constraints.DisturbanceMatrix; + hw = params.constraints.DisturbanceRHS; + W = Polyhedron('A',Hw,'b',hw); + while true + omega_last = copy(omega); + % display(n_iter) + % A_last = omega.A; + % b_last = omega.b; + display(n_iter); + display(omega.b) + omega = omega.plus(A^(n_iter)*W); + + if eq(omega.minHRep(),omega_last.minHRep()) + % if eq(omega,omega_last) + break + end + n_iter = n_iter+1; + end + H_tube = omega.A; + h_tube = omega.b; end
\ No newline at end of file diff --git a/templates/compute_tightening.m b/templates/compute_tightening.m index 8919d12..7095f9c 100644 --- a/templates/compute_tightening.m +++ b/templates/compute_tightening.m @@ -8,4 +8,22 @@ function params = compute_tightening(K_tube,H_tube,h_tube,params) % YOUR CODE HERE + Hx = params.constraints.StateMatrix; + hx = params.constraints.StateRHS; + + Hu = params.constraints.InputMatrix; + hu = params.constraints.InputRHS; + + Pxz = Polyhedron('A',Hx,'b',hx); + Puz = Polyhedron('A',Hu,'b',hu); + % Ptube = Polyhedron('A',H_tube,'b',h_tube); + + Px_tube = Pxz.minus(Polyhedron('A',H_tube,'b',h_tube)); + params.constraints.StateMatrix = Px_tube.A; + params.constraints.StateRHS = Px_tube.b; + + Pu_tube = Puz.minus(K_tube*Polyhedron('A',H_tube,'b',h_tube)) +% Pu_tube = Puz.minus(Polyhedron('A',H_tube*K_tube,'b',h_tube)); + params.constraints.InputMatrix = Pu_tube.A; + params.constraints.InputRHS = Pu_tube.b; end
\ No newline at end of file diff --git a/templates/compute_tube_controller.m b/templates/compute_tube_controller.m index adca95d..2c567f0 100644 --- a/templates/compute_tube_controller.m +++ b/templates/compute_tube_controller.m @@ -8,4 +8,9 @@ function K_tube = compute_tube_controller(p,params) % YOUR CODE HERE + Az = params.model.A; + Bz =params.model.B; + % Bz = [params.model.B,eye(params.model.nx)]; + K_tube = -place(Az,Bz,p); + % K_tube = K(1:params.model.nu,:); end
\ No newline at end of file diff --git a/templates/cprnd.m b/templates/cprnd.m new file mode 100755 index 0000000..4183ac3 --- /dev/null +++ b/templates/cprnd.m @@ -0,0 +1,296 @@ +function [X S] = cprnd(N,A,b,options) +%CPRND Draw from the uniform distribution over a convex polytope. +% X = cprnd(N,A,b) Returns an N-by-P matrix of random vectors drawn +% from the uniform distribution over the interior of the polytope +% defined by Ax <= b. A is a M-by-P matrix of constraint equation +% coefficients. b is a M-by-1 vector of constraint equation +% constants. +% +% cprnd(N,A,b,options) allows various options to be specified. + +% 'method' Specifies the algorithm. One of the strings 'gibbs', +% 'hitandrun' (the default), and 'achr'. The default +% algorithm 'hitandrun' is a vanilla hit-and-run sampler +% [1]. 'gibbs' specifies a Gibbs sampler, 'achr' is the +% Adaptive Centering Hit-and-Run algorithm of [2]. +% +% 'x0' Vector x0 is a starting point which should be interior +% to the polytope. If x0 is not supplied CPRND uses the +% Chebyshev center as the initial point. +% +% 'isotropic' Perform an adaptive istropic transformation of the +% polytope. The values 0, 1 and 2 respectively turn +% off the transformation, construct it during a runup +% period only, or continuously update the +% tranformation throughout sample production. The +% transformation makes sense only for the Gibbs and +% Hit-and-run samplers (ACHR is invariant under +% linear transformations). +% +% 'discard' Number of initial samples (post-runup) to discard. +% +% 'runup' When the method is gibbs or hitandrun and the +% isotropic transformation is used, this is the +% number of initial iterations of the algorithm in +% the untransformed space. That is a sample of size +% runup is generated and its covariance used as the +% basis of a transformation. +% When the method is achr runup is the number of +% conventional hitandrun iterations. See [2]. +% +% 'ortho' Zero/one flag. If turned on direction vectors for +% the hitandrun algorithm are generated in random +% orthonormal blocks rather than one by +% one. Experimental and of dubious utility. +% +% 'quasi' Allows the user to specify a quasirandom number generator +% (such as 'halton' or 'sobol'). Experimental and of +% dubious utility. +% +% + +% By default CPRND employs the hit-and-run sampler which may +% exhibit marked serial correlation and very long convergence times. +% +% References +% [1] Kroese, D.P. and Taimre, T. and Botev, Z.I., "Handbook of Monte +% Carlo Methods" (2011), pp. 240-244. +% [2] Kaufman, David E. and Smith, Robert L., "Direction Choice for +% Accelerated Convergence in Hit-and-Run Sampling", Op. Res. 46, +% pp. 84-95. +% +% Copyright (c) 2011-2012 Tim J. Benham, School of Mathematics and Physics, +% University of Queensland. + + function y = stdize(z) + y = z/norm(z); + end + + p = size(A,2); % dimension + m = size(A,1); % num constraint ineqs + x0 = []; + runup = []; % runup necessary to method + discard = []; % num initial pts discarded + quasi = 0; + method = 'achr'; + orthogonal = 0; + isotropic = []; + + % gendir generates a random unit (direction) vector. + gendir = @() stdize(randn(p,1)); + + % Alternative function ogendir forces directions to come in + % orthogonal bunches. + Ucache = {}; + function u = ogendir() + if length(Ucache) == 0 + u = stdize(randn(p,1)); + m = null(u'); % orthonormal basis for nullspace + Ucache = mat2cell(m',ones(1,p-1)); + else + u = Ucache{end}'; + Ucache(end) = []; + end + end + + % Check input arguments + + if m < p+1 + % obv a prob here + error('cprnd:obvprob',['At least ',num2str(p+1),' inequalities ' ... + 'required']); + end + + if nargin == 4 + if isstruct(options) + + if isfield(options,'method') + method = lower(options.method); + switch method + case 'gibbs' + case 'hitandrun' + case 'achr' + otherwise + error('cprnd:badopt',... + ['The method option takes only the ' ... + 'values "gibbs", "hitandrun", and "ACHR".']); + end + end + + if isfield(options,'isotropic') + % Controls application of isotropic transformation, + % which seems to help a lot. + isotropic = options.isotropic; + end + + if isfield(options,'discard') + % num. of samples to discard + discard = options.discard; + end + + if isfield(options,'quasi') + % Use quasi random numbers, which doesn't seem to + % help much. + quasi = options.quasi; + if quasi && ~ischar(quasi), quasi='halton'; end + if ~strcmp(quasi,'none') + qstr = qrandstream(quasi,p,'Skip',1); + gendir = @() stdize(norminv(qrand(qstr,1),0,1)'); + end + end + + if isfield(options,'x0') + % initial interior point + x0 = options.x0; + end + + if isfield(options,'runup') + % number of points to generate before first output point + runup = options.runup; + end + + if isfield(options,'ortho') + % Generate direction vectors in orthogonal + % groups. Seems to help a little. + orthogonal = options.ortho; + end + + else + x0 = options; % legacy support + end + end + + % Default and apply options + + if isempty(isotropic) + if ~strcmp(method,'achr') + isotropic = 2; + else + isotropic = 0; + end + end + + if orthogonal + gendir = @() ogendir(); + end + + % Choose a starting point x0 if user did not provide one. + if isempty(x0) + x0 = chebycenter(A,b); % prob. if p==1? + end + + % Default the runup to something arbitrary. + if isempty(runup) + if strcmp(method,'achr') + runup = 10*(p+1); + elseif isotropic > 0 + runup = 10*p*(p+1); + else + runup = 0; + end + end + + % Default the discard to something arbitrary + if isempty(discard) + if strcmp(method,'achr') + discard = 25*(p+1); + else + discard = runup; + end + end + + X = zeros(N+runup+discard,p); + + n = 0; % num generated so far + x = x0; + + % Initialize variables for keeping track of sample mean, covariance + % and isotropic transform. + M = zeros(p,1); % Incremental mean. + S2 = zeros(p,p); % Incremental sum of + % outer prodcts. + S = eye(p); T = eye(p); W = A; + + while n < N+runup+discard + y = x; + + % compute approximate stochastic transformation + if isotropic>0 + if n == runup || (isotropic > 1 && n > runup) + T = chol(S,'lower'); + W = A*T; + end + y = T\y; + end + + switch method + + case 'gibbs' + % choose p new components + for i = 1:p + % Find points where the line with the (p-1) components x_i + % fixed intersects the bounding polytope. + e = circshift(eye(p,1),i-1); + z = W*e; + c = (b - W*y)./z; + tmin = max(c(z<0)); tmax = min(c(z>0)); + % choose a point on that line segment + y = y + (tmin+(tmax-tmin)*rand)*e; + end + + case 'hitandrun' + % choose a direction + u = gendir(); + % determine intersections of x + ut with the polytope + z = W*u; + c = (b - W*y)./z; + tmin = max(c(z<0)); tmax = min(c(z>0)); + % choose a point on that line segment + y = y + (tmin+(tmax-tmin)*rand)*u; + + case 'achr' + % test whether in runup or not + if n < runup + % same as hitandrun + u = gendir(); + else + % choose a previous point at random + v = X(randi(n),:)'; + % line sampling direction is from v to sample mean + u = (v-M)/norm(v-M); + end + % proceed as in hit and run + z = A*u; + c = (b - A*y)./z; + tmin = max(c(z<0)); tmax = min(c(z>0)); + % Choose a random point on that line segment + y = y + (tmin+(tmax-tmin)*rand)*u; + end + + if isotropic>0 + x = T*y; + else + x = y; + end + + X(n+1,:)=x'; + n = n + 1; + + % Incremental mean and covariance updates + delta0 = x - M; % delta new point wrt old mean + M = M + delta0/n; % sample mean + delta1 = x - M; % delta new point wrt new mean + if n > 1 + S2 = S2 + (n-1)/(n*n)*delta0*delta0'... + + delta1*delta1'; + S0 = S; + S = S2/(n-1); % sample covariance + else + S = eye(p); + end + + end + + X = X((discard+runup+1):(N+discard+runup),:); + +end diff --git a/templates/generate_constraints.m b/templates/generate_constraints.m index 892b706..e2e4357 100644 --- a/templates/generate_constraints.m +++ b/templates/generate_constraints.m @@ -7,5 +7,10 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [H_u, h_u, H_x, h_x] = generate_constraints(params) - % YOUR CODE HERE + H_u = [eye(params.model.nu); -eye(params.model.nu)]; + h_u = params.constraints.MaxAbsThrust * ones(params.model.nu*2,1); + + H_x = [eye(3), zeros(3); -eye(3), zeros(3)]; % * inv(params.model.ScalingMatrix); + h_x = params.constraints.MaxAbsPositionXZ * [1, 0, 1, 1, 0, 1].' + ... + params.constraints.MaxAbsPositionY * [0, 1, 0, 0, 1, 0].'; end
\ No newline at end of file diff --git a/templates/generate_disturbances.m b/templates/generate_disturbances.m index 570a4c9..f08cdb5 100644 --- a/templates/generate_disturbances.m +++ b/templates/generate_disturbances.m @@ -8,4 +8,10 @@ function Wt = generate_disturbances(params) % YOUR CODE HERE + % params_z = generate_params_z(params); + Hw = params.constraints.DisturbanceMatrix; + hw = params.constraints.DisturbanceRHS; + % Pw = Polyhedron('A', Hw, 'b', hw); + N = params.model.HorizonLength; + Wt = cprnd(N,Hw,hw)'; end
\ No newline at end of file diff --git a/templates/generate_params.m b/templates/generate_params.m index ef51366..4b61e31 100644 --- a/templates/generate_params.m +++ b/templates/generate_params.m @@ -40,6 +40,19 @@ params.exercise = struct( ... 'QdiagOptA', [94.0; 0.1579; 300; 0.01; 0.10; 0.10] ... ); -% YOUR CODE HERE +% Add system model +[Ac, Bc] = generate_system_cont(params); +[At, Bt] = generate_system(Ac, Bc, params); +[A, B] = generate_system_scaled(At, Bt, params); + +params.model.A = A; +params.model.B = B; + +% Add constraints +[H_u, h_u, H_x, h_x] = generate_constraints(params); +params.constraints.InputMatrix = H_u; +params.constraints.InputRHS = h_u; +params.constraints.StateMatrix = H_x; +params.constraints.StateRHS = h_x; end diff --git a/templates/generate_system.m b/templates/generate_system.m index 9deb347..b44bef2 100644 --- a/templates/generate_system.m +++ b/templates/generate_system.m @@ -7,5 +7,9 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [A, B] = generate_system(Ac, Bc, params) - % YOUR CODE HERE -end
\ No newline at end of file + sys = ss(Ac, Bc, eye(params.model.nx), zeros(params.model.nx, params.model.nu)); + sysd = c2d(sys, params.model.TimeStep); + + A = sysd.A; + B = sysd.B; +end diff --git a/templates/generate_system_cont.m b/templates/generate_system_cont.m index 2d3ee79..048ee7b 100644 --- a/templates/generate_system_cont.m +++ b/templates/generate_system_cont.m @@ -7,5 +7,16 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [Ac, Bc] = generate_system_cont(params) - % YOUR CODE HERE -end
\ No newline at end of file + % YOUR CODE HERE + omega_n_sq = params.model.GravitationalParameter / params.model.TargetRadius^3; + omega_n = sqrt(omega_n_sq); + + Ac = [ + zeros(3), eye(3); + 3*omega_n_sq, 0, 0, 0, 2*omega_n, 0; + 0, 0, 0, -2*omega_n, 0, 0; + 0, 0, -omega_n_sq, 0, 0, 0; + ]; + + Bc = [zeros(3); eye(3);] / params.model.Mass; +end diff --git a/templates/generate_system_scaled.m b/templates/generate_system_scaled.m index eac8db8..d65c7b3 100644 --- a/templates/generate_system_scaled.m +++ b/templates/generate_system_scaled.m @@ -6,6 +6,11 @@ % Please see the LICENSE file that has been included as part of this package. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function [A,B] = generate_system_scaled(At,Bt,params) +function [A, B] = generate_system_scaled(At, Bt, params) % YOUR CODE HERE -end
\ No newline at end of file + V = params.model.ScalingMatrix; + Vinv = inv(V); + + A = V*At*Vinv; + B = V*Bt; +end diff --git a/templates/lqr_maxPI.m b/templates/lqr_maxPI.m index efa335a..99b5b22 100644 --- a/templates/lqr_maxPI.m +++ b/templates/lqr_maxPI.m @@ -6,7 +6,40 @@ % Please see the LICENSE file that has been included as part of this package. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function [H, h] = lqr_maxPI(Q,R,params) - % YOUR CODE HERE + +function [H, h] = lqr_maxPI(Q, R, params) + % Define the linear system + A = params.model.A; + B = params.model.B; + nx=params.model.nx; + nu=params.model.nu; + + s_max= params.constraints.MaxAbsPositionXZ; + y_max= params.constraints.MaxAbsPositionY; + u_max = params.constraints.MaxAbsThrust; + + K = -dlqr(A, B, Q, R); + systemLQR = LTISystem('A', A+B*K); +% absxmax = ones(nx,1)*s_max; +% absumax = ones(nu,1)*u_max; +% Xp = Polyhedron('A',[eye(nx); -eye(nx); K; -K], 'b', [absxmax;absxmax; absumax;absumax]); + Hx=params.constraints.StateMatrix; + hx=params.constraints.StateRHS; + Hu=params.constraints.InputMatrix; + hu=params.constraints.InputRHS; + Xp = Polyhedron('A',[Hx;Hu*K], 'b', [hx;hu]); + + figure(1); + systemLQR.x.with('setConstraint'); + systemLQR.x.setConstraint = Xp; + InvSetLQR = systemLQR.invariantSet(); +% InvSetLQR.plot(), alpha(0.25), title('Invariant Set for Triple Integrator under LQR Control'), xlabel('x_1'), ylabel('x_2'), zlabel('x_3'); + + H=InvSetLQR.A; + h=InvSetLQR.b; + + end + + diff --git a/templates/lqr_tuning.m b/templates/lqr_tuning.m index 4a26158..9c9aab9 100644 --- a/templates/lqr_tuning.m +++ b/templates/lqr_tuning.m @@ -8,4 +8,48 @@ function [tuning_struct, i_opt] = lqr_tuning(x0,Q,params) % YOUR CODE HERE -end
\ No newline at end of file + i_opt = nan; + best_J_u = inf; + + % Prepare an array of empty structs + tuning_struct = repmat(struct( ... + 'InitialCondition', {}, ... + 'Qdiag', {}, ... + 'MaxAbsPositionXZ', {}, ... + 'MaxAbsPositionY', {}, ... + 'MaxAbsThrust', {}, ... + 'InputCost', {}, ... + 'MaxFinalPosDiff', {}, ... + 'MaxFinalVelDiff', {}, ... + 'TrajFeasible', {} ... + ), size(Q,2), 1); + + for i=1:size(Q,2) + tuning_struct(i).InitialCondition = x0; + tuning_struct(i).Qdiag = Q(:,i); + + ctrl = LQR(diag(Q(:,i)), eye(params.model.nu), params); + [Xt, Ut, ~] = simulate(x0, ctrl, params); + + [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = ... + traj_constraints(Xt, Ut, params); + + tuning_struct(i).MaxAbsPositionXZ = s_max; + tuning_struct(i).MaxAbsPositionY = y_max; + tuning_struct(i).MaxAbsThrust = u_max; + tuning_struct(i).InputCost = J_u; + tuning_struct(i).MaxFinalPosDiff = df_max; + tuning_struct(i).MaxFinalVelDiff = vf_max; + tuning_struct(i).TrajFeasible = traj_feas; + + if traj_feas + if J_u < best_J_u + i_opt = i; + best_J_u = J_u; + end + end + end + + % because the test suite wants a column vector + tuning_struct = tuning_struct'; +end diff --git a/templates/lqr_tuning_script.m b/templates/lqr_tuning_script.m new file mode 100644 index 0000000..4a02fc6 --- /dev/null +++ b/templates/lqr_tuning_script.m @@ -0,0 +1,58 @@ +function [tuning_struct] = lqr_tuning_script(params) + % From the hint: look in the vicinity of vector from the next section + % also from the hint: qx, qy, qvx, qvy are decoupled from qz, qvz + q_start = [94, .1579, 300, .01, .1, .1]; + + % parameters for sampling + v = 0.4; % search around q +- v * q + nsamples = 3; + + q = q_start; + best_J_u = inf; + while best_J_u > 7.6 + % Create sampling grid + range = linspace(1-v, 1+v, nsamples)'; + + % Sample in qx qy qvx qvy + G = kron(q([1,2,4,5]), range); + [Sx, Sy, Svx, Svy] = ndgrid(G(:,1), G(:,2), G(:,3), G(:,4)); + Q = [Sx(:), Sy(:), q(3) * ones(size(Sx(:))), ... + Svx(:), Svy(:), q(6) * ones(size(Sx(:)))]'; + + % Find best q + [ts, i_opt] = lqr_tuning(params.model.InitialConditionA, Q, params); + + % Save new best solution + if ~ isnan(i_opt) + tuning_struct = ts(i_opt); + best_J_u = ts(i_opt).InputCost; + q = ts(i_opt).Qdiag'; + end + + % Sample in qz qvz + G = kron(q([3,6]), range); + [Sz, Svz] = ndgrid(G(:,1), G(:,2)); + qs = kron(q, ones(size(Sz(:)))); + Q = [qs(:,1:2), Sz(:), qs(:,3:4), Svz(:)]'; + + % Find best item + [ts, i_opt] = lqr_tuning(params.model.InitialConditionA, Q, params); + + % Save new best solution + if ~ isnan(i_opt) + tuning_struct = ts(i_opt); + best_J_u = ts(i_opt).InputCost; + q = ts(i_opt).Qdiag'; + end + + % Search in a narrower region, increase resolution + v = v / 2; + + % Log + fprintf("Lowest J_u so far: %d, searching with v=%f\n", best_J_u,v); + end + + % Save for test + matfile = struct("tuning_struct", tuning_struct, "q", tuning_struct.Qdiag); + save("lqr_tuning_script.mat", "-struct", "matfile"); +end diff --git a/templates/lqr_tuning_script.mat b/templates/lqr_tuning_script.mat Binary files differnew file mode 100644 index 0000000..af9681c --- /dev/null +++ b/templates/lqr_tuning_script.mat diff --git a/templates/simulate.m b/templates/simulate.m index 3138cd1..6be40c3 100644 --- a/templates/simulate.m +++ b/templates/simulate.m @@ -7,8 +7,17 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [Xt,Ut,ctrl_info] = simulate(x0, ctrl, params) + % YOUR CODE HERE + % Hint: you can access the control command with ctrl.eval(x(:,i)) -% YOUR CODE HERE -% Hint: you can access the control command with ctrl.eval(x(:,i)) + Xt = zeros(params.model.nx, params.model.HorizonLength+1); + Ut = zeros(params.model.nu, params.model.HorizonLength); + ctrl_info = repmat(struct('ctrl_feas',true), 1, params.model.HorizonLength); -end
\ No newline at end of file + Xt(:,1) = x0; + for k=1:params.model.HorizonLength + [u, ~] = ctrl.eval(Xt(:,k)); + Ut(:,k) = u; + Xt(:,k+1) = params.model.A * Xt(:,k) + params.model.B * Ut(:,k); + end +end diff --git a/templates/simulate_uncertain.m b/templates/simulate_uncertain.m index dc9df26..f74f7af 100644 --- a/templates/simulate_uncertain.m +++ b/templates/simulate_uncertain.m @@ -8,4 +8,17 @@ function [Xt,Ut,ctrl_info] = simulate_uncertain(x0, ctrl, Wt, params) % YOUR CODE HERE + Xt = [x0]; + Ut = []; + x = x0 + ctrl_info = []; + % obj = LQR(); + for i = 1:params.model.HorizonLength + [u, ctrl_info_] = ctrl.eval(x); + Ut = [Ut u]; + ctrl_info = [ctrl_info ctrl_info_]; + x = params.model.A*x + params.model.B*u + Wt(:,i); + Xt = [Xt x]; + end + % plot_trajectory(Xt, Ut, ctrl_info, params); end
\ No newline at end of file diff --git a/templates/traj_constraints.m b/templates/traj_constraints.m index 4b0a081..69d19d3 100644 --- a/templates/traj_constraints.m +++ b/templates/traj_constraints.m @@ -8,5 +8,33 @@ function [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(x,u,params) % YOUR CODE HERE + + s_max = max(max(abs(x([1,3],:)))); + y_max = max(abs(x(2,:))); + u_max = max(max(abs(u))); + + J_u = 0; + for k=1:size(u,2) + J_u = J_u + u(:,k)' * u(:,k); + end + + df_max = sqrt(sum(x(1:3,end).^2)); + vf_max = sqrt(sum(x(4:6,end).^2)); + + constr = [ + s_max <= params.constraints.MaxAbsPositionXZ, ... + y_max <= params.constraints.MaxAbsPositionY, ... + u_max <= params.constraints.MaxAbsThrust, ... + df_max <= params.constraints.MaxFinalPosDiff, ... + vf_max <= params.constraints.MaxFinalVelDiff + ]; + + % disp([s_max, y_max, df_max, vf_max]) + % disp([params.constraints.MaxAbsPositionXZ, ... + % params.constraints.MaxAbsPositionY, ... + % params.constraints.MaxFinalPosDiff, ... + % params.constraints.MaxFinalVelDiff]) + % disp(constr') + traj_feas = all(constr); end 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 |