stateestimation-self-derivatives/run.m

clear
clc
% yalmip('clear')
addpath('.\Powerflow')
[~, ~, ~, ~,Volt,Vangle,Y,Yangle,r,c,newwordParameter,PG,QG,PD,QD,Balance]=pf('ieee14.dat', '0');
%% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% <EFBFBD><EFBFBD>ѹ <EFBFBD>ڵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֧·<EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD>ڵ㹦<EFBFBD><EFBFBD> ֧·<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%%
%% ״̬<EFBFBD><EFBFBD>
% <EFBFBD><EFBFBD>ѹ <EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%%
%% <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
sigma=0.05;% <EFBFBD><EFBFBD>׼<EFBFBD><EFBFBD>
%% <EFBFBD><EFBFBD>ѹ
%<EFBFBD><EFBFBD>ѹ<EFBFBD><EFBFBD>ֵ
rVolt=Volt; %<EFBFBD><EFBFBD>ֵ
BalanceVolt=Volt(Balance);
mVolt=rVolt.*(normrnd(0,sigma,length(Volt),1)+1);%<EFBFBD><EFBFBD>ѹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
rVAngel=Vangle;
%% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%ע<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
cmpY=Y.*exp(1j*sparse(r,c,Yangle,length(Y),length(Y)));%<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɾ<EFBFBD><EFBFBD><EFBFBD>
cmpV=Volt.*exp(1j*Vangle); %<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѹ
cmpI=cmpY*cmpV;% ע<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
rI=abs(cmpI); %ע<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
mI=rI.*(normrnd(0,sigma,length(rI),1)+1);%<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%% ֧·<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% ֧·<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lineI=newwordParameter.line.lineI;
lineJ=newwordParameter.line.lineJ;
lineR=newwordParameter.line.lineR;
lineX=newwordParameter.line.lineX;
lineB2=newwordParameter.line.lineB2;
lineG=real(1./(lineR+1j*lineX));
lineB=imag(1./(lineR+1j*lineX));
cmpBranchI=(cmpV(lineI)-cmpV(lineJ))./(lineR+1j*lineX);%<EFBFBD><EFBFBD><EFBFBD><EFBFBD>֧·<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
rBranchI=abs(cmpBranchI);% ֧·<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
mBranchI=rBranchI.*(normrnd(0,sigma,length(rBranchI),1)+1);%֧·<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%% ֧·<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
rBranchP=real((cmpV(lineI)).*conj(cmpBranchI))+real(cmpV(lineI) .*conj(1j*lineB2.*cmpV(lineI) ) );
% rV=abs(cmpV);
% rA=angle(cmpV);
% a=rV(lineI).^2.*(lineG)-rV(lineI).*rV(lineJ).*(lineG.*cos(rA(lineI)-rA(lineJ) )+lineB.*sin(rA(lineI)-rA(lineJ) ) );
% b=-rV(lineI).^2.*(lineB+lineB2)-rV(lineI).*rV(lineJ).*(lineG.*sin(rA(lineI)-rA(lineJ) )-lineB.*cos(rA(lineI)-rA(lineJ) ) );
% rBranchP=rBranchP(abs(rBranchP)>1e-5);
mBranchP=rBranchP.*(normrnd(0,sigma,length(rBranchP),1)+1);%֧·<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
rBranchQ=imag((cmpV(lineI)).*conj(cmpBranchI))+imag(cmpV(lineI) .*conj(1j*lineB2.*cmpV(lineI) ) );
% rBranchQ=rBranchQ(abs(rBranchQ)>1e-5);
mBranchQ=rBranchQ.*(normrnd(0,sigma,length(rBranchQ),1)+1);%֧·<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%% ע<EFBFBD>빦<EFBFBD><EFBFBD>
rPD=PD;
PDi=find(PD~=0);
rQD=QD;
QDi=find(QD~=0);
rPG=PG;
PGi=find(PG~=0);
rQG=QG;
QGi=find(QG~=0);
mPD=rPD.*(normrnd(0,sigma,length(rPD),1)+1);
mQD=rQD.*(normrnd(0,sigma,length(rQD),1)+1);
mPG=rPG.*(normrnd(0,sigma,length(rPG),1)+1);
mQG=rQG.*(normrnd(0,sigma,length(rQG),1)+1);
%% 0ע<EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>
zerosInjectionIndex=1:length(Volt);
zerosInjectionIndex=zerosInjectionIndex( ~(PD~=0|QD~=0|PG~=0|QG~=0) );
%% <EFBFBD><EFBFBD>ѹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
rTransP=TransPower( newwordParameter,cmpY,rVolt,rVAngel );
rTransQ=TransReactivePower( newwordParameter,cmpY,rVolt,rVAngel );
mTransP=rTransP.*(normrnd(0,sigma,length(rTransP),1)+1);
mTransQ=rTransQ.*(normrnd(0,sigma,length(rTransQ),1)+1);
%% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD>빦<EFBFBD><EFBFBD>
% <EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD>ֻ<EFBFBD>з<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľڵ<EFBFBD>
PDQDi=union(PDi,QDi);
onlyPG=setdiff(PGi,PDQDi);
onlyQG=setdiff(QGi,PDQDi);
%% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><EFBFBD><EFBFBD>
stateVarCount=2*length(Volt);
measurements=length(mVolt)+length(mBranchI)+length(mBranchP)+length(mBranchQ)+length(mPG)+length(mQG)+length(mTransP)+length(mTransQ);
fprintf('<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> %f\n',measurements/stateVarCount);
%% Opti ToolBox
Busnum=length(Volt);
%% save
% save('mVolt','mVolt');
% save('mPG','mPG');
% save('mQG','mQG');
% save('mBranchI','mBranchI');
% save('mBranchP','mBranchP');
% save('mBranchQ','mBranchQ');
% save('mTransP','mTransP');
% save('mTransQ','mTransQ');
%% load
% load('mVolt');
% load('mPG');
% load('mQG');
% load('mBranchI');
% load('mBranchP');
% load('mBranchQ');
% load('mTransP');
% load('mTransQ');
%% <EFBFBD>Լ<EFBFBD>д<EFBFBD><EFBFBD>΢<EFBFBD>ִ<EFBFBD><EFBFBD><EFBFBD>
% <EFBFBD><EFBFBD><EFBFBD>¶<EFBFBD><EFBFBD><EFBFBD>Jacobi<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SEVolt=sparse(ones(length(mVolt),1));
SEVAngel=sparse(zeros(length(mVolt),1));
maxD=1000;
Iteration=0;
while maxD>1e-3;
dV_dV=sparse(1:length(mVolt),1:length(mVolt),1,length(mVolt),length(mVolt));%<EFBFBD><EFBFBD>ѹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>΢<EFBFBD><EFBFBD>
dV_dTyta=sparse(length(mVolt),length(mVolt));
dLPij_dVi=sparse(1:length(lineI),lineJ, ...
    -SEVolt(lineJ).*( ...
    lineG.*cos(SEVAngel(lineI)-SEVAngel(lineJ)) +lineB.*sin(SEVAngel(lineI)-SEVAngel(lineJ))...
    )... 
    +2*(lineG).*SEVolt(lineI) ...
    ,length(lineI),length(mVolt));%<EFBFBD><EFBFBD>·<EFBFBD><EFBFBD>
