clear all
close all
fi10=0;           
fi20=0;
XO=55.4e-3; YO=-10e-3; AC=132.07e-3; CP=168.808e-3; CD=40.576e-3; DE=152e-3; BE=42.6e-3; ABx=-1e-3; ABy=3e-3; delta=7.89*pi/180;
b3=CP; b4=AC; b5=CD; b6=DE; b7=BE; b8=ABx; b9=ABy; beta1=0; beta2=pi/2; beta8=3*pi/2; beta9=0;  


alfa=95.81*pi/180+0.01*(rand-0.5); gama=5.69*pi/180+0.01*(rand-0.5); b10=50.5e-3+0.001*(rand-0.5); b11=145.8e-3+0.001*(rand-0.5); b12=30.428e-3+0.001*(rand-0.5); b13=40.618e-3+0.001*(rand-0.5); b14=172.83e-3+0.001*(rand-0.5); b15=30.5e-3+0.001*(rand-0.5); b16=150e-3+0.001*(rand-0.5); b17=20.7e-3+0.001*(rand-0.5); b18=40.7e-3+0.001*(rand-0.5); b19=AC+0.001*(rand-0.5); beta17=3*pi/2; beta18=0;
 % pøesné hodnoty alfa=109.81*pi/180; gama=2.69*pi/180; b10=38.5e-3; b11=158.8e-3; b12=39.428e-3; b13=43.618e-3; b14=162.83e-3; b15=38.5e-3; b16=142e-3; b17=16.7e-3; b18=34.7e-3; b19=AC;
b123=sqrt(b12^2+b13^2);

odhady=[b3 b4 b5 b6 b7 b8 b9 b10 b11 b123 b14 b15 b16 b17 b18 alfa gama delta];
rozdily=[];
load presne_rozmery2

rozdily=[odhady-presne];

load data70   
XP=[];
YP=[];
FI1=[];
FI2=[];
% pocet poloh
n_poloh=size(QQ,2);
for i=1:n_poloh  % matice Xových a Yových souøadnic bodu P
    XP(i)=P(i,1);
    YP(i)=P(i,2);
end
XP=XP';
YP=YP';
for i=1:n_poloh   % matice nezávislých úhlù fi1 a fi2
    FI1(i)=FI(i,1); 
    FI2(i)=FI(i,2);
end
FI1=FI1';
FI2=FI2';
% upravime parametry jako vektory
XOM=XO.*ones(n_poloh,1);  
YOM=YO.*ones(n_poloh,1);

B3M=b3.*ones(n_poloh,1);
B4M=b4.*ones(n_poloh,1);
B5M=b5.*ones(n_poloh,1);
B6M=b6.*ones(n_poloh,1);
B7M=b7.*ones(n_poloh,1);
B8M=b8.*ones(n_poloh,1);
B9M=b9.*ones(n_poloh,1);
DELTAM=delta.*ones(n_poloh,1);
FI10M=fi10.*ones(n_poloh,1);
FI20M=fi20.*ones(n_poloh,1);

BETA1M=beta1.*ones(n_poloh,1);
BETA2M=beta2.*ones(n_poloh,1);
BETA8M=beta8.*ones(n_poloh,1);
BETA9M=beta9.*ones(n_poloh,1);

BETA3M=Z(3,:)';  
BETA6M=Z(4,:)'; 

B10M=b10.*ones(n_poloh,1);
B11M=b11.*ones(n_poloh,1);
B123M=b123.*ones(n_poloh,1);
B14M=b14.*ones(n_poloh,1);
B15M=b15.*ones(n_poloh,1);
B16M=b16.*ones(n_poloh,1);
B17M=b17.*ones(n_poloh,1);
B18M=b18.*ones(n_poloh,1);
ALFAM=alfa.*ones(n_poloh,1);
GAMAM=gama.*ones(n_poloh,1);
k=1;

for i=1:n_poloh
BETA10M(i,1)=50*pi/180;    %promenna
end;

for i=1:n_poloh
BETA11M(i,1)=355*pi/180;    %promenna
end;

for i=1:n_poloh
BETA123M(i,1)=220*pi/180;    %promenna
end;
BETA16M=BETA6M;    %promenna

