clear;
clc;
meas14;
lldata=linedata14;
Ybus=Lfybus(lldata);
G = real(Ybus);
B = imag(Ybus);
b = bbus(lldata);
%To know the number of the available measurements
nv=size(v_m,1);
npi=size(pi_m,1);
nqi=size(qi_m,1);
npf=size(pf_m,1);
nqf=size(qf_m,1);
nmeas=nv+npi+nqi+npf+nqf;

%To get number of buses
a=[max(v_m(:,1)) 
   max(pi_m(:,1)) 
   max(pi_m(:,1)) 
   max(qi_m(:,1)) 
   max(pf_m(:,1)) 
   max(pf_m(:,2)) 
   max(qf_m(:,1)) 
   max(qf_m(:,2))];
nbus=max(a);

%Assign the measured data to a variable z
z=[v_m(:,2);pi_m(:,2);qi_m(:,2);pf_m(:,3);qf_m(:,3)];

%The measurment errors
errors=[v_m(:,4);pi_m(:,3);qi_m(:,3);pf_m(:,4);qf_m(:,4)];
Ri = diag(errors); % Measurement Error..
v = ones(nbus,1); % Initialize the bus voltages..
del = zeros(nbus,1); % Initialize the bus angles..

for i=1:nv
    del(v_m(i,1))=v_m(i,3)*pi/180;
    if v_m(i,3)==0
        s=v_m(i,1)  %slack bus number
    end
end
    
x= [del(2:nbus); v];   % Initial State Vector..


