%WLS Estimation SEQUENTIAL
%Refer 
%1) A G Phadke etal. "An alternative for including phasor measurements in
%   state estimators," IEEE Trans on power systems, vol 21, no 4, Nov 
%   2006, pp 1930-1937

%2) R Sodhi, S C Srivastava and S N Singh, "Phasor-assisted hybrid state 
%   estimator," Electric power components and systems, 2010, vol 38, pp 
%   533-544

%Stage I Conventional
%================================
%Conventional SE Observability check
t_obs_conv=m_Pinj(:,1);
if flag_eq==1
    [t_obs_conv]=[t_obs_conv;z_Pinj];
end
[L_unob,flag_obs]=check_obs_CSE(line,connect_m,t_obs_conv,m_Pflow(:,1),nob);
if flag_obs==1
    disp('SEQUENTIAL SE STAGE I OBSERVABILITY PASSED');
else
    disp('SEQUENTIAL SE STAGE I OBSERVABILITY FAILED');
    break;
end
clear t_obs_conv L_unob flag_obs;
    
SE_WLS_conv_polar;%output x_est_conv (abs angle real imag)
x_est_I=x_est_conv;clear x_est_conv;
%==============================================================

%Stage II SE : Conventional data
%=================================
%Evaluate ZII_conv=[VReal;VImag] (2*nob-1 X 1)
%RII_conv=[Rreal;Rimag] (2*nob-1 X 1)
%HII_conv=[Hreal_real Hreal_imag;Himag_real Himag_imag] (2*nob-1 X 2*nob-1)
if flag_eq==1
    %Evaluate E1 from inv(E)=[E1 E2;E3 E4]
    temp=inv(E);
    E1=temp(1:2*nob-1,1:2*nob-1);
    temp_var=E1*gain_t*E1';
    clear E1 temp;
else
    temp_var=inv(gain_t);
end

Rpolar=zeros(2*nob,1);%[Rang Rabs]
for i=1:2*nob-1
    if i<slack
        Rpolar(i,1)=temp_var(i,i);
    else
        Rpolar(i+1,1)=temp_var(i,i);
    end
end
RII_polar=[Rpolar(1:nob,1) Rpolar(nob+1:2*nob,1)];%[Rang Rabs]
[RII_conv,ZII_conv]=convert_polar2rect(RII_polar,x_est_I(:,1:2),slack);
HII_conv=[eye(nob) zeros(nob,nob-1);zeros(nob-1,nob) eye(nob-1)];
clear temp_var Rpolar RII_polar i;

%Stage II estimator: Post processing PMU data
%==============================================
% Evaluate ZII_V RII_V and HII_V from PMU Voltage measurement
[ZII_V,RII_V,HII_V]=eval_voltage_sequential(m_VMA,nob,slack);

%Evaluate ZII_I RII_I and HII_I from PMU current either in polar or
%rectangular form for SE
[ZII_I,RII_I,HII_I]=eval_current_sequential(m_IMA,m_IRI,line_pi,nob,slack);
%==================================================================

ZII=[ZII_conv;ZII_V;ZII_I];
JII=[HII_conv;HII_V;HII_I];
temp=[RII_conv;RII_V;RII_I];
RII=diag(temp);
% clear ZII_conv ZII_V ZII_IMA ZII_IRI JII_conv JII_V JII_IMA JII_IRI;
% clear RII_conv RII_V RII_IMA RII_IRI temp;

x_temp=inv(JII'*inv(RII)*JII)*JII'*inv(RII)*ZII;
if slack==nob
    temp=[x_temp;0];
else
    temp=[x_temp(1:nob+slack-1,1);0;x_temp(nob+slack:2*nob-1,1)];
end
VV=temp(1:nob,1)+1i*temp(nob+1:2*nob,1);
%output from stage II of sequential SE x_est_II (abs angle real imag)
x_est_II=[abs(VV) angle(VV) real(VV) imag(VV)];
clear x_temp temp VV;
