From c5cdd03fc0588f7d34d8b97713caa6d19e5becea Mon Sep 17 00:00:00 2001 From: YanzhenXiangRobotics Date: Wed, 10 May 2023 23:13:51 +0200 Subject: ADD: pass task 25 --- templates/chebycenter.m | 20 +++ templates/cprnd.m | 296 ++++++++++++++++++++++++++++++++++++++ templates/generate_disturbances.m | 6 + templates/generate_params_z.m | 1 + templates/license.txt | 27 ++++ 5 files changed, 350 insertions(+) create mode 100755 templates/chebycenter.m create mode 100755 templates/cprnd.m create mode 100644 templates/license.txt 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/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/generate_params_z.m b/templates/generate_params_z.m index a7cfaa9..6d25f63 100644 --- a/templates/generate_params_z.m +++ b/templates/generate_params_z.m @@ -9,6 +9,7 @@ function [params_z] = generate_params_z(params) % initialize params_z params_z = params; +% display(params.model); % add initial condition of z-subsystem params_z.model = rmfield(params_z.model,{'InitialConditionA', ... diff --git a/templates/license.txt b/templates/license.txt new file mode 100644 index 0000000..4d6117c --- /dev/null +++ b/templates/license.txt @@ -0,0 +1,27 @@ +Copyright (c) 2011, Tim Benham +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the distribution + * Neither the name of the University of Queensland nor the names + of its contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. -- cgit v1.2.1