summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorNao Pross <np@0hm.ch>2023-05-24 16:50:06 +0200
committerNao Pross <np@0hm.ch>2023-05-24 17:03:43 +0200
commit1f12e47e6d1fe8b365806aa0b42de237970d53bf (patch)
treeba9c739eb170397a7b7a9303e3ea7d1a03a94844
parentTake deliverables for soft contraints from yuanxu (diff)
downloadmpc_pe-1f12e47e6d1fe8b365806aa0b42de237970d53bf.tar.gz
mpc_pe-1f12e47e6d1fe8b365806aa0b42de237970d53bf.zip
Take deliverables for Robust MPC from yanzhen
According to table 12 - generate_disturbances - simulate_uncertain - compute_tube_contorller - compute_minRPI - compute_tightening - MPC_TUBE - MPC_TUBE/eval (contained in MPC_TUBE.m) - MPC_TUBE_script (and its output MPC_TUBE_params.mat)
-rw-r--r--templates/MPC_TUBE.m47
-rw-r--r--templates/MPC_TUBE_params.matbin0 -> 1537 bytes
-rw-r--r--templates/MPC_TUBE_script.m15
-rwxr-xr-xtemplates/chebycenter.m20
-rw-r--r--templates/compute_minRPI.m28
-rw-r--r--templates/compute_tightening.m18
-rw-r--r--templates/compute_tube_controller.m5
-rwxr-xr-xtemplates/cprnd.m296
-rw-r--r--templates/generate_disturbances.m6
-rw-r--r--templates/simulate_uncertain.m13
10 files changed, 446 insertions, 2 deletions
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
new file mode 100644
index 0000000..9db7ef4
--- /dev/null
+++ b/templates/MPC_TUBE_params.mat
Binary files differ
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_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/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