while(1)
   
    %Measurement Function, h
    h1 = v(v_m(:,1),1); %get v from the initial values
    h2 = zeros(npi,1);
    h3 = zeros(nqi,1);
    h4 = zeros(npf,1);
    h5 = zeros(nqf,1);
    
    %Calculating h
    
    for i = 1:npi
        m = pi_m(i,1);  %to get number from first column of pi_m
        for k = 1:nbus
            h2(i) = h2(i) + v(m)*v(k)*(G(m,k)*cos(del(m)-del(k)) + B(m,k)*sin(del(m)-del(k)));
        end
    end
    
    for i = 1:nqi
        m = qi_m(i,1);
        for k = 1:nbus
            h3(i) = h3(i) + v(m)*v(k)*(G(m,k)*sin(del(m)-del(k)) - B(m,k)*cos(del(m)-del(k)));
        end
    end
    
    for i = 1:npf
        m = pf_m(i,1);
        n = pf_m(i,2);
        h4(i) = -v(m)^2*G(m,n) - v(m)*v(n)*(-G(m,n)*cos(del(m)-del(n)) - B(m,n)*sin(del(m)-del(n)));
    end
    
    for i = 1:nqf
        m = qf_m(i,1);
        n = qf_m(i,2);
        h5(i) = -v(m)^2*(-B(m,n)+b(m,n)) - v(m)*v(n)*(-G(m,n)*sin(del(m)-del(n)) + B(m,n)*cos(del(m)-del(n)));
    end
    
    h = [h1; h2; h3; h4; h5];
    
     % Residue..
    r = z - h;
    
     % Jacobian..
    % H11 - Derivative of V with respect to angles.. All Zeros
    H11 = zeros(nv,nbus-1);
    
        % H12 - Derivative of V with respect to V.. 
    H12 = zeros(nv,nbus);
    for k = 1:nv
        for n = 1:nbus
            if n == k
                H12(k,n) = 1;
            end
        end
    end
    
    % H21 - Derivative of Real Power Injections with Angles..
    H21 = zeros(npi,nbus-1);
    for i = 1:npi
        m = pi_m(i,1);
        for k = 1:(nbus-1)
            if k+1 == m
                for n = 1:nbus
                    H21(i,k) = H21(i,k) + v(m)* v(n)*(-G(m,n)*sin(del(m)-del(n)) + B(m,n)*cos(del(m)-del(n)));
                end
                H21(i,k) = H21(i,k) - v(m)^2*B(m,m);
            else
                H21(i,k) = v(m)* v(k+1)*(G(m,k+1)*sin(del(m)-del(k+1)) - B(m,k+1)*cos(del(m)-del(k+1)));
            end
        end
    end
    
      % H22 - Derivative of Real Power Injections with V..
    H22 = zeros(npi,nbus);
    for i = 1:npi
        m = pi_m(i,1);
        for k = 1:nbus
            if k == m
                for n = 1:nbus
                    H22(i,k) = H22(i,k) + v(n)*(G(m,n)*cos(del(m)-del(n)) + B(m,n)*sin(del(m)-del(n)));
                end
                H22(i,k) = H22(i,k) + v(m)*G(m,m);
            else
                H22(i,k) = v(m)*(G(m,k)*cos(del(m)-del(k)) + B(m,k)*sin(del(m)-del(k)));
            end
        end
    end
    
    
       % H31 - Derivative of Reactive Power Injections with Angles..
    H31 = zeros(nqi,nbus-1);
    for i = 1:nqi
        m = qi_m(i,1);
        for k = 1:(nbus-1)
            if k+1 == m
                for n = 1:nbus
                    H31(i,k) = H31(i,k) + v(m)* v(n)*(G(m,n)*cos(del(m)-del(n)) + B(m,n)*sin(del(m)-del(n)));
                end
                H31(i,k) = H31(i,k) - v(m)^2*G(m,m);
            else
                H31(i,k) = v(m)* v(k+1)*(-G(m,k+1)*cos(del(m)-del(k+1)) - B(m,k+1)*sin(del(m)-del(k+1)));
            end
        end
    end
    
     % H32 - Derivative of Reactive Power Injections with V..
    H32 = zeros(nqi,nbus);
    for i = 1:nqi
        m = qi_m(i,1);
        for k = 1:nbus
            if k == m
                for n = 1:nbus
                    H32(i,k) = H32(i,k) + v(n)*(G(m,n)*sin(del(m)-del(n)) - B(m,n)*cos(del(m)-del(n)));
                end
                H32(i,k) = H32(i,k) - v(m)*B(m,m);
            else
                H32(i,k) = v(m)*(G(m,k)*sin(del(m)-del(k)) - B(m,k)*cos(del(m)-del(k)));
            end
        end
    end
    
        % H41 - Derivative of Real Power Flows with Angles..
    H41 = zeros(npf,nbus-1);
    for i = 1:npf
        m = pf_m(i,1);
        n = pf_m(i,2);
        for k = 1:(nbus-1)
            if k+1 == m
                H41(i,k) = v(m)* v(n)*(-G(m,n)*sin(del(m)-del(n)) + B(m,n)*cos(del(m)-del(n)));
            else if k+1 == n
                H41(i,k) = -v(m)* v(n)*(-G(m,n)*sin(del(m)-del(n)) + B(m,n)*cos(del(m)-del(n)));
                else
                    H41(i,k) = 0;
                end
            end
        end
    end
    
    % H42 - Derivative of Real Power Flows with V..
    H42 = zeros(npf,nbus);
    for i = 1:npf
        m = pf_m(i,1);
        n = pf_m(i,2);
        for k = 1:nbus
            if k == m
                H42(i,k) = -v(n)*(-G(m,n)*cos(del(m)-del(n)) - B(m,n)*sin(del(m)-del(n))) - 2*G(m,n)*v(m);
            else if k == n
                H42(i,k) = -v(m)*(-G(m,n)*cos(del(m)-del(n)) - B(m,n)*sin(del(m)-del(n)));
                else
                    H42(i,k) = 0;
                end
            end
        end
    end
    
    
        % H51 - Derivative of Reactive Power Flows with Angles..
    H51 = zeros(nqf,nbus-1);
    for i = 1:nqf
        m = qf_m(i,1);
        n = qf_m(i,2);
        for k = 1:(nbus-1)
            if k+1 == m
                H51(i,k) = -v(m)* v(n)*(-G(m,n)*cos(del(m)-del(n)) - B(m,n)*sin(del(m)-del(n)));
            else if k+1 == n
                H51(i,k) = v(m)* v(n)*(-G(m,n)*cos(del(m)-del(n)) - B(m,n)*sin(del(m)-del(n)));
                else
                    H51(i,k) = 0;
                end
            end
        end
    end
    
        % H52 - Derivative of Reactive Power Flows with V..
    H52 = zeros(nqf,nbus);
    for i = 1:nqf
        m = qf_m(i,1);
        n = qf_m(i,2);
        for k = 1:nbus
            if k == m
                H52(i,k) = -v(n)*(-G(m,n)*sin(del(m)-del(n)) + B(m,n)*cos(del(m)-del(n))) - 2*v(m)*(-B(m,n)+ b(m,n));
            else if k == n
                H52(i,k) = -v(m)*(-G(m,n)*sin(del(m)-del(n)) + B(m,n)*cos(del(m)-del(n)));
                else
                    H52(i,k) = 0;
                end
            end
        end
    end
    
     % Measurement Jacobian, H..
    H = [H11 H12; H21 H22; H31 H32; H41 H42; H51 H52];
    %Ob = rank(H);
    
    %solve for dx using peters & wilinson method
    %Abur's book page 61
