clear
clc
format long G
%Výpoèet smìrodatné odchylky výšky koncového bodu 
syms dRR dSab dS dSS dScd dSdg dQ dN dP  dSef dK dScg dSeh dW dY dShn dSfn d1 d2 d3

% X1 = (dRR*cos(acos(((dRR*dRR)+(dSab*dSab)-(d1*d1))/(2*dRR*dSab))))
% X2 =( dSS*cos((acos(((dRR*dRR)+(dSab*dSab)-(d1*d1))/(2*dRR*dSab)))+(acos(((dRR*dRR)+(dSS*dSS)-(dY*dY))/(2*dRR*dSS)))-pi))
% X3 = (dP*cos(((acos(((dRR*dRR)+(dSab*dSab)-(d1*d1))/(2*dRR*dSab)))+(acos(((dRR*dRR)+(dSS*dSS)-(dY*dY))/(2*dRR*dSS)))-pi)+(2*pi-(acos(((dSS*dSS)+(dS*dS)-(dQ*dQ))/(2*dS*dSS)))-(acos(((dS*dS)+(dScd*dScd)-(d2*d2))/(2*dS*dScd)))-(acos(((dScd*dScd)+(dSdg*dSdg)-(dScg*dScg))/(2*dScd*dSdg)))-(acos(((dSdg*dSdg)+(dP*dP)-(dN*dN))/(2*dSdg*dP))))-pi))
% X4 = (dSeh*cos(((acos(((dRR*dRR)+(dSab*dSab)-(d1*d1))/(2*dRR*dSab)))+(acos(((dRR*dRR)+(dSS*dSS)-(dY*dY))/(2*dRR*dSS)))-pi)+(2*pi-(acos(((dSS*dSS)+(dS*dS)-(dQ*dQ))/(2*dS*dSS)))-(acos(((dS*dS)+(dScd*dScd)-(d2*d2))/(2*dS*dScd)))-(acos(((dScd*dScd)+(dSdg*dSdg)-(dScg*dScg))/(2*dScd*dSdg)))-(acos(((dSdg*dSdg)+(dP*dP)-(dN*dN))/(2*dSdg*dP))))-pi))
% X5 = (dW*cos((((acos(((dRR*dRR)+(dSab*dSab)-(d1*d1))/(2*dRR*dSab)))+(acos(((dRR*dRR)+(dSS*dSS)-(dY*dY))/(2*dRR*dSS)))-pi)+(2*pi-(acos(((dSS*dSS)+(dS*dS)-(dQ*dQ))/(2*dS*dSS)))-(acos(((dS*dS)+(dScd*dScd)-(d2*d2))/(2*dS*dScd)))-(acos(((dScd*dScd)+(dSdg*dSdg)-(dScg*dScg))/(2*dScd*dSdg)))-(acos(((dSdg*dSdg)+(dP*dP)-(dN*dN))/(2*dSdg*dP))))-pi)+(2*pi-(acos(((dW*dW)+(dShn*dShn)-(dK*dK))/(2*dW*dShn)))-acos((((sqrt(((dSef*dSef)+(dSeh*dSeh)-(2*dSef*dSeh*cos(pi-(acos(((dP*dP)+(dN*dN)-(dSdg*dSdg))/(2*dP*dN)))-(acos(((dN*dN)+(dSef*dSef)-(d3*d3))/(2*dN*dSef))))))))*(sqrt(((dSef*dSef)+(dSeh*dSeh)-(2*dSef*dSeh*cos(pi-(acos(((dP*dP)+(dN*dN)-(dSdg*dSdg))/(2*dP*dN)))-(acos(((dN*dN)+(dSef*dSef)-(d3*d3))/(2*dN*dSef)))))))))+(dShn*dShn)-(dSfn*dSfn))/(2*(sqrt(((dSef*dSef)+(dSeh*dSeh)-(2*dSef*dSeh*cos(pi-(acos(((dP*dP)+(dN*dN)-(dSdg*dSdg))/(2*dP*dN)))-(acos(((dN*dN)+(dSef*dSef)-(d3*d3))/(2*dN*dSef))))))))*dShn)))-(acos(((dSeh*dSeh)+( (sqrt(((dSef*dSef)+(dSeh*dSeh)-(2*dSef*dSeh*cos(pi-(acos(((dP*dP)+(dN*dN)-(dSdg*dSdg))/(2*dP*dN)))-(acos(((dN*dN)+(dSef*dSef)-(d3*d3))/(2*dN*dSef))))))))* (sqrt(((dSef*dSef)+(dSeh*dSeh)-(2*dSef*dSeh*cos(pi-(acos(((dP*dP)+(dN*dN)-(dSdg*dSdg))/(2*dP*dN)))-(acos(((dN*dN)+(dSef*dSef)-(d3*d3))/(2*dN*dSef)))))))))-(dSef*dSef))/(2*dSeh* (sqrt(((dSef*dSef)+(dSeh*dSeh)-(2*dSef*dSeh*cos(pi-(acos(((dP*dP)+(dN*dN)-(dSdg*dSdg))/(2*dP*dN)))-(acos(((dN*dN)+(dSef*dSef)-(d3*d3))/(2*dN*dSef)))))))))))-pi))
%výpoèet souøadnice X (Z) koncového bodu
X5 = (dRR*cos(acos(((dRR*dRR)+(dSab*dSab)-(d1*d1))/(2*dRR*dSab))))+( dSS*cos((acos(((dRR*dRR)+(dSab*dSab)-(d1*d1))/(2*dRR*dSab)))+(acos(((dRR*dRR)+(dSS*dSS)-(dY*dY))/(2*dRR*dSS)))-pi))+(dP*cos(((acos(((dRR*dRR)+(dSab*dSab)-(d1*d1))/(2*dRR*dSab)))+(acos(((dRR*dRR)+(dSS*dSS)-(dY*dY))/(2*dRR*dSS)))-pi)+(2*pi-(acos(((dSS*dSS)+(dS*dS)-(dQ*dQ))/(2*dS*dSS)))-(acos(((dS*dS)+(dScd*dScd)-(d2*d2))/(2*dS*dScd)))-(acos(((dScd*dScd)+(dSdg*dSdg)-(dScg*dScg))/(2*dScd*dSdg)))-(acos(((dSdg*dSdg)+(dP*dP)-(dN*dN))/(2*dSdg*dP))))-pi))+(dSeh*cos(((acos(((dRR*dRR)+(dSab*dSab)-(d1*d1))/(2*dRR*dSab)))+(acos(((dRR*dRR)+(dSS*dSS)-(dY*dY))/(2*dRR*dSS)))-pi)+(2*pi-(acos(((dSS*dSS)+(dS*dS)-(dQ*dQ))/(2*dS*dSS)))-(acos(((dS*dS)+(dScd*dScd)-(d2*d2))/(2*dS*dScd)))-(acos(((dScd*dScd)+(dSdg*dSdg)-(dScg*dScg))/(2*dScd*dSdg)))-(acos(((dSdg*dSdg)+(dP*dP)-(dN*dN))/(2*dSdg*dP))))-pi))+(dW*cos((((acos(((dRR*dRR)+(dSab*dSab)-(d1*d1))/(2*dRR*dSab)))+(acos(((dRR*dRR)+(dSS*dSS)-(dY*dY))/(2*dRR*dSS)))-pi)+(2*pi-(acos(((dSS*dSS)+(dS*dS)-(dQ*dQ))/(2*dS*dSS)))-(acos(((dS*dS)+(dScd*dScd)-(d2*d2))/(2*dS*dScd)))-(acos(((dScd*dScd)+(dSdg*dSdg)-(dScg*dScg))/(2*dScd*dSdg)))-(acos(((dSdg*dSdg)+(dP*dP)-(dN*dN))/(2*dSdg*dP))))-pi)+(2*pi-(acos(((dW*dW)+(dShn*dShn)-(dK*dK))/(2*dW*dShn)))-acos((((sqrt(((dSef*dSef)+(dSeh*dSeh)-(2*dSef*dSeh*cos(pi-(acos(((dP*dP)+(dN*dN)-(dSdg*dSdg))/(2*dP*dN)))-(acos(((dN*dN)+(dSef*dSef)-(d3*d3))/(2*dN*dSef))))))))*(sqrt(((dSef*dSef)+(dSeh*dSeh)-(2*dSef*dSeh*cos(pi-(acos(((dP*dP)+(dN*dN)-(dSdg*dSdg))/(2*dP*dN)))-(acos(((dN*dN)+(dSef*dSef)-(d3*d3))/(2*dN*dSef)))))))))+(dShn*dShn)-(dSfn*dSfn))/(2*(sqrt(((dSef*dSef)+(dSeh*dSeh)-(2*dSef*dSeh*cos(pi-(acos(((dP*dP)+(dN*dN)-(dSdg*dSdg))/(2*dP*dN)))-(acos(((dN*dN)+(dSef*dSef)-(d3*d3))/(2*dN*dSef))))))))*dShn)))-(acos(((dSeh*dSeh)+( (sqrt(((dSef*dSef)+(dSeh*dSeh)-(2*dSef*dSeh*cos(pi-(acos(((dP*dP)+(dN*dN)-(dSdg*dSdg))/(2*dP*dN)))-(acos(((dN*dN)+(dSef*dSef)-(d3*d3))/(2*dN*dSef))))))))* (sqrt(((dSef*dSef)+(dSeh*dSeh)-(2*dSef*dSeh*cos(pi-(acos(((dP*dP)+(dN*dN)-(dSdg*dSdg))/(2*dP*dN)))-(acos(((dN*dN)+(dSef*dSef)-(d3*d3))/(2*dN*dSef)))))))))-(dSef*dSef))/(2*dSeh* (sqrt(((dSef*dSef)+(dSeh*dSeh)-(2*dSef*dSeh*cos(pi-(acos(((dP*dP)+(dN*dN)-(dSdg*dSdg))/(2*dP*dN)))-(acos(((dN*dN)+(dSef*dSef)-(d3*d3))/(2*dN*dSef)))))))))))-pi));

%matice derivací podle mìøených velièin
Q1 = [diff(X5, d1),diff(X5,d2),diff(X5,d3)];

%parametry minibagru Kubota KX71
dRR = 1.09;
dSab = 0.295;
dS = 1.11;
dSS = 1.28;
dScd = 0.345;
dSdg = 0.330;
dQ = 0.425;
dN = 0.89;
dP = 1.075;
dSef = 0.225;
dK = 0.705;
dScg = 0.510;
dSeh = 0.125;
dW = 0.700;
dY = 2.24;
dShn = 0.125;
dSfn = 0.225;
d1 = 1.0;
d2 = 0.9;
d3 = 0.8;
Q = subs(Q1);

%tvorba váhové matice
m1 = 0.016;
m2 = 0.016;
m3 = 0.016;
M = [1/m1;1/m2;1/m3];
P = diag(M);

%výpoèet kovariaèní matice
kov_mat = Q*P*Q';
sm_odch = sqrt(kov_mat);
sm_odch
