clc; clear all; close all; format long g;

%Bakalarska prace - vyrovnani volne site (c. 202)

%Autor: Kryštof Sedlák

%%%%%Vypocet

rad = pi/200;

%%%%Zadani

%%%Cislovani bodu
% 35514 = 1 opr
% 35515 = 2 opr
% 35517 = 3 opr
% 4003 = 4
% 2021 = 5
% 2022 = 6
% 2023 = 7
% 2024 = 8

Md = [4, 1, 161.950;
      4, 2, 145.007;
      4, 3, 199.378;
      4, 5,  44.995;
      4, 6,  37.000;
      4, 7,  68.435;
      4, 8,  37.002;
      6, 5,  81.989;
      6, 7,  92.178;
      7, 5,  62.288;
      8, 5,  70.449;
      8, 6,  37.205;
      8, 7,  105.436];

%   ZB NB Smer [gon];
Ms = [4, 1, 170.2490;
      4, 2,  55.6256;
      4, 3, 368.6123;
      4, 5,  73.7631;
      4, 6, 272.0782;
      4, 7,   4.1379;
      4, 8, 205.0057;
      5, 6,  74.4416;
      5, 7, 161.2231;
      5, 8,  44.5386;
      6, 5, 253.2250;
      6, 7, 207.2322;
      6, 8, 318.7664;
      7, 5, 352.4391;
      7, 6,  19.6634;
      7, 8, 397.0969;
      8, 5, 399.9989;
      8, 6, 295.4419;
      8, 7, 361.3418];

%Presnosti
PresS = 0.0006; %gon
PresD = 0.001; %m

PresSr = PresS*rad;

%Presnost pouzita pro charakteristiky presnosti
PCP = 0; %0 --> aposteriorni, 1 --> apriorni

%%%%Vypocet
n = length(Md(:, 3)) + length(Ms(:, 3)); %Pocet mereni
nd = length(Md(:, 3));
ns = length(Ms(:, 3));
r = 3; %Pocet podminek

%Vypocet pribliznych orientacnich posunu
POP4 = (smernik(606799.194, 1088055.631, 606872.160, 1087911.050) - 170.2490)*rad; %4 - 1
POP5 = (smernik(606840.421, 1088073.657, 606803.636, 1088123.922) - 161.2231)*rad; %5 - 7
POP6 = (smernik(606765.696, 1088039.917, 606796.289, 1088018.743) - 318.7664)*rad; %6 - 8
POP7 = (smernik(606803.636, 1088123.922, 606840.421, 1088073.657) - 352.4391)*rad; %7 - 5
POP8 = (smernik(606796.289, 1088018.743, 606765.696, 1088039.917) - 295.4419)*rad; %8 - 6

%Priblizne souradnice
x0 = [1087911.050;  %PS Bod 1
       606872.160;
      1088148.723;  %PS Bod 2
       606910.373;
      1088231.230;  %PS Bod 3
       606704.879;
      1088055.631;  %S Bod 4
       606799.194;
             POP4;  %POP Bod 4
      1088073.657;  %S Bod 5
       606840.421;
             POP5;  %POP Bod 5
      1088039.917;  %PS Bod 6
       606765.696;
             POP6;  %POP Bod 6
      1088123.922;  %PS Bod 7
       606803.636;
             POP7;  %POP Bod 7
      1088018.743;  %PS Bod 8
       606796.289;
             POP8];  %POP Bod 8
         
x02 = x0;

k = length(x0); %Pocet neznamych

%Derivace
syms X1 Y1 X2 Y2 OP

fd = sqrt((X2 - X1)^2 + (Y2 - Y1)^2);
fs = atan((Y2 - Y1)/(X2 - X1)) + OP;

fdX1 = diff(fd, X1);
fdY1 = diff(fd, Y1);
fdX2 = diff(fd, X2);
fdY2 = diff(fd, Y2);
fdOP = diff(fd, OP);

fsX1 = diff(fs, X1);
fsY1 = diff(fs, Y1);
fsX2 = diff(fs, X2);
fsY2 = diff(fs, Y2);
fsOP = diff(fs, OP);

sp = [X1; Y1; X2; Y2; OP];

%%%Cyklus
pod = 1;
poc = 0;

while pod == 1

%Kvadranty
while x0(9, 1) < 0
    x0(9, 1) = x0(9, 1) + 2*pi;
