AA_no_control.m

%%% this is the axis aligned reachable set computation. Note that in
%%% this instance we assume the initial set (defined as a zonotope) 
%%% using X0_center and X0_gen is axis-aligned to the Z-Eigenvectors

clear;
close;
%% Initial setup of problem
dim = 3; % dimension of the system
T = 0.15;

% set options for reachability analysis:
time = 0:0.01:T;

% fix the random generator
rng(10)

% Construct orthonormal columns from svd
[orthCol, ~, ~] = svd(randn(dim, dim));

% Construct orthogonally deomposable tensor using the definition
odecoTensor = zeros(dim, dim, dim);
lambda = -0.5*ones(1,dim);
for j = 1:size(orthCol, 2)
    kronVector = kron(kron(orthCol(:, j), orthCol(:, j)), orthCol(:, j));
    odecoTensor = odecoTensor+lambda(j)*reshape(kronVector, dim, dim, dim);
end

% creating the initial condition sets in the frame of reference of the
% Z-Eigenvectors. Note: they are axis-aligned in this code
tX0_center = 10*ones(dim,1); 
tX0_gen = 0.5 * eye( dim );

% calculating the initial condition sets in the original frame of reference
X0_center = orthCol * tX0_center;
X0_gen = orthCol * tX0_gen;


% Explicit formula original system (note the "a" argument is the
% coordinates of the projection of the initial condition onto the orthCol'
% basis set)
homoPolyExp = @(t, a) sum((1 - lambda .* a .* t).^(-1) .* a .* orthCol, 2);

% the dynamical system (xdot)
homoPoly = @(x, u) reshape(reshape(odecoTensor, [numel(x)^2, numel(x)]) * x, numel(x), numel(x))*x;

% Explicit formula in the transformed reference frame, the orthcol terms
% found in homoPolyExp become I here because the axes are aligned to the
% Z-Eigenvectors
thomoPoly = @(a, u) sum((lambda' .* a.^2) .* eye(numel(a)), 2);

% the dynamical system in the transformed frame (xdot) 
thomoPolyExp =@(t,a) sum((1 - lambda' .* a .* t).^(-1) .* a .* eye(numel(a)), 2);

%% Algorithm 1 of paper
for i = 1:length(time)
    gen = [];
    R{ i } = 0;
    cen = zeros(dim,1);
    for j = 1:size( tX0_gen, 2 )
        upp = thomoPolyExp( time( i ), tX0_center+ tX0_gen( :, j ) );
        low = thomoPolyExp( time( i ), tX0_center-tX0_gen( :, j ) );
        cen(j) = (upp(j)+low(j))/2;
        gen = [gen,(upp-low)/2];
    end
    R{i} = zonotope( cen, gen );
end

% flipping back to original coordinate system
S = R;
for i = 1:length(R)
    c = R{i}.center;
    g = R{i}.generators;
    S{i} = zonotope( orthCol * c, orthCol * g );
    
end

%% Problem setup for implementing CORA
% figuring out the initial zonotope set
params.R0 = zonotope( tX0_center,  tX0_gen );
params.U = zonotope([0]);
params.tStart = 0;
params.tFinal = T;

sys = nonlinearSys(thomoPoly);

% defining initial conditions
hparams.R0 = zonotope( [ X0_center,  X0_gen ] );
hparams.U = zonotope([0]);
hparams.tStart = 0;
hparams.tFinal = T;

% simulation parameters
options.timeStep = 0.01; % this should match the simulation above
options.taylorTerms=5; % number of taylor terms for reachable sets
options.zonotopeOrder= 10; % zonotope order... increase this for more complicated systems.
options.maxError = 1000*ones(dim, 1); % our zonotopes shouldn't be "splitting", so this term doesn't matter for now
options.verbose = false;
options.uTrans = 0; % we won't be using any inputs, as traj. params specify trajectories
options.advancedLinErrorComp = 0;
options.tensorOrder = 3;
options.reductionInterval = inf;
options.reductionTechnique = 'girard';
options.alg = 'poly';
options.intermediateOrder = 50;
options.errorOrder = 1;

simOpt.points = 15;
simOpt.fracVert = 0.5;
simOpt.fracInpVert = 0.5;
simOpt.inpChanges = 6;
hsys = nonlinearSys(homoPoly);
% simulating the randomly initialized points
simRes = simulateRandom(hsys, hparams, simOpt);

% Generating reachable sets using CORA
R_cora = reach(hsys, hparams, options);

%% Plotting the results - 3 dimensional systems; modify as needed
dims = {[1 2] [2 3] [3 1]};
dim_labels = {'x', 'y', 'z'}; % Labels for dimensions

for k = 1:length(dims)
    projDim = dims{k};
    dim_label_x = dim_labels{projDim(1)};
    dim_label_y = dim_labels{projDim(2)};
    
    figure; hold on;

    % plot reachable sets generated by Algorithm 1
    for i = 1:length(time)
        plot(S{i}, projDim, 'c', 'Filled', false, 'EdgeColor', 'red','DisplayName', 'Proposed algorithm');
    end

    % plot initial set
    plot(hparams.R0, projDim, 'w', 'Filled', false, 'EdgeColor', 'blue', 'DisplayName', 'Initial set');

    % plot simulation results - proxy for ground truth
    plot(simRes, projDim, 'k', 'DisplayName', 'Simulation');

    % plot reachable sets generated by CORA
    plot(R_cora, projDim, 'Filled', false,'EdgeColor', 'green', 'DisplayName', 'RS-Cora');

    % Label plot
    xlabel(dim_label_x);
    ylabel(dim_label_y);
    title(['Projection onto ' dim_label_x ' vs ' dim_label_y]);
    %legend();
end