MJLS MATLAB

buildScenarioTree.m

function tree = buildScenarioTree(N,T,P)
%Build Scenario Tree
if T==0
    tree=(1:N)';
else
    tree=permsRep(N,T+1); %list all the tree without considering P
    %****filter the tree "T" and "N" according to P******
    %if P(i,j)=0 then it is impossible to jump from mode i to mode j
    if nargin>2
        k=1;
        for i=1:size(tree,1)
            incr=1;
            for j=1:T
                if P(tree(k,j),tree(k,j+1))==0
                    tree(k,:)=[];
                    incr=0;
                    break
                end
            end
            k=k+incr;
        end
    end
end

FormulateMJLSMPCLV.m

function [controller, Tree, yalmip_struct] = ...
    FormulateMJLSMPCLV(probStruct,sysStruct,LV)
% do_write_file = false;
N = probStruct.N;
Prob = probStruct.Prob;
A = sysStruct.A;
B = sysStruct.B;
xmin = sysStruct.xmin;
xmax = sysStruct.xmax;
umin = sysStruct.umin;
umax = sysStruct.umax;
R = probStruct.R;
Q = probStruct.Q;
[nx,nu] = size(B{1});
disp('[MJLS] Constructing the scenario tree...');
Tree          = ConstructScenarioTreeMJLS(Prob,N,false);
disp('[MJLS] Imposing the constraints...');
% if do_write_file, fileID = fopen('yalmipBigProblem.m','w'); end
for iMode = 1:length(Tree)
    disp(['[MJLS] MODE: ' num2str(iMode) ' out of ' num2str(length(Tree))]);
    AncestorNodes = Tree{iMode}.AncestorNodes;
    LeafNodes     = Tree{iMode}.LeafNodes;
    Nodes         = Tree{iMode}.Nodes;
    ChildrenNodes = Tree{iMode}.ChildrenNodes;
    nNodes        = length(Tree{iMode}.Nodes);

    %fprintf(fileID,'x = sdpvar(repmat(%d,1,%d),repmat(1,1,%d));\n', ...
    %    nx, nNodes, nNodes);
    
    x = sdpvar(repmat(nx,1,nNodes),repmat(1,1,nNodes));
    
    %fprintf(fileID,'u = sdpvar(repmat(%d,1,%d),repmat(1,1,%d));\n',...
    %    nu, length(AncestorNodes), length(AncestorNodes));
    
    u = sdpvar(repmat(nu,1,length(AncestorNodes)),repmat(1,1,length(AncestorNodes)));
    
    %fprintf(fileID,'F=[];\n');
    
    F=[];
    disp(['[MJLS]   No. Children nodes: ' num2str(length(ChildrenNodes))]);
    %Dynamics equality constraints
    for iChild=ChildrenNodes
        Parent = Nodes{iChild}.Parent;
        Event  = Nodes{Parent}.Event;
     %   fprintf(fileID,'F = F + set(x{%d}==sysStruct.A{%d}*x{%d}+sysStruct.B{%d}*u{%d});\n',...
     %       iChild, Event,Parent,Event,Parent);
        F = F + set(x{iChild}==lv_update(x{Parent},u{Parent}, LV{Event}));
    end
    %State-input constraints and cost function for the Ancestor Nodes

    %For the root node we use include only the input vector, in order to be
    %able to use YALMIP's "optimizer" command. We don't lose something anyway
    Event=Nodes{1}.Event;
    % fprintf(fileID, 'obj=u{1}''*probStruct.R{%d}*u{1}\n', Event);
    obj=u{1}'*R{Event}*u{1};
    %The same goes for the state constraints of the root node;they are not needed
    % fprintf(fileID,'F = F + set(sysStruct.umin{%d}<=u{1}<=sysStruct.umax{%d});\n', Event, Event);
    F = F + set(umin{Event}<=u{1}<=umax{Event});
    %Next we enforce constraints for all the other ancestor nodes
    disp(['[MJLS]   No. Ancestor nodes: ' num2str(length(AncestorNodes(2:end)))]);
    for iAnc=AncestorNodes(2:end)
        Event=Nodes{iAnc}.Event;
        %fprintf(fileID,'F = F + set(sysStruct.xmin{%d}<=x{%d}<=sysStruct.xmax{%d});\n', Event, iAnc, Event);
        F = F + set(xmin{Event}<=x{iAnc}<=xmax{Event});
        %fprintf(fileID,'F = F + set(sysStruct.umin{%d}<=u{%d}<=sysStruct.umax{%d});\n', Event, iAnc, Event);
        F = F + set(umin{Event}<=u{iAnc}<=umax{Event});
        p_i=Nodes{iAnc}.Prob;
        %fprintf(fileID,'obj=obj+%g*(x{%d}''*probStruct.Q{%d}*x{%d}+u{%d}''*probStruct.R{%d}*u{%d});\n', p_i,...
        %    iAnc,Event,iAnc,iAnc,Event,iAnc);
        obj=obj+p_i*(x{iAnc}'*Q{Event}*x{iAnc}+u{iAnc}'*R{Event}*u{iAnc});
    end

    %Finally we impose terminal constraints for the Leas Nodes
    % And terminal cost for the Leaf Nodes
    Pfinal=probStruct.Pfinal;
    P_N=probStruct.P_N;
    disp(['[MJLS]   No. Leaf nodes    : ' num2str(length(LeafNodes))]);
    Hf=cell(length(Pfinal),1);
    Kf=cell(length(Pfinal),1);
    for i=1:length(Pfinal),
        [Hf{i},Kf{i}]=double(Pfinal(i));
    end
    disp('[MJLS]   Leaf Nodes - Iterating...');
    for iLeaf=LeafNodes
        p_i=Nodes{iLeaf}.Prob;
        Event=Nodes{iLeaf}.Event;
        % fprintf(fileID,'F = F+set(Hf{%d}*x{%d}<=Kf{%d});\n',Event,iLeaf,Event);
        F = F+set(x{iLeaf}'*P_N{Event}*x{iLeaf}<=probStruct.alpha);        
        % fprintf(fileID,'obj=obj+%g*x{%d}''*probStruct.P_N{%d}*x{%d};\n',p_i, iLeaf, Event,iLeaf);
        obj=obj+p_i*x{iLeaf}'*P_N{Event}*x{iLeaf};
    end
    disp('[MJLS]   Done - Setting up the optimizer...');
    yalmip_struct{iMode}.F=F;
    yalmip_struct{iMode}.obj=obj;
    yalmip_struct{iMode}.settings=sdpsettings('verbose',2);        
    controller{iMode} = optimizer(F,obj,...
        sdpsettings('verbose',2),x{1},u{1});
    disp('[MJLS]   Done!');
end

MJLSInvariantSet.m

function [O,Oint,k]=MJLSInvariantSet(AK,X_CL,P)
nModes=length(AK);
nx=length(AK{1});
O=X_CL;

for k=1:300
    if (mod(k,4)==0), 
        disp(['* Invariant set calculation ' num2str(k)]);
        Oint2
    end
    for iMode=1:nModes
        pi=P(iMode,:);
        p_gt0=find(pi>0);
        Oint2=O(p_gt0(1));
        for jMode=2:length(p_gt0)
            Oint2=intersect(Oint2,O(p_gt0(jMode)));
        end
        [On(iMode),keptrows,feasible]=domain(Oint2,AK{iMode},zeros(nx,1),O(iMode),1);
%         [On(iMode),keptrows,feasible]=domain(Oint,AK{iMode},zeros(nx,1),polytope,1);
    end
    eqP=zeros(1,nModes);
    for j=1:nModes
        eqP(j)=(O(j)==On(j));
    end
    if all(eqP);break;else O=On;end
end
Oint=O(1);
for jMode=2:nModes
    Oint=intersect(Oint,O(jMode));
end

terminalConditions4MJLS.m

function [probStruct] = terminalConditions4MJLS(probStruct,sysStruct)
% %
% %Solves the CARE for the unconstrained MJLS via LMI optimization
% %
% % CARE Ai'*Si*Ai-Xi-Ai'*Bi*Si* [Ri+Bi'*Si*Bi]^-1 *Bi'*Si*Ai+Qi=0 
% %Si = sum_j(pij*Xj),Xi>=0
A=sysStruct.A;B=sysStruct.B;
xmin=sysStruct.xmin;xmax=sysStruct.xmax;
umin=sysStruct.umin;umax=sysStruct.umax;
Q=probStruct.Q;R=probStruct.R;
P=probStruct.Prob;

[nx,nu]=size(B{1});
nModes=length(A);

for i=1:nModes
    X{i}=sdpvar(nx,nx);
end
Xsum=0;
F=set([]);
for i=1:nModes
    EX{i}=zeros(nx,nx);
    for j=1:nModes
        EX{i}=EX{i}+P(i,j)*X{j};
    end
    Si=[-X{i}+A{i}'*EX{i}*A{i}+Q{i} A{i}'*EX{i}*B{i}
        B{i}'*EX{i}*A{i} B{i}'*EX{i}*B{i}+R{i}];
    %     F=F+set(Si>0)+set(sysStruct{i}.B'*EX{i}*sysStruct{i}.B+probStruct{i}.R>0);
    F=F+set(Si>=1e-12);
    Xsum=Xsum+X{i};
end

sol = solvesdp(F,-trace(Xsum),sdpsettings('verbose',1));
Htot = [];Ktot = [];

for iMode=1:nModes
    probStruct.P_N{iMode} = double(X{iMode});
    probStruct.K{iMode}   = -inv(B{iMode}'*double(EX{iMode})*B{iMode}+R{iMode})*...
        B{iMode}'*double(EX{iMode})*A{iMode};
    AK{iMode}=A{iMode}+B{iMode}*probStruct.K{iMode};
    Hi{iMode}=[eye(nx);-eye(nx);probStruct.K{iMode};-probStruct.K{iMode}];
    Ki{iMode}=[xmax{iMode};-xmin{iMode};umax{iMode};-umin{iMode}];
    X_CL(iMode)=polytope(Hi{iMode},Ki{iMode});
    Htot=[Htot;Hi{iMode}];
    Ktot=[Ktot;Ki{iMode}];
end
disp('[MJLS] Calculating the invariant set...');
[probStruct.Pfinal,Oint,k]=MJLSInvariantSet(AK,X_CL,P);