%State estimation with gaussian distribution
clear all;clc;
MAX_ITER=30;%Number of maximum iteration for SE
char sol_time;%solution time for SE
tol=1e-6;%tolerance for state estimation solution

input_data;
if lf_flag==0
    return;
end

%Read measurement data for SE from 'meas_data.xlsx'
%Sheet       |  variable
%=======================================================================
%linedata    | [line]
%bus_shunt   | [bus_shunt]
%bus_meas    | [meas_bus] ->meas_value meas_type bus_no
%line_meas   | [line_meas]->meas_value meas_type line_no from_bus to_bus
%zero_inj    | z_Bus_no z_type(z_Pinj=1 & z_Qinj=1)
%zero_flow   | z_type(Z_Pflow=1 &z_Qflow=2) z_line_no from_bus to_bus
%
%%meas_type bus | PINJECTION->1 , QINJECTION->2 , VABS->3 , VANG_RAD->4
%%meas_type line| PFLOW->1 , QFLOW->2 , IABS->3 , IANG_RAD->4 IREAL->5 , 
%               | IIMAG->6
%
%OUTPUT      | line bus_shunt z_Pinj z_Qinj z_Pflow z_Qflow
%            | meas_bus meas_line
read_meas_data;
if flag_return==1
    return;
end

% Formation of YBUS and line admittance parameter in pi model
[YBUS]=y_sparse(line,nob,nol,bus_shunt);
GBUS=real(YBUS);BBUS=imag(YBUS);
[line_pi]=process_line(line);
[bus_connect,connect_m]=get_connect(line,nob,nol);

%Separate all measurements for SE
%INPUT:  meas_bus   meas_line flag_error line nol
%OUTPUT: m_Pinj m_Qinj m_Pflow m_Qflow m_VMA m_IMA m_IRI slack_ang
%        each variable m_* has three col: col1: meas_index col2:meas_value 
%        and col3 as variance
get_meas4SE;

% check_observability_main;%obs analysis for different SE
if flag_SE==2
    % ALL PMU Measurement based SE
    %check observability
    [L_unob,flag_obs]=check_obs_PMUSE(m_VMA,m_IMA,m_IRI,line_pi,nob); 
    if flag_obs==1
        disp('PMU ONLY LINEAR SE OBSERVABILITY PASSED');
    else
        disp('PMU ONLY LINEAR SE OBSERVABILITY FAILED');
        break;
    end
    %x_est_PMU: column 1:4 ->abs_V ang_V real_V imag_V in order
    [x_est_PMU]=SE_WLS_PMU(m_VMA,m_IMA,m_IRI,flag_PMUSE,nob,line_pi);
%     disp('   V_abs V_angle_radian Vreal    Vimag');
%     disp(x_est_PMU);
elseif flag_SE==3
    if flag_PMU==2
        %POST PROCESSING OF PMU DATA i.e. Sequential SE
        %Stage I output:x_est_conv and stage II Output:x_est_II
        %both output has 4 column, abs_V ang_V real_V imag_V in order
        SE_WLS_sequential;%OUTPUT x_est_conv and x_est_II
        %y_test;
%         disp('   V_abs V_angle_radian Vreal    Vimag');
%         disp(x_est_II);
    else
        %PRE PROCESSING
        %PMU voltage index for observability check after converting I->V
        [t_obs_V]=get_PMUindex_obs(m_VMA,m_IRI,m_IMA,line,nob,nol);
        
        %Hybrid SE Observability check
        t_obs_inj=m_Pinj(:,1);
        if flag_eq==1
            [t_obs_inj]=[t_obs_inj;z_Pinj];
        end
        [L_unob,flag_obs]=check_obs_HSE(line,connect_m,t_obs_inj,m_Pflow(:,1),t_obs_V,nob);
        if flag_obs==1
            disp('HYBRID SE OBSERVABILITY PASSED');
        else
            disp('HYBRID SE OBSERVABILITY FAILED');
            break;
        end
        
        %OUTPUT x_est_HSE: column 1:4 ->abs_V ang_V real_V imag_V in order
        SE_WLS_HSE;
%         disp('   V_abs V_angle_radian Vreal    Vimag');
%         disp(x_est_HSE);
    end
else
    %Conventional SE Observability check
    t_obs_inj=m_Pinj(:,1);
    if flag_eq==1
        [t_obs_inj]=[t_obs_inj;z_Pinj];
    end
    [L_unob,flag_obs]=check_obs_CSE(line,connect_m,t_obs_inj,m_Pflow(:,1),nob);
    if flag_obs==1
        disp('CONVENTIONAL SE OBSERVABILITY PASSED');
    else
        disp('CONVENTIONAL SE OBSERVABILITY FAILED');
        break;
    end
    
    %OUTPUT as x_est_conv: column 1:4 ->abs_V ang_V real_V imag_V in order
    SE_WLS_conv_polar;
%     disp('   V_abs V_angle_radian Vreal    Vimag');
%     disp(x_est_conv);
    clear E gain_t;
    clear E gain_t;
end

output_display;