clc
clear
clear
% yalmip('clear')
tic
[kmax,Precision,UAngel,Volt,Busnum,PVi,PVu,Balance,Y,Angle,P0,Q0,r,c,GB, ...
    Linei,Linej,Transfori,Transforj,GenU,GenL,GenC,PG,QG,PD,QD,CenterA,PGi,PVQU,PVQL, ...
    Liner,Linex,Lineb,Transforr,Transforx,Transfork0]= ...
    pf('E:/算例/新民Ⅰ906_2729823_2012-09-06/newFIle20.txt');

%% 潮流等式
AngleIJ=sparse(r,c,UAngel(r)-UAngel(c)-Angle',Busnum,Busnum);
PGBal=PD+diag(Volt)*Y.*cos(AngleIJ)*Volt';
QGBal=QD+diag(Volt)*Y.*sin(AngleIJ)*Volt';
%% 初值-即测量值
PG0=PG;
QG0=QG;
PD0=PD;
QD0=QD;
PDReal=PD;%真值
QDReal=QD;%真值
PG0(Balance)=PGBal(Balance);
QG0(Balance)=QGBal(Balance);
QG0(PVi)=QGBal(PVi);
PG(Balance)=PGBal(Balance);
QG(PVi)=QGBal(PVi);
Volt0=Volt;
% PF=1;
% AngleIJ=sparse(r,c,UAngel(r)-UAngel(c)-Angle',Busnum,Busnum);
% dP=PG-PD-diag(Volt)*Y.*cos(AngleIJ)*Volt';
% dPD=abs(dP./PD);
% dQ=QG-QD-diag(Volt)*Y.*sin(AngleIJ)*Volt';
% dQD=abs(dQ./QD);
% maxdPQ=max([dPD(dPD<10);dQD(dQD<10)]);
xVolt=Volt;
xUAngel=UAngel;
% VMatrix=sparse(r,c,UAngel(r)-UAngel(c)-Angle',Busnum,Busnum);
% dP=PG-PD-diag(xVolt)*(Y.*cos(VMatrix))*xVolt';
rPD=PD;
rQD=QD;
rVolt=Volt';
rVAngel=UAngel';
BalVolt=Volt(Balance);
[Volt,UAngel,Init_Z,Init_W,Init_L,Init_U,Init_Y,PG,QG,RestraintCount,wPG,wQG,wPD,wQD,wVolt,wLoadCurrent,PD,PD0,QD,randPDind,Loadi,notLoadi]=OPF_Init(Busnum,Balance,PG,QG,Volt,GenU,GenL,PVi,PGi,PVQU,PVQL,PD0,QD0,QD,PD);

%% 加噪音
load('20PD0');
load('20QD0');
mPD=PD0;
mQD=QD0;
mVolt=(1+normrnd(0,0.05,length(rVolt),1)).*rVolt;
% PD0=(1+normrnd(0,0.05,length(PD0),1)).*PD0;
% save('PD0','PD0');
% QD0=(1+normrnd(0,0.05,length(QD0),1)).*QD0;
% save('QD0','QD0');
%% 目标函数

%% Opti Toolbox
Busnum=length(Volt);
PDi=find(PD~=0);
QDi=find(QD~=0);
% PD0=PD0(PDi);
% QD0=QD0(QDi);
%% 电流真实值
rLoadCurrent=LoadCurrent( rVolt,rVAngel,rPD(PDi),rQD(QDi),PDi,QDi );
%% 电流测量值
mLoadCurrent=(1+normrnd(0,0.05,length(rLoadCurrent),1)).*rLoadCurrent;
seOpti=Opti();
seOpti=seOpti.init(mVolt,PDi,QDi,wPD,wQD,wVolt,wLoadCurrent,mPD,mQD,rPD(PDi),rQD(QDi),Y,Angle,r,c,PG,QG,Balance,mLoadCurrent);
opts = optiset('solver','ipopt');
opts.maxiter=85500;
opts.maxtime=30000;
opts.maxfeval=85000;
opts.maxnodes=85000;
opts.tolrfun=1e-4;
opts.tolafun=1e-4;
opts.warnings='all';
opts.display='iter';
x0=[0.8*rPD(PDi);0.8*rQD(QDi); ...
    ones(length(Volt),1); ...
    zeros(length(Volt),1)];
% x0=[PD(PDi);QD(QDi);xVolt';xUAngel'];
[~,seOpti]=seOpti.equ(x0);
cl=seOpti.Getcl();
cu=seOpti.Getcu();
Opt = opti('fun',@seOpti.obj,'ndec',length(Volt)*2+length(PDi)+length(QDi),'nl',@seOpti.equ,cl,cu,'options',opts)
% Opt = opti('fun',@seOpti.obj,'ndec',length(Volt)*2+length(PDi)+length(QDi),'options',opts)
[x,fval,exitflag,info] = solve(Opt,x0);
info
fval=seOpti.obj(x);
fprintf('目标函数: %.20f\n',fval);
toc
rVolt=Volt0';
rVAngel=xUAngel';
SEVolt=x(length(PDi)+length(QDi)+1:length(Volt)+length(PDi)+length(QDi));
SEVAngel=x(length(Volt)+length(PDi)+length(QDi)+1:end);
fprintf('最大偏差\n')
PD=x(1:length(PDi));
QD=x(length(PDi)+1:length(PDi)+length(QDi));
MaxDeviation(rVolt,SEVolt,rVAngel,SEVAngel,rPD(PDi),rQD(QDi),PD,QD)
fprintf('统计偏差\n')
StatDeviation(rVolt,SEVolt,rVAngel,SEVAngel,rPD(PDi),rQD(QDi),PD,QD)
%% 约束检查
% seOpti.equ(x);
% sum([SEVolt;PD;QD]>cu(length(SEVolt)*2+2:end));
% sum([SEVolt;PD;QD]<cl(length(SEVolt)*2+2:end));