end
while x0(9, 1) > 2*pi
    x0(9, 1) = x0(9, 1) - 2*pi;
end

while x0(12, 1) < 0
    x0(12, 1) = x0(12, 1) + 2*pi;
end
while x0(12, 1) > 2*pi
    x0(12, 1) = x0(12, 1) - 2*pi;
end

while x0(15, 1) < 0
    x0(15, 1) = x0(15, 1) + 2*pi;
end
while x0(15, 1) > 2*pi
    x0(15, 1) = x0(15, 1) - 2*pi;
end

while x0(18, 1) < 0
    x0(18, 1) = x0(18, 1) + 2*pi;
end
while x0(18, 1) > 2*pi
    x0(18, 1) = x0(18, 1) - 2*pi;
end

while x0(21, 1) < 0
    x0(21, 1) = x0(21, 1) + 2*pi;
end
while x0(21, 1) > 2*pi
    x0(21, 1) = x0(21, 1) - 2*pi;
end

%Pocet iteraci
poc = poc + 1;

%Matice vah (P)
for i = 1:(nd)
    P(i, i) = 1/(PresD^2); %Delky
end

for i = (nd + 1):n
    P(i, i) = 1/(PresSr^2); %Smery
end

%Matice mereni (l)
for i = 1:(nd)
    l(i, 1) = Md(i, 3); %Delky
end

for i = (nd + 1):n
    l(i, 1) = Ms(i - nd, 3)*rad; %Smery
end

%Pomocna matice (MP)
MP = [x0(1, 1), x0(2, 1), 1, 2, 3, NaN;
      x0(3, 1), x0(4, 1), 4, 5, 6, NaN;
      x0(5, 1), x0(6, 1), 7, 8, 9, NaN;
      x0(7, 1), x0(8, 1), 10, 11, 12, x0(9, 1);
      x0(10, 1), x0(11, 1), 13, 14, 15, x0(12, 1);
      x0(13, 1), x0(14, 1), 16, 17, 18, x0(15, 1);
      x0(16, 1), x0(17, 1), 19, 20, 21, x0(18, 1);
      x0(19, 1), x0(20, 1), 22, 23, 24, x0(21, 1)];

%Matice pribliznych mereni (l0)
for i = 1:nd
    l0(i, 1) = sqrt(((MP(Md(i, 2), 1) - MP(Md(i, 1), 1))^2) + ((MP(Md(i, 2), 2) - MP(Md(i, 1), 2))^2));
end

for i = (nd + 1):n
    l0(i, 1) = (smernik(MP(Ms(i - nd, 1), 2), MP(Ms(i - nd, 1), 1), MP(Ms(i - nd, 2), 2), MP(Ms(i - nd, 2), 1))*rad) - MP(Ms(i - nd, 1), 6);
end

for i = (nd + 1):n
    while l0(i, 1) < 0
        l0(i, 1) = l0(i, 1) + (2*pi);
    end
    while l0(i, 1) > (2*pi)
        l0(i, 1) = l0(i, 1) - (2*pi);
    end
end

%Matice redukovanych mereni (lc)
lc = l0 - l;

for i = (nd + 1):n
    if l(i, 1) > 397*rad && l0(i, 1) < 3*rad
        lc(i, 1) = l0(i, 1) - (l(i, 1) - 400*rad);
    elseif l0(i, 1) > 397*rad && l(i, 1) < 3*rad
        lc(i, 1) = (l0(i, 1) - 400*rad) - l(i, 1);
    end
end

%Matice derivaci (A)
for i = 1:nd;
    %X1; Y1; X2; Y2; OP
    dp = [MP(Md(i, 1), 1); MP(Md(i, 1), 2); MP(Md(i, 2), 1); MP(Md(i, 2), 2); MP(Md(i, 1), 6)];
    A(i, MP(Md(i, 1), 3)) = eval(vpa(subs(fdX1, sp, dp))); %X1
    A(i, MP(Md(i, 1), 4)) = eval(vpa(subs(fdY1, sp, dp))); %Y1
    A(i, MP(Md(i, 1), 5)) = 0;
    A(i, MP(Md(i, 2), 3)) = eval(vpa(subs(fdX2, sp, dp))); %X2
    A(i, MP(Md(i, 2), 4)) = eval(vpa(subs(fdY2, sp, dp))); %Y2
    A(i, MP(Md(i, 2), 5)) = 0;
