function RobInv
% Robust invariant Set Examples
%
% T. Besselmann, ETH Zurich, 18.08.2008
% F. Borrelli, UC Berkeley, 05.07.2009
% M. Herceg, Automatic Control Laboratory, ETH Zurich 2014
%
% requires MPT3

close all

%% printing parameters
label_font_size = 14;
tick_font_size = 10;
line_width = 0.8;
axeswidth=0.2;
% create new folder for storing figures
mkdir('figures');
figure_name1 = ['.',filesep,'figures',filesep,'RobInv_ex_14_1'];
figure_name2 = ['.',filesep,'figures',filesep,'RobInv_ex_14_2'];
width = 6;
set(0,'defaulttextinterpreter','none');

%% core code
% Stable Autonomous system with additive noise
% x(k+1) = A*x(k) + w(k)
A=[0.5 0;1 -0.5];

% create model in MPT3 interface
model1= LTISystem('A',A);

% constraints on states
model1.x.min = [-10;-10];
model1.x.max = [ 10; 10];

% state constraints
X = Polyhedron('lb',model1.x.min,'ub',model1.x.max);
% noise constraints
W = Polyhedron('lb',[-1;-1],'ub',[1;1]);

% compute the invariant set including the intermediate steps
P1 = reach_set(model1, X, Polyhedron.emptySet(2),  W);

fig1 = figure;
plot(X,'color',[0.7 0.7 0.7],'linewidth',line_width);
hold on
plot(P1,'color',[0.5 0.5 0.5],'linewidth',line_width) 
border = 1;
axis([model1.x.min(1)-border,model1.x.max(1)+border,model1.x.min(2)-border,model1.x.max(2)+border]);


ht1=text(7,-8,'$\mathcal{X}$');
set(ht1, 'FontSize', label_font_size);
ht2=text(-1,-0.5,'$\mathcal{O}_{\infty}$');
set(ht2, 'FontSize', label_font_size);
xt = transpose(-10:5:10);
yt = transpose(-10:5:10);
set(gca,'XTick',xt);
set(gca,'YTick',yt);
set(gca,'YTickLabel',num2str(xt));
set(gca,'XTickLabel',num2str(yt));


set(gca,'LineWidth',axeswidth)
set(gca,'FontSize', tick_font_size);

hx1 = xlabel('$x_1$');
set(hx1, 'FontSize', label_font_size);
hy1 = ylabel('$x_2$');
set(hy1, 'FontSize', label_font_size);
    
    
% Unstable System with additive noise
% x(k+1) = A*x(k) + B*u(k) + w(k)
A=[1.5 0;1 -1.5];
B=[1;0];

model2 = LTISystem('A',A,'B',B); 
model2.u.min = -5;
model2.u.max = 5;
model2.x.min = [-10;-10];
model2.x.max = [ 10; 10];

% input constraints
U = Polyhedron('lb',model2.u.min,'ub',model2.u.max);

% compute the invariant set including the intermediate steps
P2 = reach_set(model2, X, U, 0.1*W);

fig2 = figure;
plot(X,'color',[0.7 0.7 0.7],'linewidth',line_width);
hold on
plot(P2,'color',[0.5 0.5 0.5],'linewidth',line_width) 
border = 1;
axis([model2.x.min(1)-border,model2.x.max(1)+border,model2.x.min(2)-border,model2.x.max(2)+border]);

ht1=text(7,-8,'$\mathcal{X}$');
set(ht1, 'FontSize', label_font_size);
ht2=text(-2,-0.5,'$\mathcal{C}_{\infty}$');
set(ht2, 'FontSize', label_font_size);
xt = transpose(-10:5:10);
yt = transpose(-10:5:10);
set(gca,'XTick',xt);
set(gca,'YTick',yt);
set(gca,'YTickLabel',num2str(xt));
set(gca,'XTickLabel',num2str(yt));


set(gca,'LineWidth',axeswidth)
set(gca,'FontSize', tick_font_size);

hx1 = xlabel('$x_1$');
set(hx1, 'FontSize', label_font_size);
hy1 = ylabel('$x_2$');
set(hy1, 'FontSize', label_font_size);



%% save book figures
% generate FIG file
disp('Hit any key to print the figures.');
pause
saveas(fig1, [figure_name1,'_matlab'], 'fig');

% print
laprint(fig1, [figure_name1,'_tex'],'scalefonts','off','width',width);


% generate FIG file
saveas(fig2, [figure_name2,'_matlab'], 'fig');

% print
laprint(fig2, [figure_name2,'_tex'],'scalefonts','off','width',width);


end

function P = reach_set(model, X, U,  W)
% computation of a robust invariant set for given LTImodel 
%
maxIterations = 50;
X0 = X; % initial set constraint
for i = 1:maxIterations
    % subtract noise    
    S = X0 - W;
    % backward reachable set
    R = model.reachableSet('X', S, 'U', U,...
        'direction', 'backward');
    % intersect with the state constraints
    P = R.intersect(X0).minHRep();
    if P==X0
        break
    else
        X0 = P;
    end
end

end
