clc

clear all

% Einlesen der Daten

opts = detectImportOptions('AWL_1520.csv');
opts.EmptyLineRule = 'read';
t = readtable('AWL_1520.csv'); %gesamte Messung t

t = t{:,:}; %Erstellen Matrix aus t
% Aufsplitten der Daten in Epochen

ne999 = find(t(:,1)~=999)';                             % Nonzero Elements (transposed)
ix0 = unique([ne999(1) ne999(diff([999 ne999])>1)]);        % Segment Start Indices
ix1 = ne999([find(diff([999 ne999])>1)-1 length(ne999)]);   % Segment End Indices
for k1 = 1:length(ix0)
Epoche{k1} = t(ix0(k1):ix1(k1),:);             % (Included the column)
end

k = Epoche;




for k = 1:1;




% Designmatrix aufstellen

azi = Epoche{1,k}(1:end,1); %Azimut


ele = Epoche{1,k}(1:end,2); %Elevation


C = Epoche{1,k}(1:end,3);%Residuen


S = Epoche{1,k}(1:end,4); %S/N


Uhr = Epoche{1,k}(1:end,5:7);%Uhr

%A-Matrix

Nord = cosd(azi).*cosd(ele); %Nordwerte

Ost = sind(azi).*cosd(ele); %Ostwerte

H = sind(ele); %Hoehe

A = horzcat (Nord,Ost,H,Uhr); %Verkettung in A-Matrix



A = A;

ele2 = ele;
ele2(ele <= 0) = 10;
ele2(ele > 0 & ele <= 60) = 1 ./ 1.103 .* sind(ele(ele > 0 & ele <= 60) + 5);
ele2(ele > 60) = 1;

ele3 = ele2.^2;

ele4 = 1 ./ ele3;

P = diag(ele4);

l = C;

N = A.'*P*A;

x = N^-1*A.'*P*l;

v = A*x-l;

n = length(A);
u = 6;

s(k) = sqrt((v'*P*v)/(n-u));


% Iterative Berechnung



s0 = 0.1;

sabsolut = ele2 .* s0;



wi = v./sabsolut;

a = 1;
b = 3;
c = 13;




min = 0.0001;
zaehler = 0;

Normliste = zeros(100,1);

Pm = P; 




for m = 1:500;

 
    
N = A.'*Pm*A;

xm = N^-1*A.'*Pm*l;

xmnorm = norm(xm);

Normliste(m) = xmnorm;

vm = A*xm-l;

wi = vm./sabsolut;

gwi = abs(wi);

Liste = zeros(length(gwi),1);
Liste2 = zeros(length(gwi),1);





for o = 1:length(gwi)
    
    if gwi(o) < a;
       Liste(o) = 1;
       Liste2(o)= 1;
      
       

       
    elseif gwi(o) >=a & gwi(o) < b;
       Liste(o) = a / gwi(o);
     Liste2(o)= 2;
      

    
    elseif gwi(o) >=b & gwi(o) < c;
        Liste(o) =  a * ((c-gwi(o))/((c-b)*gwi(o)));
         Liste2(o)= 3;
        

    else gwi(o) >= c;
        Liste(o) = 0;
         Liste2(o)= 4;
       

        
    end
end




zaehler = zaehler +1;


if m >= 2
    if abs(Normliste(m)-Normliste(m-1)) < min
        break
    end
end
Gm = Liste;
Pm = P.*Gm;


end

y(:,k) = xm;



end 

y(4:6,:) = [];
y(:, all(isnan(y)) ) = []; 

N = y(1,:);
Nordfehler = std(N)

O = y(2,:);
Ostfehler = std(O)

H = y(3,:);
Hoehenfehler =std(H)