% effettua la calibrazione di un robot a 2 GDL (quello proposto nelle dispense all'inizio con due coppie rotoidali)

% gli errori sono presenti sia sulle lunghezze del link che sulle posizioni angolari degli encoder

% parametri geometrici
l1=100;                 % [mm]
l2=90;

% Errori introdotti
delta_l1=3             %[mm]
delta_l2=-2            %[mm]
delta_q1=0.01          % [rad]
delta_q2=-0.02         % [rad]

n=5;                   % numero di punti misurato
% assegno dei valori casuali a q1 e q2 (10 valori)
vet_q1=rand([n,1])*2*pi;
vet_q2=rand([n,1])*2*pi;

% inizializzazione delle matrici
delta_P=[];
phi=[];

% calcolo dei punti misurati e dei punti nominali
for ii=1:n
    q1=vet_q1(ii);
    q2=vet_q2(ii);
    Pm=Punto_misurato(q1,q2,l1,l2,delta_l1,delta_l2,delta_q1,delta_q2);         % punto misurato
    Pn=l1*[cos(q1);sin(q1)]+l2*[cos(q1+q2);sin(q1+q2)];                         % punto nominale
    mat_der_parziali=de_f_de_delta(q1,q2,l1,l2);                                % matrice delle derivate parziali
    
    % si assemblano le varie matrici 
    delta_P=[delta_P;Pm-Pn];
    phi=[phi;mat_der_parziali];
end

% soluzione attraverso la pseudoinversa
risultato=inv(phi'*phi)*phi'*delta_P