BETA17M=(3*pi/2).*ones(n_poloh,1);
BETA18M=zeros(n_poloh,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Df1_dbeta3=zeros(n_poloh,n_poloh);  
Df1_dbeta6=zeros(n_poloh,n_poloh);  
Df2_dbeta3=zeros(n_poloh,n_poloh);
Df2_dbeta6=zeros(n_poloh,n_poloh);
Df3_dbeta3=zeros(n_poloh,n_poloh);
Df3_dbeta6=zeros(n_poloh,n_poloh);
Df4_dbeta3=zeros(n_poloh,n_poloh);
Df4_dbeta6=zeros(n_poloh,n_poloh);
Df5_dbeta3=zeros(n_poloh,n_poloh);
Df5_dbeta6=zeros(n_poloh,n_poloh);
Df6_dbeta3=zeros(n_poloh,n_poloh);
Df6_dbeta6=zeros(n_poloh,n_poloh);
Df7_dbeta3=zeros(n_poloh,n_poloh);
Df7_dbeta6=zeros(n_poloh,n_poloh);
Df8_dbeta3=zeros(n_poloh,n_poloh);
Df8_dbeta6=zeros(n_poloh,n_poloh);

Df1_dbeta10=zeros(n_poloh,n_poloh);  
Df1_dbeta11=zeros(n_poloh,n_poloh);  
Df2_dbeta10=zeros(n_poloh,n_poloh);
Df2_dbeta11=zeros(n_poloh,n_poloh);
Df3_dbeta10=zeros(n_poloh,n_poloh);
Df3_dbeta11=zeros(n_poloh,n_poloh);
Df4_dbeta10=zeros(n_poloh,n_poloh);
Df4_dbeta11=zeros(n_poloh,n_poloh);
Df5_dbeta10=zeros(n_poloh,n_poloh);
Df5_dbeta11=zeros(n_poloh,n_poloh);
Df6_dbeta10=zeros(n_poloh,n_poloh);
Df6_dbeta11=zeros(n_poloh,n_poloh);
Df7_dbeta10=zeros(n_poloh,n_poloh);
Df7_dbeta11=zeros(n_poloh,n_poloh);
Df8_dbeta10=zeros(n_poloh,n_poloh);
Df8_dbeta11=zeros(n_poloh,n_poloh);

Df1_dbeta123=zeros(n_poloh,n_poloh);  
Df1_dbeta16=zeros(n_poloh,n_poloh);  
Df2_dbeta123=zeros(n_poloh,n_poloh);
Df2_dbeta16=zeros(n_poloh,n_poloh);
Df3_dbeta123=zeros(n_poloh,n_poloh);
Df3_dbeta16=zeros(n_poloh,n_poloh);
Df4_dbeta123=zeros(n_poloh,n_poloh);
Df4_dbeta16=zeros(n_poloh,n_poloh);
Df5_dbeta123=zeros(n_poloh,n_poloh);
Df5_dbeta16=zeros(n_poloh,n_poloh);
Df6_dbeta123=zeros(n_poloh,n_poloh);
Df6_dbeta16=zeros(n_poloh,n_poloh);
Df7_dbeta123=zeros(n_poloh,n_poloh);
Df7_dbeta16=zeros(n_poloh,n_poloh);
Df8_dbeta123=zeros(n_poloh,n_poloh);
Df8_dbeta16=zeros(n_poloh,n_poloh);


for j=1:10
   % j iterací pro vsechny polohy najednou

    BETA4M=FI1+FI10M; %%vztahy k 1. èásti
    BETA7M=FI2+FI20M;
    B1M=XP-XOM;  
    B2M=YP-YOM; 
    
    BETA5M=BETA3M-DELTAM;  
    %%vztahy k 2 èásti
    BETA14M=BETA3M-GAMAM;
    BETA15M=BETA10M+ALFAM;
    BETA19M=BETA4M-pi.*ones(n_poloh,1);

    %rovnice vazeb
    dr1=B1M.*cos(BETA1M)+B2M.*cos(BETA2M)+B3M.*cos(BETA3M)+B4M.*cos(BETA4M);
    dr2=B1M.*sin(BETA1M)+B2M.*sin(BETA2M)+B3M.*sin(BETA3M)+B4M.*sin(BETA4M);
    dr3=B1M.*cos(BETA1M)+B2M.*cos(BETA2M)+B3M.*cos(BETA3M)+B5M.*cos(BETA5M)+B6M.*cos(BETA6M)+B7M.*cos(BETA7M)+B8M.*cos(BETA8M)+B9M.*cos(BETA9M);
    dr4=B1M.*sin(BETA1M)+B2M.*sin(BETA2M)+B3M.*sin(BETA3M)+B5M.*sin(BETA5M)+B6M.*sin(BETA6M)+B7M.*sin(BETA7M)+B8M.*sin(BETA8M)+B9M.*sin(BETA9M);
    %2. èást
    dr5=B10M.*cos(BETA10M)+B11M.*cos(BETA11M)+B123M.*cos(BETA123M)+B14M.*cos(BETA14M);
    dr6=B10M.*sin(BETA10M)+B11M.*sin(BETA11M)+B123M.*sin(BETA123M)+B14M.*sin(BETA14M);
    dr7=B15M.*cos(BETA15M)+B16M.*cos(BETA16M)+B17M.*cos(BETA17M)+B18M.*cos(BETA18M)+B4M.*cos(BETA19M);
    dr8=B15M.*sin(BETA15M)+B16M.*sin(BETA16M)+B17M.*sin(BETA17M)+B18M.*sin(BETA18M)+B4M.*sin(BETA19M);
    
    dr=-[dr1
         dr2
         dr3
         dr4
         dr5
         dr6
         dr7
         dr8];
   
    figure(1) 
    subplot(4,2,1),plot(dr1),grid on,title('dr1');
    subplot(4,2,2),plot(dr2),grid on,title('dr2');
    subplot(4,2,3),plot(dr3),grid on,title('dr3');
    subplot(4,2,4),plot(dr4),grid on,title('dr4');
    subplot(4,2,5),plot(dr1),grid on,title('dr5');
    subplot(4,2,6),plot(dr2),grid on,title('dr6');
    subplot(4,2,7),plot(dr3),grid on,title('dr7');
    subplot(4,2,8),plot(dr4),grid on,title('dr8');
     
    % vektorovy (pro vsechny polohy) vypocet citlivosti
    df1_db10=zeros(n_poloh,1);
    df1_db11=zeros(n_poloh,1);
    df1_db123=zeros(n_poloh,1);
    df1_db14=zeros(n_poloh,1);
    df1_db15=zeros(n_poloh,1);
    df1_db16=zeros(n_poloh,1);
    df1_db17=zeros(n_poloh,1);
    df1_db18=zeros(n_poloh,1);
    df1_dalfa=zeros(n_poloh,1);
    df1_dgama=zeros(n_poloh,1);

    df1_db3=cos(BETA3M);
    df1_db4=cos(BETA4M);
    df1_db5=zeros(n_poloh,1);
    df1_db6=zeros(n_poloh,1);
    df1_db7=zeros(n_poloh,1);
    df1_db8=zeros(n_poloh,1);
    df1_db9=zeros(n_poloh,1);
    df1_ddelta=zeros(n_poloh,1);
    df1_dfi10=B4M.*-sin(BETA4M);
    df1_dfi20=zeros(n_poloh,1);
    df1_dxo=-cos(BETA1M);
    df1_dyo=-cos(BETA2M);
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df1_dbeta3(i,i)=B3M(i)*-sin(BETA3M(i));
    end 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2. zderivovaná rovnice vazby
    df2_db10=zeros(n_poloh,1);
    df2_db11=zeros(n_poloh,1);
    df2_db123=zeros(n_poloh,1);
    df2_db14=zeros(n_poloh,1);
    df2_db15=zeros(n_poloh,1);
    df2_db16=zeros(n_poloh,1);
    df2_db17=zeros(n_poloh,1);
    df2_db18=zeros(n_poloh,1);
    df2_dalfa=zeros(n_poloh,1);
    df2_dgama=zeros(n_poloh,1);
    
    df2_db3=sin(BETA3M);
    df2_db4=sin(BETA4M);
    df2_db5=zeros(n_poloh,1);
    df2_db6=zeros(n_poloh,1);
    df2_db7=zeros(n_poloh,1);
    df2_db8=zeros(n_poloh,1);
    df2_db9=zeros(n_poloh,1);
    df2_ddelta=zeros(n_poloh,1);
    df2_dfi10=B4M.*cos(BETA4M);
    df2_dfi20=zeros(n_poloh,1);
    df2_dxo=-sin(BETA1M);
    df2_dyo=-sin(BETA2M);
    for i=1:n_poloh
      Df2_dbeta3(i,i)=B3M(i)*cos(BETA3M(i));
    end    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    3. zderivovaná rovnice
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    vazby
    df3_db10=zeros(n_poloh,1);
    df3_db11=zeros(n_poloh,1);
    df3_db123=zeros(n_poloh,1);
    df3_db14=zeros(n_poloh,1);
    df3_db15=zeros(n_poloh,1);
    df3_db16=zeros(n_poloh,1);
    df3_db17=zeros(n_poloh,1);
    df3_db18=zeros(n_poloh,1);
    df3_dalfa=zeros(n_poloh,1);
    df3_dgama=zeros(n_poloh,1);
    
    df3_db3=cos(BETA3M);
    df3_db4=zeros(n_poloh,1);
    df3_db5=cos(BETA5M);
    df3_db6=cos(BETA6M);
    df3_db7=cos(BETA7M);
    df3_db8=cos(BETA8M);
    df3_db9=cos(BETA9M);
    df3_ddelta=B5M.*sin(BETA5M);
    df3_dfi10=zeros(n_poloh,1);
    df3_dfi20=-B7M.*sin(BETA7M);
    df3_dxo=-cos(BETA1M);
    df3_dyo=-cos(BETA2M);
    for i=1:n_poloh
      Df3_dbeta3(i,i)=B3M(i)*-sin(BETA3M(i))+B5M(i)*-sin(BETA5M(i));
    end    
    for i=1:n_poloh
     Df3_dbeta6(i,i)=B6M(i)*-sin(BETA6M(i));
    end   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4. zderivovaná rovnice vazby  
    df4_db10=zeros(n_poloh,1);
    df4_db11=zeros(n_poloh,1);
    df4_db123=zeros(n_poloh,1);
    df4_db14=zeros(n_poloh,1);
    df4_db15=zeros(n_poloh,1);
    df4_db16=zeros(n_poloh,1);
    df4_db17=zeros(n_poloh,1);
    df4_db18=zeros(n_poloh,1);
    df4_dalfa=zeros(n_poloh,1);
    df4_dgama=zeros(n_poloh,1);
    
    df4_db3=sin(BETA3M);
    df4_db4=zeros(n_poloh,1);
    df4_db5=sin(BETA5M);
    df4_db6=sin(BETA6M);
    df4_db7=sin(BETA7M);
    df4_db8=sin(BETA8M);
    df4_db9=sin(BETA9M);
    df4_ddelta=-B5M.*cos(BETA5M);
    df4_dfi10=zeros(n_poloh,1);
    df4_dfi20=B7M.*cos(BETA7M);
    df4_dxo=-sin(BETA1M);
    df4_dyo=-sin(BETA2M);
    for i=1:n_poloh
      Df4_dbeta3(i,i)=B3M(i)*cos(BETA3M(i))+B5M(i)*cos(BETA5M(i));
    end    
    for i=1:n_poloh
      Df4_dbeta6(i,i)=B6M(i)*cos(BETA6M(i));
    end  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2. èást
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 5. vazba
    df5_db10=cos(BETA10M);
    df5_db11=cos(BETA11M);
    df5_db123=cos(BETA123M);
    df5_db14=cos(BETA14M);
    df5_db15=zeros(n_poloh,1);
    df5_db16=zeros(n_poloh,1);
    df5_db17=zeros(n_poloh,1);
    df5_db18=zeros(n_poloh,1);
    df5_dalfa=zeros(n_poloh,1);
    df5_dgama=B14M.*sin(BETA14M);

    df5_db3=zeros(n_poloh,1);
    df5_db4=zeros(n_poloh,1);
    df5_db5=zeros(n_poloh,1);
    df5_db6=zeros(n_poloh,1);
    df5_db7=zeros(n_poloh,1);
    df5_db8=zeros(n_poloh,1);
    df5_db9=zeros(n_poloh,1);
    df5_ddelta=zeros(n_poloh,1);
    df5_dfi10=zeros(n_poloh,1);
    df5_dfi20=zeros(n_poloh,1);
    df5_dxo=zeros(n_poloh,1);
    df5_dyo=zeros(n_poloh,1);
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df5_dbeta3(i,i)=B14M(i)*-sin(BETA14M(i));
    end 
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df5_dbeta10(i,i)=B10M(i)*-sin(BETA10M(i));
    end 
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df5_dbeta11(i,i)=B11M(i)*-sin(BETA11M(i));
    end 
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df5_dbeta123(i,i)=B123M(i)*-sin(BETA123M(i));
    end 
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 6. vazba
    df6_db10=sin(BETA10M);
    df6_db11=sin(BETA11M);
    df6_db123=sin(BETA123M);
    df6_db14=sin(BETA14M);
    df6_db15=zeros(n_poloh,1);
    df6_db16=zeros(n_poloh,1);
    df6_db17=zeros(n_poloh,1);
    df6_db18=zeros(n_poloh,1);
    df6_dalfa=zeros(n_poloh,1);
    df6_dgama=B14M.*-cos(BETA3M-GAMAM);

    df6_db3=zeros(n_poloh,1);
    df6_db4=zeros(n_poloh,1);
    df6_db5=zeros(n_poloh,1);
    df6_db6=zeros(n_poloh,1);
    df6_db7=zeros(n_poloh,1);
    df6_db8=zeros(n_poloh,1);
    df6_db9=zeros(n_poloh,1);
    df6_ddelta=zeros(n_poloh,1);
    df6_dfi10=zeros(n_poloh,1);
    df6_dfi20=zeros(n_poloh,1);
    df6_dxo=zeros(n_poloh,1);
    df6_dyo=zeros(n_poloh,1);
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df6_dbeta3(i,i)=B14M(i)*cos(BETA14M(i));
    end 
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df6_dbeta10(i,i)=B10M(i)*cos(BETA10M(i));
    end 
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df6_dbeta11(i,i)=B11M(i)*cos(BETA11M(i));
    end 
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df6_dbeta123(i,i)=B123M(i)*cos(BETA123M(i));
    end 
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%7. vazba
    df7_db10=zeros(n_poloh,1);
    df7_db11=zeros(n_poloh,1);
    df7_db123=zeros(n_poloh,1);
    df7_db14=zeros(n_poloh,1);
    df7_db15=cos(BETA15M);
    df7_db16=cos(BETA16M);
    df7_db17=cos(BETA17M);
    df7_db18=cos(BETA18M);
    df7_dalfa=B15M.*-sin(BETA15M);
    df7_dgama=zeros(n_poloh,1);

    df7_db3=zeros(n_poloh,1);
    df7_db4=cos(BETA19M);
    df7_db5=zeros(n_poloh,1);
    df7_db6=zeros(n_poloh,1);
    df7_db7=zeros(n_poloh,1);
    df7_db8=zeros(n_poloh,1);
    df7_db9=zeros(n_poloh,1);
    df7_ddelta=zeros(n_poloh,1);
    df7_dfi10=B4M.*-sin(BETA19M);
    df7_dfi20=zeros(n_poloh,1);
    df7_dxo=zeros(n_poloh,1);
    df7_dyo=zeros(n_poloh,1);
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df7_dbeta16(i,i)=B16M(i)*-sin(BETA16M(i));
    end 
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df7_dbeta10(i,i)=B15M(i)*-sin(BETA15M(i));
    end     
    
    %%%%%%%%%%%%%%%%%%%%%%% 8. vazba    
    df8_db10=zeros(n_poloh,1);
    df8_db11=zeros(n_poloh,1);
    df8_db123=zeros(n_poloh,1);
    df8_db14=zeros(n_poloh,1);
    df8_db15=sin(BETA15M);
    df8_db16=sin(BETA16M);
    df8_db17=sin(BETA17M);
    df8_db18=sin(BETA18M);
    df8_dalfa=B15M.*cos(BETA15M);
    df8_dgama=zeros(n_poloh,1);

    df8_db3=zeros(n_poloh,1);
    df8_db4=sin(BETA19M);
    df8_db5=zeros(n_poloh,1);
    df8_db6=zeros(n_poloh,1);
    df8_db7=zeros(n_poloh,1);
    df8_db8=zeros(n_poloh,1);
    df8_db9=zeros(n_poloh,1);
    df8_ddelta=zeros(n_poloh,1);
    df8_dfi10=B4M.*cos(BETA19M);
    df8_dfi20=zeros(n_poloh,1);
    df8_dxo=zeros(n_poloh,1);
    df8_dyo=zeros(n_poloh,1);
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df8_dbeta16(i,i)=B16M(i)*cos(BETA16M(i));
    end 
    for i=1:n_poloh   %jiná derivace v každé poloze
     Df8_dbeta10(i,i)=B15M(i)*cos(BETA15M(i));
    end 
    
    % jacobian
    Jacobi=[df1_db10, df1_db11,df1_db123, df1_db14,df1_db15, df1_db16,df1_db17,df1_db18,df1_dgama,df1_dalfa,df1_db3, df1_db4, df1_db5, df1_db6, df1_db7, df1_db8,  df1_db9, df1_ddelta, df1_dfi10, df1_dfi20, df1_dxo, df1_dyo, Df1_dbeta6, Df1_dbeta3, Df1_dbeta10, Df1_dbeta11, Df1_dbeta123, Df1_dbeta16;
            df2_db10, df2_db11,df2_db123, df2_db14,df2_db15, df2_db16,df2_db17,df2_db18,df2_dgama,df2_dalfa,df2_db3, df2_db4, df2_db5, df2_db6, df2_db7, df2_db8,  df2_db9, df2_ddelta, df2_dfi10, df2_dfi20, df2_dxo, df2_dyo, Df2_dbeta6, Df2_dbeta3, Df2_dbeta10, Df2_dbeta11, Df2_dbeta123, Df2_dbeta16;
            df3_db10, df3_db11,df3_db123, df3_db14,df3_db15, df3_db16,df3_db17,df3_db18,df3_dgama,df3_dalfa,df3_db3, df3_db4, df3_db5, df3_db6, df3_db7, df3_db8,  df3_db9, df3_ddelta, df3_dfi10, df3_dfi20, df3_dxo, df3_dyo, Df3_dbeta6, Df3_dbeta3, Df3_dbeta10, Df3_dbeta11, Df3_dbeta123, Df3_dbeta16;
            df4_db10, df4_db11,df4_db123, df4_db14,df4_db15, df4_db16,df4_db17,df4_db18,df4_dgama,df4_dalfa,df4_db3, df4_db4, df4_db5, df4_db6, df4_db7, df4_db8,  df4_db9, df4_ddelta, df4_dfi10, df4_dfi20, df4_dxo, df4_dyo, Df4_dbeta6, Df4_dbeta3, Df4_dbeta10, Df4_dbeta11, Df4_dbeta123, Df4_dbeta16;
            df5_db10, df5_db11,df5_db123, df5_db14,df5_db15, df5_db16,df5_db17,df5_db18,df5_dgama,df5_dalfa,df5_db3, df5_db4, df5_db5, df5_db6, df5_db7, df5_db8,  df5_db9, df5_ddelta, df5_dfi10, df5_dfi20, df5_dxo, df5_dyo, Df5_dbeta6, Df5_dbeta3, Df5_dbeta10, Df5_dbeta11, Df5_dbeta123, Df5_dbeta16;
            df6_db10, df6_db11,df6_db123, df6_db14,df6_db15, df6_db16,df6_db17,df6_db18,df6_dgama,df6_dalfa,df6_db3, df6_db4, df6_db5, df6_db6, df6_db7, df6_db8,  df6_db9, df6_ddelta, df6_dfi10, df6_dfi20, df6_dxo, df6_dyo, Df6_dbeta6, Df6_dbeta3, Df6_dbeta10, Df6_dbeta11, Df6_dbeta123, Df6_dbeta16;
            df7_db10, df7_db11,df7_db123, df7_db14,df7_db15, df7_db16,df7_db17,df7_db18,df7_dgama,df7_dalfa,df7_db3, df7_db4, df7_db5, df7_db6, df7_db7, df7_db8,  df7_db9, df7_ddelta, df7_dfi10, df7_dfi20, df7_dxo, df7_dyo, Df7_dbeta6, Df7_dbeta3, Df7_dbeta10, Df7_dbeta11, Df7_dbeta123, Df7_dbeta16;
            df8_db10, df8_db11,df8_db123, df8_db14,df8_db15, df8_db16,df8_db17,df8_db18,df8_dgama,df8_dalfa,df8_db3, df8_db4, df8_db5, df8_db6, df8_db7, df8_db8,  df8_db9, df8_ddelta, df8_dfi10, df8_dfi20, df8_dxo, df8_dyo, Df8_dbeta6, Df8_dbeta3, Df8_dbeta10, Df8_dbeta11, Df8_dbeta123, Df8_dbeta16];
   
        kalibrovatelnost=(cond(Jacobi'*Jacobi))

    
    [U,S,V]=svd(Jacobi'*Jacobi);
    UTQ=U'*Jacobi'*dr;
    S=diag(S);
    y=UTQ./S;
    Difpar=V*y;    
   
    % zlepseni parametru
    
    B10M=B10M+Difpar(1)*ones(n_poloh,1);
    B11M=B11M+Difpar(2)*ones(n_poloh,1);
    B123M=B123M+Difpar(3)*ones(n_poloh,1);
    B14M=B14M+Difpar(4)*ones(n_poloh,1);
    B15M=B15M+Difpar(5)*ones(n_poloh,1);
    B16M=B16M+Difpar(6)*ones(n_poloh,1);
    B17M=B17M+Difpar(7)*ones(n_poloh,1);
    B18M=B18M+Difpar(8)*ones(n_poloh,1);
    GAMAM=GAMAM+Difpar(9)*ones(n_poloh,1);
    ALFAM=ALFAM+Difpar(10)*ones(n_poloh,1);
    
    
    
	B3M=B3M+Difpar(11)*ones(n_poloh,1);
	B4M=B4M+Difpar(12)*ones(n_poloh,1);
	B5M=B5M+Difpar(13)*ones(n_poloh,1);
	B6M=B6M+Difpar(14)*ones(n_poloh,1);
	B7M=B7M+Difpar(15)*ones(n_poloh,1);
	B8M=B8M+Difpar(16)*ones(n_poloh,1);	
    B9M=B9M+Difpar(17)*ones(n_poloh,1);	
    DELTAM=DELTAM+Difpar(18)*ones(n_poloh,1);
    FI10M=FI10M+Difpar(19)*ones(n_poloh,1);
    FI20M=FI20M+Difpar(20)*ones(n_poloh,1);
    XOM=XOM+Difpar(21)*ones(n_poloh,1);	
    YOM=YOM+Difpar(22)*ones(n_poloh,1);	
%%%
for i=1:n_poloh  
   BETA6M(i)=BETA6M(i)+Difpar(i+22); 
end
for i=1:n_poloh  
   BETA3M(i)=BETA3M(i)+Difpar(i+22+n_poloh);  
end
for i=1:n_poloh  
   BETA10M(i)=BETA10M(i)+Difpar(i+22+2*n_poloh);  
end
for i=1:n_poloh  
   BETA11M(i)=BETA11M(i)+Difpar(i+22+3*n_poloh);  
end
for i=1:n_poloh  
   BETA123M(i)=BETA123M(i)+Difpar(i+22+4*n_poloh);  
end
for i=1:n_poloh  
   BETA16M(i)=BETA16M(i)+Difpar(i+22+5*n_poloh);  
end
%%%

odhady=[B3M(1) B4M(1) B5M(1) B6M(1) B7M(1) B8M(1) B9M(1) B10M(1) B11M(1) B123M(1) B14M(1) B15M(1) B16M(1) B17M(1) B18M(1) ALFAM(1) GAMAM(1) DELTAM(1)];

rozdily=[rozdily;
         odhady-presne];

end
    B10M(n_poloh,1)
    B11M(n_poloh,1)
    B123M(n_poloh,1)
    B14M(n_poloh,1)
    B15M(n_poloh,1)
    B16M(n_poloh,1)
    B17M(n_poloh,1)
    B18M(n_poloh,1)
    GAMAM(n_poloh,1)
    ALFAM(n_poloh,1)
    %
B3M(n_poloh,1)
B4M(n_poloh,1)
B5M(n_poloh,1)
B6M(n_poloh,1)
B7M(n_poloh,1)
B8M(n_poloh,1)
B9M(n_poloh,1)
DELTAM(n_poloh,1)
FI10M(n_poloh,1)
FI20M(n_poloh,1)
XOM(n_poloh,1)
YOM(n_poloh,1)




figure
plot(rozdily)
legend('B3M(1)', 'B4M(1)', 'B5M(1)', 'B6M(1)', 'B7M(1)', 'B8M(1)', 'B9M(1)', 'B10M(1)', 'B11M(1)', 'B123M(1)', 'B14M(1)', 'B15M(1)', 'B16M(1)', 'B17M(1)', 'B18M(1)', 'ALFAM(1)' ,'GAMAM(1)', 'DELTAM(1)');

figure
plot(rozdily(1:6,[1:7]))
legend('b_3', 'b_4', 'b_5', 'b_6', 'b_7', 'b_8', 'b_9');
xlabel('iterace [-]')
ylabel('odchylka od presne hodnoty [m]')
title('Vyvoj kalibrovanych parameteru')
grid