dLPij_dVj=sparse(1:length(lineI),lineJ, ...
    -SEVolt(lineI).*( ...
    lineG.*cos(SEVAngel(lineI)-SEVAngel(lineJ)) +lineB.*sin(SEVAngel(lineI)-SEVAngel(lineJ))...
    ) ...
    ,length(lineI),length(mVolt));%<EFBFBD><EFBFBD>·<EFBFBD><EFBFBD>
dLPij_dThetai=sparse(1:length(lineI),lineJ, ...
    SEVolt(lineI).*SEVolt(lineJ).*( ...
    lineG.*sin(SEVAngel(lineI)-SEVAngel(lineJ)) -lineB.*cos(SEVAngel(lineI)-SEVAngel(lineJ))...
    )... 
    ,length(lineI),length(mVolt));%<EFBFBD><EFBFBD>·<EFBFBD><EFBFBD>
dLPij_dThetaj=sparse(1:length(lineI),lineJ, ...
    -SEVolt(lineI).*SEVolt(lineJ).*( ...
    lineG.*sin(SEVAngel(lineI)-SEVAngel(lineJ)) -lineB.*cos(SEVAngel(lineI)-SEVAngel(lineJ))...
    )... 
    ,length(lineI),length(mVolt));%<EFBFBD><EFBFBD>·<EFBFBD><EFBFBD>

dLQij_dVi=sparse(1:length(lineI),lineJ, ...
    -SEVolt(lineJ).*( ...
    lineG.*cos(SEVAngel(lineI)-SEVAngel(lineJ)) -lineB.*sin(SEVAngel(lineI)-SEVAngel(lineJ))...
    )... 
    -2*(lineB+lineB2).*SEVolt(lineI) ...
    ,length(lineI),length(mVolt));%<EFBFBD><EFBFBD>·<EFBFBD><EFBFBD>