end

for i = (nd + 1):n;
    %X1; Y1; X2; Y2; OP
    dp = [MP(Ms(i - nd, 1), 1); MP(Ms(i - nd, 1), 2); MP(Ms(i - nd, 2), 1); MP(Ms(i - nd, 2), 2); MP(Ms(i - nd, 1), 6)];
    A(i, MP(Ms(i - nd, 1), 3)) = eval(vpa(subs(fsX1, sp, dp))); %X1
    A(i, MP(Ms(i - nd, 1), 4)) = eval(vpa(subs(fsY1, sp, dp))); %Y1
    A(i, MP(Ms(i - nd, 1), 5)) = -1; %OP
    A(i, MP(Ms(i - nd, 2), 3)) = eval(vpa(subs(fsX2, sp, dp))); %X2
    A(i, MP(Ms(i - nd, 2), 4)) = eval(vpa(subs(fsY2, sp, dp))); %Y2
    A(i, MP(Ms(i - nd, 2), 5)) = 0;
end

%Smazani neexistujicicch orientacnich posunu
A(:, 3) = [];
A(:, 5) = [];
A(:, 7) = [];

%Matice linearizovanych podminek (B')
Xt = (MP(1, 1) + MP(2, 1) + MP(3, 1))/3;
Yt = (MP(1, 2) + MP(2, 2) + MP(3, 2))/3;

Bt = zeros(3, k);
Bt(1, 1) = MP(1, 2) - Yt;
Bt(1, 2) = -(MP(1, 1) - Xt);
Bt(1, 3) = MP(2, 2) - Yt;
Bt(1, 4) = -(MP(2, 1) - Xt);
Bt(1, 5) = MP(3, 2) - Yt;
Bt(1, 6) = -(MP(3, 1) - Xt);
Bt(2, 1) = 1;
Bt(2, 3) = 1;
Bt(2, 5) = 1;
Bt(3, 2) = 1;
Bt(3, 4) = 1;
Bt(3, 6) = 1;

%Matice "B"
B = Bt';

%Matice "b"
b = zeros(3, 1);

%Matice A' P A / B'
ATPA = A' * P * A;
ATPABT = [ATPA' Bt']';

%Matice B 0
nul = zeros(3, 3);
B0 = [Bt nul']';

%Matice ATPAB
ATPAB = [ATPABT B0];
ATPAB = 0.5*(ATPAB + ATPAB');

%Matice ATPL' / b
ATPlc = A' * P * lc;
ATPlcb = [ATPlc' b']';

%Matice C1 - C3
ATPABinv = ATPAB^(-1);
C1 = ATPABinv(1:k, 1:k);

%Matice prirustku a korelatu (dxk)
dxk = -ATPAB^(-1) * ATPlcb;

%Matice prirustku (dx)
dx = dxk(1:k, 1);

%Nove priblizne souradnice (x0)
x0 = x0 + dx;
for i = 3:7;
    if x0(i*3, 1) > 2*pi
        x0(i*3, 1) = x0(i*3, 1) - 2*pi;
    elseif x0(i*3, 1) < 0
        x0(i*3, 1) = x0(i*3, 1) + 2*pi;
    end
end

%Opravy (v)
v = A*dx + lc;

%Vyrovnana mereni
lvyr = l + v;
lvyrg = lvyr;
lvyrg((nd + 1):end, 1) = lvyrg((nd + 1):end, 1)/rad;
for i = 1:length(lvyrg)
    if lvyrg(i, 1) > 400
        while lvyrg(i, 1) > 400
            lvyrg(i, 1) = lvyrg(i, 1) - 400;
        end
    elseif lvyrg(i, 1) < 0
        lvyrg(i, 1) = lvyrg(i, 1) + 400;
    end
end

%Konec cyklu
Mdx = [dx(1, 1),  dx(2, 1),  NaN;
       dx(3, 1),  dx(4, 1),  NaN;
       dx(5, 1),  dx(6, 1),  NaN;
       dx(7, 1),  dx(8, 1),  dx(9, 1);
       dx(10, 1), dx(11, 1), dx(12, 1);
       dx(13, 1), dx(14, 1), dx(15, 1);
       dx(16, 1), dx(17, 1), dx(18, 1);
       dx(19, 1), dx(20, 1), dx(21, 1)];

for i = 1:length(Md)
    delY = MP(Md(i, 1), 2) - MP(Md(i, 2), 2);
    delX =  MP(Md(i, 1), 1) - MP(Md(i, 2), 1);
    dx1 = Mdx(Md(i, 1), 1);
    dx2 = Mdx(Md(i, 2), 1);
    dy1 = Mdx(Md(i, 1), 2);
    dy2 = Mdx(Md(i, 2), 2);
    delk12 = sqrt(delY^2 + delX^2);
    dfd2(i, 1) = (1/(delk12^3))*((delY^2)*(dx1^2 + dx2^2) + (delX^2)*(dy1^2 + dy2^2) - 2*delY*delX*dx1*dy1 - 2*delY*delX*dx2*dy2 + 2*delY*delX*dx1*dy2 + 2*delY*delX*dx2*dy1 - 2*(delY^2)*dx1*dx2 - 2*(delX^2)*dy1*dy2);
end

for i = 1:length(Ms)
    delY = MP(Ms(i, 1), 2) - MP(Ms(i, 2), 2);
    delX =  MP(Ms(i, 1), 1) - MP(Ms(i, 2), 1);
    dx1 = Mdx(Ms(i, 1), 1);
    dx2 = Mdx(Ms(i, 2), 1);
    dy1 = Mdx(Ms(i, 1), 2);
    dy2 = Mdx(Ms(i, 2), 2);
    delk12 = sqrt(delY^2 + delX^2);
    dfs2(i, 1) = (2/(delk12^4))*(delY*delX*(dx1^2 + dx2^2) - delY*delX*(dy1^2 + dy2^2) - 2*delY*delX*dx1*dx2 - (delX^2 - delY^2)*(dx1*dy1 - dx2*dy1 - dx1*dy2 + dx2*dy2) + 2*delY*delX*dy1*dy2);
end

if all(abs(dfd2) < 10^(-5)) && all(abs(dfs2) < (10^(-5))*rad)
    pod = 0;
end

end

%Postup 2
C = (A'*P*lc);
N = (A'*P*A);
N = (1/2)*(N + N');
[U, S] = eig(N);
U2 = U;
U2(:, 1:3) = [];
S2 = S;
S2(:, 1:3) = [];
S2(1:3, :) = [];
X0 = -U2*S2^-1*U2'*C;
H = null(A);
t = -(B'*H)^-1*B'*X0;
X = X0 + H*t;

XV = x02 + X;

E = eye(n);

Red = (E - A*U2*S2^-1*U2'*A'*P); %Redundantni matice

v2 = Red*lc;

SigV = Red*inv(P); %Kovariancni matice
SigV = 0.5*(SigV + SigV');
[V, D] = eig(SigV);
V2 = V;
V2(:, 1:18) = [];
D2 = D(19:32, 19:32);

vh = sqrt(inv(D2))*V2'*v2; %Nekorelovane opravy (str. hod. = 0; rozptyl = 1)

%Vyrovnane nezname (x)
x = x0;
xg = x;
for i=3:7;
    xg(i*3, 1) = x(i*3, 1)/rad;
    if xg(i*3, 1) > 400
        xg(i*3, 1) = xg(i*3, 1) - 400;
    elseif xg(i*3, 1) < 0
        xg(i*3, 1) = xg(i*3, 1) + 400;
    end
end

%Aposteriorni smerodatna odchylka jednotkova (s0)
s0 = sqrt((v'*P*v)/(n + r - k));

alp_025 = chi2inv(0.975, (n + r - k));
alp_975 = chi2inv(0.025, (n + r - k));

hm = sqrt(alp_025/(n + r - k));
dm = sqrt(alp_975/(n + r - k));

if s0 >= dm && s0 <= hm
    int = 1; %s0 odpovida apriorni hodnote (1)
else
    int = 0; %s0 neodpovida apriorni hodnote (1)
end

%Smerodatne odchylky vyrovnanych neznamych
if PCP == 0
    Mx = (s0^2)*(ATPAB^(-1));
elseif PCP == 1
    Mx = ATPAB^(-1);
end

for i=1:(length(Mx)-3);
    Mxo(i, 1) = sqrt(Mx(i, i));
end

Mxog = Mxo;

for i=3:7;
    Mxog(i*3, 1) = Mxo(i*3, 1)/rad;
end

%Smerodatne odchylky vyrovnanych mereni
if PCP == 0
    Mlv = (s0^2)*A*C1*A';
elseif PCP == 1
    Mlv = A*C1*A';
end

for i = 1:length(Mlv)
    mlv(i, 1) = sqrt(Mlv(i, i));
end

%Normovane opravy
if PCP == 0
    for i = 1:length(SigV)
        vn(i, 1) = v2(i)/(s0*sqrt(SigV(i, i))); %Stredni hodnota = 0, rozptyl = 1
    end
elseif PCP == 1
    for i = 1:length(SigV)
        vn(i, 1) = v2(i)/(1*sqrt(SigV(i, i))); %Stredni hodnota = 0, rozptyl = 1
    end
end

%mxy
mxy(1, 1) = sqrt((Mxog(1, 1)^2 + Mxog(2, 1)^2)/2); %[m]
mxy(2, 1) = sqrt((Mxog(3, 1)^2 + Mxog(4, 1)^2)/2); %[m]
mxy(3, 1) = sqrt((Mxog(5, 1)^2 + Mxog(6, 1)^2)/2); %[m]
mxy(4, 1) = sqrt((Mxog(7, 1)^2 + Mxog(8, 1)^2)/2); %[m]
mxy(5, 1) = sqrt((Mxog(10, 1)^2 + Mxog(11, 1)^2)/2); %[m]
mxy(6, 1) = sqrt((Mxog(13, 1)^2 + Mxog(14, 1)^2)/2); %[m]
mxy(7, 1) = sqrt((Mxog(16, 1)^2 + Mxog(17, 1)^2)/2); %[m]
mxy(8, 1) = sqrt((Mxog(19, 1)^2 + Mxog(20, 1)^2)/2); %[m]

%mp
mp(1, 1) = sqrt(Mxog(1, 1)^2 + Mxog(2, 1)^2); %[m]
mp(2, 1) = sqrt(Mxog(3, 1)^2 + Mxog(4, 1)^2); %[m]
mp(3, 1) = sqrt(Mxog(5, 1)^2 + Mxog(6, 1)^2); %[m]
mp(4, 1) = sqrt(Mxog(7, 1)^2 + Mxog(8, 1)^2); %[m]
mp(5, 1) = sqrt(Mxog(10, 1)^2 + Mxog(11, 1)^2); %[m]
mp(6, 1) = sqrt(Mxog(13, 1)^2 + Mxog(14, 1)^2); %[m]
mp(7, 1) = sqrt(Mxog(16, 1)^2 + Mxog(17, 1)^2); %[m]
mp(8, 1) = sqrt(Mxog(19, 1)^2 + Mxog(20, 1)^2); %[m]

%%%Elipsy chyb

%35514
c_35514 = sqrt((Mx(1, 1) - Mx(2, 2))^2 + 4*Mx(1, 2)^2);
a_35514 = sqrt((Mx(1, 1) + Mx(2, 2) + c_35514)/2);
b_35514 = sqrt((Mx(1, 1) + Mx(2, 2) - c_35514)/2);
alp_35514 = atan2(2*Mx(1, 2), Mx(1, 1) - Mx(2, 2))/2;
while alp_35514 > 2*pi
    alp_35514 = alp_35514 - 2*pi;
end
while alp_35514 < 0
    alp_35514 = alp_35514 + 2*pi;
end
alp_35514g = alp_35514/rad;
if alp_35514g > 200
    alp_35514g = alp_35514g - 200;
end

%35515
c_35515 = sqrt((Mx(3, 3) - Mx(4, 4))^2 + 4*Mx(3, 4)^2);
a_35515 = sqrt((Mx(3, 3) + Mx(4, 4) + c_35515)/2);
b_35515 = sqrt((Mx(3, 3) + Mx(4, 4) - c_35515)/2);
alp_35515 = atan2(2*Mx(3, 4), Mx(3, 3) - Mx(4, 4))/2;
while alp_35515 > 2*pi
    alp_35515 = alp_35515 - 2*pi;
end
while alp_35515 < 0
    alp_35515 = alp_35515 + 2*pi;
end
alp_35515g = alp_35515/rad;
if alp_35515g > 200
    alp_35515g = alp_35515g - 200;
end

%35517
c_35517 = sqrt((Mx(5, 5) - Mx(6, 6))^2 + 4*Mx(5, 6)^2);
a_35517 = sqrt((Mx(5, 5) + Mx(6, 6) + c_35517)/2);
b_35517 = sqrt((Mx(5, 5) + Mx(6, 6) - c_35517)/2);
alp_35517 = atan2(2*Mx(5, 6), Mx(5, 5) - Mx(6, 6))/2;
while alp_35517 > 2*pi
    alp_35517 = alp_35517 - 2*pi;
end
while alp_35517 < 0
    alp_35517 = alp_35517 + 2*pi;
end
alp_35517g = alp_35517/rad;
if alp_35517g > 200
    alp_35517g = alp_35517g - 200;
end

%4003
c_4003 = sqrt((Mx(7, 7) - Mx(8, 8))^2 + 4*Mx(7, 8)^2);
a_4003 = sqrt((Mx(7, 7) + Mx(8, 8) + c_4003)/2);
b_4003 = sqrt((Mx(7, 7) + Mx(8, 8) - c_4003)/2);
alp_4003 = atan2(2*Mx(7, 8), Mx(7, 7) - Mx(8, 8))/2;
while alp_4003 > 2*pi
    alp_4003 = alp_4003 - 2*pi;
end
while alp_4003 < 0
    alp_4003 = alp_4003 + 2*pi;
end
alp_4003g = alp_4003/rad;
if alp_4003g > 200
    alp_4003g = alp_4003g - 200;
end

%2021
c_2021 = sqrt((Mx(10, 10) - Mx(11, 11))^2 + 4*Mx(10, 11)^2);
a_2021 = sqrt((Mx(10, 10) + Mx(11, 11) + c_2021)/2);
b_2021 = sqrt((Mx(10, 10) + Mx(11, 11) - c_2021)/2);
alp_2021 = atan2(2*Mx(10, 11), Mx(10, 10) - Mx(11, 11))/2;
while alp_2021 > 2*pi
    alp_2021 = alp_2021 - 2*pi;
end
while alp_2021 < 0
    alp_2021 = alp_2021 + 2*pi;
end
alp_2021g = alp_2021/rad;
if alp_2021g > 200
    alp_2021g = alp_2021g - 200;
end

%2022
c_2022 = sqrt((Mx(13, 13) - Mx(14, 14))^2 + 4*Mx(13, 14)^2);
a_2022 = sqrt((Mx(13, 13) + Mx(14, 14) + c_2022)/2);
b_2022 = sqrt((Mx(13, 13) + Mx(14, 14) - c_2022)/2);
alp_2022 = atan2(2*Mx(13, 14), Mx(13, 13) - Mx(14, 14))/2;
while alp_2022 > 2*pi
    alp_2022 = alp_2022 - 2*pi;
end
while alp_2022 < 0
    alp_2022 = alp_2022 + 2*pi;
end
alp_2022g = alp_2022/rad;
if alp_2022g > 200
    alp_2022g = alp_2022g - 200;
end

%2023
c_2023 = sqrt((Mx(16, 16) - Mx(17, 17))^2 + 4*Mx(16, 17)^2);
a_2023 = sqrt((Mx(16, 16) + Mx(17, 17) + c_2023)/2);
b_2023 = sqrt((Mx(16, 16) + Mx(17, 17) - c_2023)/2);
alp_2023 = atan2(2*Mx(16, 17), Mx(16, 16) - Mx(17, 17))/2;
while alp_2023 > 2*pi
    alp_2023 = alp_2023 - 2*pi;
end
while alp_2023 < 0
    alp_2023 = alp_2023 + 2*pi;
end
alp_2023g = alp_2023/rad;
if alp_2023g > 200
    alp_2023g = alp_2023g - 200;
end

%2024
c_2024 = sqrt((Mx(19, 19) - Mx(20, 20))^2 + 4*Mx(19, 20)^2);
a_2024 = sqrt((Mx(19, 19) + Mx(20, 20) + c_2024)/2);
b_2024 = sqrt((Mx(19, 19) + Mx(20, 20) - c_2024)/2);
alp_2024 = atan2(2*Mx(19, 20), Mx(19, 19) - Mx(20, 20))/2;
while alp_2024 > 2*pi
    alp_2024 = alp_2024 - 2*pi;
end
while alp_2024 < 0
    alp_2024 = alp_2024 + 2*pi;
end
alp_2024g = alp_2024/rad;
if alp_2024g > 200
    alp_2024g = alp_2024g - 200;
end