augmentedOPFLMI.m~

function [vgs,pgsNonSlack, thetaSlack, K,ssCost, trCostEstimate ] = augmentedOPFLMI( z0,networkS,...
    deltaploadg,deltaqloadg,deltaploadl,deltaqloadl, alpha, Tlqr)
%AUGMENTEDOPF  implements augmented opf per equation (18) CDC 2016. 
%    [vs,thetas, pgs,qgs, K ] = augmentedOPF( z0,...
 %   deltaploadg,deltaqloadg,deltaploadl,deltaqloadl) implements the
 %   augmetned OPF based on linear approximation of a known equilibrium z0
 % for the power systems described by nonlinear equations g(x,a,u) and
 % h(x,a,u). 
 %
 % Description of Outputs: 
 % 1. vs: the calculated optimal steady-state voltage magnitude, size(N,1).
 % 2. thetas: the calculated optimal steady-state voltage angle in radians,
 % size(N,1). 
 % 3. pgs: the calculated optimal steady-state real power injection
 % (setpoints) in pu Watts, size(G,1). 
 % 4. qgs: the calculated optimal steady-state reactive power injection 
% in pu Vars, size(G,1). 
% 5. K: the calculated optimal linear feedback gain, size(2*G,3*G)
% 6. ssCost: is the calculated steady-state cost of real power generation
% 7. trCostEstimate: is the gama---an estimate of the transient cost
% 
% Description of Inputs: 
% 1. z0: the  equilibrium point used for linearization
% 2. deltaploadg: the difference of the new desired  real power to the initial load level
% for generator nodes, size(G,1).
% 3. deltaqloadg: the difference of the new desired  reactive power to the initial load level
% for generator nodes, size(G,1).
% 4. deltaploadl: the difference of the new desired  real power to the initial load level
% for load nodes, size(L,1).
% 5. deltaqloadl: the difference of the new desired  reactive power to the initial load level
% for generator nodes, size(L,1)
% 6. alpha: alpha
% 7. Tlqr: is the Tlqr factor for transient control
% See also approxOPF, LQRstep
%
% Required:
% 

global Sbase N G L...
    deltaIdx omegaIdx eIdx mIdx...
    thetaIdx vIdx pgIdx qgIdx prefIdx fIdx...
    
global gen_set


 
    
%% Obtaining the jacobians:
 [ gx,ga,gu ] = gFunctionJacobVectorized(z0);
[ hx, ha,hu ] = hFunctionJacobVectorized( z0);
Asys=sparse(gx-ga*inv(ha)*hx);
Bsys=sparse(gu-ga*inv(ha)*hu);





%% costs:
c2k=networkS.gencost(:,5).*Sbase.^2;
c1k=networkS.gencost(:,6).*Sbase; 
c0k=networkS.gencost(:,7);




delta0=z0(deltaIdx); 
omega0=z0(omegaIdx); 
e0=z0(eIdx); 
m0=z0(mIdx);

pref0=z0(prefIdx); 
f0=z0(fIdx);

x0=[delta0;omega0;e0;m0];
u0=[pref0;f0];


h1Idx=1:G;
h2Idx=G+1:2*G;
h3Idx=2*G+1:3*G;
h4Idx=3*G+1:4*G;
h5Idx=4*G+1:4*G+L;
h6Idx=4*G+L+1:4*G+2*L;
deltaD=zeros(h6Idx(end),1);


deltaD(h3Idx)=-deltaploadg;
deltaD(h4Idx)=-deltaqloadg;
deltaD(h5Idx)=-deltaploadl;
deltaD(h6Idx)=-deltaqloadl;


%% Basis for P
Lp=((4*G)^2+(4*G))/2; % Number of lower-triangular elements of matrix P
% EIs=zeros(4*G,4*G,Lp); 
EIs=cell(Lp,1); 
% PiMatGamma=zeros(4*G+1,4*G+1,Lp); 
PiMatGamma=cell(Lp,1); 
% PiMatA=zeros(10*G,10*G,Lp); 
PiMatA=cell(Lp,1);

RowNum=1;
ColNum=1;
for m=1:Lp
 if RowNum<=4*G
EIs{m}=sparse([RowNum;ColNum], [ColNum;RowNum], [1;1],4*G,4*G);
RowNum=RowNum+1;
 else
 RowNum=RowNum-4*G+ColNum;
 ColNum=ColNum+1;