dLQij_dVj=sparse(1:length(lineI),lineJ, ...
    -SEVolt(lineI).*( ...
    lineG.*sin(SEVAngel(lineI)-SEVAngel(lineJ)) -lineB.*cos(SEVAngel(lineI)-SEVAngel(lineJ))...
    ) ...
    ,length(lineI),length(mVolt));%<EFBFBD><EFBFBD>·<EFBFBD><EFBFBD>
dLQij_dThetai=sparse(1:length(lineI),lineJ, ...
    -SEVolt(lineI).*SEVolt(lineJ).*( ...
    lineG.*cos(SEVAngel(lineI)-SEVAngel(lineJ)) +lineB.*sin(SEVAngel(lineI)-SEVAngel(lineJ))...
    ) ...
    ,length(lineI),length(mVolt));%<EFBFBD><EFBFBD>·<EFBFBD><EFBFBD>
dLQij_dThetaj=sparse(1:length(lineI),lineJ, ...
    SEVolt(lineI).*SEVolt(lineJ).*( ...
    lineG.*cos(SEVAngel(lineI)-SEVAngel(lineJ)) +lineB.*sin(SEVAngel(lineI)-SEVAngel(lineJ))...
    ) ...
    ,length(lineI),length(mVolt));%<EFBFBD><EFBFBD>·<EFBFBD><EFBFBD>
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>翹Ϊ0<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>NaN
zerosRXInd=union(find(abs(lineR)<1e-5),find(abs(lineX)<1e-5));
dLPij_dVi=dLPij_dVi(setdiff( 1:size(dLPij_dVi,1),zerosRXInd),:);
dLPij_dVj=dLPij_dVj(setdiff( 1:size(dLPij_dVj,1),zerosRXInd),:);
dLPij_dThetai=dLPij_dThetai(setdiff( 1:size(dLPij_dThetai,1),zerosRXInd),:);
dLPij_dThetaj=dLPij_dThetaj(setdiff( 1:size(dLPij_dThetaj,1),zerosRXInd),:);
dLQij_dVi=dLQij_dVi(setdiff( 1:size(dLQij_dVi,1),zerosRXInd),:);
dLQij_dVj=dLQij_dVj(setdiff( 1:size(dLQij_dVj,1),zerosRXInd),:);
dLQij_dThetai=dLQij_dThetai(setdiff( 1:size(dLQij_dThetai,1),zerosRXInd),:);
dLQij_dThetaj=dLQij_dThetaj(setdiff( 1:size(dLQij_dThetaj,1),zerosRXInd),:);
%% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
H=[dV_dV ,dV_dTyta;
  dLPij_dVi+dLPij_dVj, dLPij_dThetai+dLPij_dThetaj;
  dLQij_dVi+dLQij_dVj, dLQij_dThetai+dLQij_dThetaj];%jacobi
% H=[dV_dV];
h=SEVolt;
z=mVolt;
W=sparse(1:length(mVolt),1:length(mVolt),1/sigma.^2,length(mVolt),length(mVolt));
G=H'*W*H;
g=-H'*W*(z-h);
% ƽ<EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǻ㶨;
% G(length(mVolt)+Balance,:)=0;
% G(:,length(mVolt)+Balance)=0;
% G=G+sparse(length(mVolt)+Balance,length(mVolt)+Balance,1,length(mVolt)*2,length(mVolt)*2);
% g(length(mVolt)+Balance)=0;
dX=G\-g;
maxD=max(abs(dX))
% <EFBFBD><EFBFBD><EFBFBD>±<EFBFBD><EFBFBD><EFBFBD>
SEVolt=SEVolt+dX(1:length(mVolt));
Iteration=Iteration+1;
SEVAngel=SEVAngel+dX(length(mVolt)+1:end);
end
%% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
fprintf('<EFBFBD><EFBFBD><EFBFBD><EFBFBD>%d<EFBFBD><EFBFBD>\n',Iteration);
h=SEVolt;
fval=full((z-h)'*W*(z-h));
fprintf('Ŀ<EFBFBD>꺯<EFBFBD><EFBFBD>Ϊ:%f\n',fval);
fprintf('<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n');
(abs(rVolt-double(SEVolt)))./(rVolt)
MaxDeviation(rVolt,SEVolt,rVAngel,SEVAngel)
plotAndComparison( rVolt,rVAngel,SEVolt,SEVAngel )