按Power System State Estimation: Theory and Implementation by Ali Abur书中内容添加等式约束，虽然没看懂迭代公式是怎么来的。由于大系统的目标函数离0较远，使得Gauss-Norton的收敛性不好。

Signed-off-by: facat <facat@facat.cn>

run.m

@@ -63,6 +63,7 @@ mQG=rQG.*(normrnd(0,sigma,length(rQG),1)+1);
 %% 0注入节点
 zerosInjectionIndex=1:length(Volt);
 zerosInjectionIndex=zerosInjectionIndex( ~(PD~=0|QD~=0|PG~=0|QG~=0) );
+% zerosInjectionIndex=zeros(0,0);
 %% 发电机注入功率
 % 先找到只有发电机的节点
 PDQDi=union(PDi,QDi);
@@ -102,9 +103,11 @@ SEVolt(Balance)=rVolt(Balance);
 SEVAngle=sparse(zeros(length(mVolt),1));
 maxD=1000;
 Iteration=0;
-g=100;
-while max(abs(g))>1e-5;
-% while g>1e-5
+optimalCondition=100;
+eps=1e-4;
+% while max(abs(g))>1e-5;
+% while maxD>1e-5
+while max(abs(optimalCondition))>eps
     % 电压
     dV_dV=sparse(1:length(mVolt),1:length(mVolt),1,length(mVolt),length(mVolt));%电压量测量的微分
     dV_dTyta=sparse(length(mVolt),length(mVolt));
@@ -195,6 +198,35 @@ while max(abs(g))>1e-5;
         transG.*cos(SEVAngle(transI)-SEVAngle(transJ)) +transB.*sin(SEVAngle(transI)-SEVAngle(transJ))...
         ) ...
         ,length(transI),length(mVolt));%变压器
+    %% 考虑等式约束
+    % 等式约束的Jacobi
+    r=newwordParameter.r;
+    c=newwordParameter.c;
+    Yangle=newwordParameter.Yangle;
+    VAngleIJ=sparse(r,c,SEVAngle(r)-SEVAngle(c) -Yangle,length(mVolt),length(mVolt)) ;
+    YdotSin=Y.* ( spfun(@sin,VAngleIJ) );
+    YdotCos=Y.* ( spfun (@cos, VAngleIJ ) );
+    diag_Volt_YdotCos=diag(SEVolt)*YdotCos;
+    diag_Volt_YdotSin=diag(SEVolt)*YdotSin;
+    YdotCosVolt=YdotCos*Volt;
+    YdotSinVolt=YdotSin*Volt;
+    diag_Volt_YdotCosVolt=diag_Volt_YdotCos*Volt;
+    diag_Volt_YdotSinVolt=diag_Volt_YdotSin*Volt;
+    diag_YdotSinVolt_=diag(YdotSinVolt);
+    diag_YdotCosVolt_=diag(YdotCosVolt);
+    dPdTyta=diag_Volt_YdotSin*diag(SEVolt)-diag_YdotSinVolt_*diag(SEVolt); % 简化第三次
+    dQdTyta=-diag_Volt_YdotCos*diag(SEVolt)+diag_YdotCosVolt_*diag(SEVolt);%dQ/dThyta
+    dPdV=diag_YdotCosVolt_+diag_Volt_YdotCos;%dP/dV
+    dQdV=diag_YdotSinVolt_+diag_Volt_YdotSin;%dQ/dV
+    % C 是等式约束 c 的Jacobi
+    C=[dPdV dPdTyta;
+        dQdV dQdTyta];
+    C=C(zerosInjectionIndex,:);
+    % 形成等式约束 c
+    nodeP=diag_Volt_YdotCosVolt;
+    nodeQ=diag_Volt_YdotSinVolt;
+    nodePQ=[nodeP;nodeQ];
+    c=nodePQ(zerosInjectionIndex);
     %% 进入迭代
     H=[dV_dV,dV_dTyta;
         dLPij_dVi+dLPij_dVj,dLPij_dThetai+dLPij_dThetaj ;
@@ -211,26 +243,35 @@ while max(abs(g))>1e-5;
     z=[mVolt;mBranchP;mBranchQ;mTransP;mTransQ];
     G=H'*W*H;
     g=-H'*W*(z-h);
-    % 平衡节点相角恒定;
-    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;
+   
+    % 形成大的求解矩阵
+    a=[G C';
+        C zeros(size(C,1),size(C,1))];
+    b=[-g;-c];
+    % 平衡节点相角恒定
+    a(length(mVolt)+Balance,:)=0;
+    a(:,length(mVolt)+Balance)=0;
+    a=a+sparse(length(mVolt)+Balance,length(mVolt)+Balance,1,size(a,1),size(a,1));
+    b(length(mVolt)+Balance)=0;
     % 平衡节点电压恒定;
-    G(Balance,:)=0;
-    G(:,Balance)=0;
-    G=G+sparse(Balance,Balance,1,length(mVolt)*2,length(mVolt)*2);
-    g(Balance)=0;
-    % 求解修正量
-    dX=G\-g;
+    a(Balance,:)=0;
+    a(:,Balance)=0;
+    a=a+sparse(Balance,Balance,1,size(a,1),size(a,1));
+    b(Balance)=0;
+    dX=a\b;
     dXStep=1;
 %     dXStep=Armijo(z,newwordParameter,W,SEVolt,SEVAngle,dX,g );
-    maxD=max(abs(dX))
+    maxD=max(abs(dX(1:length(mVolt))))
     fprintf('max abs g:%f\n',full(max(abs(g))));
     % 更新变量
     SEVolt=SEVolt+dX(1:length(mVolt))*dXStep;
     Iteration=Iteration+1;
-    SEVAngle=SEVAngle+dX(length(mVolt)+1:end)*dXStep;
+    SEVAngle=SEVAngle+dX(length(mVolt)+1:length(mVolt)*2)*dXStep;
+    lamda=-dX(length(mVolt)*2+1:end);
+    optimalCondition=[-g+C'*lamda;
+        c];
+    optimalCondition(Balance)=0;
+    optimalCondition(Balance+length(mVolt))=0;
 end
 %% 输出结果
 fprintf('迭代%d次\n',Iteration);
@@ -241,7 +282,8 @@ fprintf('
 MaxDeviation(rVolt,SEVolt,rVAngel,SEVAngle)
 plotAndComparison( rVolt,rVAngel,SEVolt,SEVAngle )
 % 检查最优性条件
-if any(abs(g)>1e-5)
+
+if any(abs(optimalCondition)>eps)
     fprintf('最优性条件不满足\n')
 else
     fprintf('最优性条件满足\n')