EIs{m}=sparse([RowNum;ColNum], [ColNum;RowNum], [1;1],4*G,4*G);
 RowNum=RowNum+1;
 end
  

       PiMatGamma{m}=[0, zeros(1,4*G); zeros(4*G,1), -EIs{m}];
 PiMatA{m}=[Asys*EIs{m}+EIs{m}*Asys.', EIs{m}, zeros(4*G,2*G); 
     EIs{m}, zeros(4*G,4*G), zeros(4*G,2*G); 
     zeros(2*G,4*G), zeros(2*G,4*G), zeros(2*G,2*G)];

end


% GammaMatGamma=[-1, zeros(1,4*G); zeros(4*G,1), zeros(4*G)];
GammaMatGamma=sparse(1,1,-1,4*G+1,4*G+1);


%% Basis for x^s
FIs=speye(4*G); 
XsMatGamma=cell(4*G,1);
for i=1:4*G
 XsMatGamma{i}=[0, FIs(:,i).'; FIs(:,i), zeros(4*G)];
end


X0MatGamma=sparse([0, -x0.'; -x0,zeros(4*G)]);

%% Basis for Y
Ly=(2*G)*(4*G);
GIs=cell(Ly,1);
YiMatA=cell(Ly,1);
for m=1:Ly
    [RowIdx,ColIdx]=ind2sub([2*G 4*G],m);
    GIs{m}=sparse(RowIdx,ColIdx,1,2*G,4*G);
    YiMatA{m}=[Bsys*GIs{m}  sparse(4*G,4*G) sparse(4*G,2*G); 
       sparse(4*G,4*G)  sparse(4*G,4*G) sparse(4*G,2*G);
        GIs{m} sparse(2*G,4*G) sparse(2*G,2*G)];
end

%% Basis for R
RTildeOI=ones(2*G,1); 
% RTildeIs=zeros(2*G,2*G,2*G);
RTildeIs=cell(2*G,1); 
for i=1:2*G
%     RTildeIs(i,i,i)=1;
RTildeIs{i}=sparse(i,i,1,2*G,2*G);
end
RTildePgIJ=[repmat(-alpha./(networkS.gen(:,9)./Sbase).',G,1); sparse(G,G)];
RTildeQgIJ=[sparse(G,G); repmat(-alpha./(networkS.gen(:,4)./Sbase).', G,1)];

%% Basis for Q
QTildeOI=ones(4*G,1); 
% QTildeIs=zeros(4*G,4*G,4*G);
QTilde=cell(4*G,1);
for i=1:4*G
%     QTildeIs(i,i,i)=1;
QTildeIs{i}=sparse(i,i,1,4*G,4*G);
end
QTildePgIJ=[repmat(-alpha./(networkS.gen(:,9)./Sbase).',G,1); 
    repmat(-alpha./(networkS.gen(:,9)./Sbase).',G,1); 
  sparse(G,G);
    repmat(-alpha./(networkS.gen(:,9)./Sbase).',G,1)];

QTildeQgIJ=[zeros(G,G); zeros(G,G);...
    repmat(-alpha./(networkS.gen(:,4)./Sbase).', G,1); 
    sparse(G,G)];


%% PgMatA, QgMatA
% PgMatA=zeros(10*G,10*G,G); 
PgMatA=cell(G,1); 
% QgMatA=zeros(10*G,10*G,G); 
QgMatA=cell(G,1); 

    
for j=1:G
PgMatARTilde=sparse(2*G,2*G); 
QgMatARTilde=sparse(2*G,2*G);
for i=1:2*G
    PgMatARTilde=PgMatARTilde+RTildePgIJ(i,j)*(-RTildeIs{i});
    QgMatARTilde=QgMatARTilde+RTildeQgIJ(i,j)*(-RTildeIs{i});

end
PgMatA{j}(8*G+1:10*G,8*G+1:10*G)=PgMatARTilde;
QgMatA{j}(8*G+1:10*G,8*G+1:10*G)=QgMatARTilde;


PgMatAQTilde=sparse(4*G,4*G); 
QgMatAQTilde=sparse(4*G,4*G);
for i=1:4*G
    PgMatAQTilde=PgMatAQTilde+QTildePgIJ(i,j)*(-QTildeIs{i});
    QgMatAQTilde=QgMatAQTilde+QTildeQgIJ(i,j)*(-QTildeIs{i});
end

PgMatA{j}(4*G+1:8*G, 4*G+1:8*G)=PgMatAQTilde;
QgMatA{j}(4*G+1:8*G, 4*G+1:8*G)=QgMatAQTilde;
    
end


MatOA=sparse(10*G,10*G);
MatARTildeO=sparse(2*G,2*G); 
for i=1:2*G
    MatARTildeO=MatARTildeO+RTildeOI(i)*(-RTildeIs{i});
end
MatOA(8*G+1:10*G,8*G+1:10*G)=MatARTildeO;

MatAQTildeO=sparse(4*G,4*G); 
for i=1:4*G
    MatAQTildeO=MatAQTildeO+QTildeOI(i)*(-QTildeIs{i});
end
MatOA(4*G+1:8*G, 4*G+1:8*G)=MatAQTildeO;

% all the required matrices are created:
%PiMatGamma
%GammaMatGamma
%XsMatGamma
%X0MatGamma
%PiMatA
%PgMatA
%QgMatA
%MatOA

% converting some cells to specific matrices
PiMatGammaMat=sparse((4*G+1)^2,Lp);
PiMatAMat=sparse((10*G);



cvx_begin quiet
cvx_solver mosek
variables zs(2*N+8*G)   gama2
variable P(Lp,1) 
variable Y(Ly,1)


deltas=zs(deltaIdx); 
omegas=zs(omegaIdx); 
es=zs(eIdx); 
ms=zs(mIdx);
vs=zs(vIdx);
thetas=zs(thetaIdx); 
pgs=zs(pgIdx); 
qgs=zs(qgIdx); 
prefs=zs(prefIdx); 
fs=zs(fIdx); 

xs=[deltas;omegas;es;ms];
us=[prefs;fs];
tic
minimize( c2k.'*square(pgs) + c1k.'*pgs+c0k.'*ones(G,1)+ (Tlqr)*gama2) ;
subject to:




omegas==omega0;

-pi<=thetas<=pi







zeros(4*G,1) == [gx, ga, gu]*( zs-z0); 
deltaD==[hx,ha,hu]*(zs-z0);


networkS.bus(:,13)<= vs<=networkS.bus(:,12);
networkS.gen(:,5)./Sbase<=qgs<=networkS.gen(:,4)./Sbase;
networkS.gen(:,10)./Sbase <= pgs <= networkS.gen(:,9)./Sbase; 

slackIdx=find(networkS.bus(:,2)==3); % finds the slack bus
thetas(slackIdx)==0;


SumPiGamma=cvx(sparse(4*G+1,4*G+1));
SumXsGamma=cvx(sparse(4*G+1,4*G+1));

SumPiA=cvx(sparse(10*G,10*G));
SumYiA=cvx(sparse(10*G,10*G));
SumPgA=cvx(sparse(10*G,10*G));
SumQgA=cvx(sparse(10*G,10*G));


fprintf('first loop \n'); 
for m=1:Lp
    SumPiGamma=SumPiGamma+P(m).*PiMatGamma{m};
     SumPiA=SumPiA+P(m)*PiMatA{m}; 
end

for i=1:4*G
    SumXsGamma=SumXsGamma+xs(i).*XsMatGamma{i};
end

% LMI1
-(SumPiGamma+gama2*GammaMatGamma+SumXsGamma+X0MatGamma)==semidefinite(4*G+1);



fprintf('second loop'); 
for m=1:Ly
    
    SumYiA=SumYiA+Y(m)*(YiMatA{m}+YiMatA{m}.');
end

for i=1:G
    SumPgA=SumPgA+pgs(i)*PgMatA{i};
    SumQgA=SumQgA+qgs(i)*QgMatA{i}; 
end

%LMI2:
-(SumPiA+SumYiA+SumPgA+SumQgA+MatOA)==semidefinite(10*G);
   


   
cvx_end

toc

PMat=sparse(4*G,4*G);
RowNum=1;
ColNum=1;
for m=1:Lp
 if RowNum<=4*G
PMat(RowNum,ColNum)=P(m);
PMat(ColNum,RowNum)=P(m);
RowNum=RowNum+1;
 else
 RowNum=RowNum-4*G+ColNum;
 ColNum=ColNum+1;
PMat(RowNum,ColNum)=P(m);
PMat(ColNum,RowNum)=P(m);
 RowNum=RowNum+1;
 end
  

end

P=PMat;
[ Qinv,Rinv ] = QinvRinv( pgs,qgs,alpha, zeros(4*G), zeros(2*G),networkS );
K=-Rinv*Bsys.'*conj(inv(P));
ssCost=c2k.'*(pgs.^2) + c1k.'*pgs+c0k.'*ones(G,1);
trCostEstimate=gama2;

vgs=vs(gen_set);
pgsNonSlack=pgs(networkS.bus(gen_set,2)==2);
thetaSlack=thetas(slackIdx);
end