%     H1=Ri^-0.5*H;
%     r1=Ri^-0.5*r;
%     [L,U]=lu(H1);
%     dy=inv(L'*L)*L'*r1;
%     dx=inv(U)*dy;
    

    % solve for dx using orthognal factorization and hybrid method
    %Abur's book page 60
%     H1=Ri^-0.5*H;
%     r1=Ri^-0.5*r;
%     [Qn,U] = qr(H1,0);
%     dzq=Qn'*r1;
%     dx=inv(U)*dzq;
    
    % Solve for dx using Gain Matrix, Gm and normal equations
    Gm = H'*inv(Ri)*H;
    dx = inv(Gm)*(H'*inv(Ri)*r); % from equation 2.12 page 34

    x = x+ dx;
    del(2:end) = x(1:nbus-1);
    v = x(nbus:end);
    % Check for solution accuracy "epslon, step no. 6 at page 36
    accuracy = max(abs(dx));
    if accuracy < 1e-2
        break;
    end

end
 
%to know the critical measurements Abur's book 119
K=H*inv(Gm)*H'*inv(Ri);
S=eye(length(K))-K;
omega=S*Ri;
omega_sum=sum(omega);   %to add matrix columns to check critical measurements if sum=0

%to detect bad data for a confidence degree of 95%
%Objective Function..
J = sum(inv(Ri)*r.^2);
DF=nmeas-nbus;      %degree of freedom
p=chi2cdf(J,DF)*100;
if p>95
    disp('Bad data has been suspected');
else
    disp('No bad data');
end

%identify bad data in measurements using normalized residuals Abur's page
%127

for i=1:nmeas
    r_n(i)=abs(r(i))/omega(i,i);  %calculate normalized residuals
end
r_n;
% for i=1:nmeas
%     for j=i+1:nmeas
%         if abs(r_n(i))>abs(r_n(j))
%             k=i
%             break;
%         end
%     end
% end

Del = 180/pi*del;
E2 = [v Del]; % Bus Voltages and angles..
v
Del
disp('-------- State Estimation ------------------');
disp('--------------------------');
disp('| Bus |    V   |  Angle  | ');
disp('| No  |   pu   |  Degree | ');
disp('--------------------------');
for m = 1:nbus
    fprintf('%4g', m); fprintf('  %8.4f', v(m)); fprintf('   %8.4f', Del(m)); fprintf('\n');
end
disp('---------------------------------------------');
