% Identificazione del modello di un sensore di posizione

clear all, close all, pack, clc

% posizione misurata in metri, con offset di posizione
z_misurata=(0.037:0.001:0.08)';

% posizione misurata in metri, senza offset di posizione
z=z_misurata-z_misurata(1);

% tensione di uscita misurata in volt
Vz=[-6.9522; -6.9530; -6.9873; -6.9922; -6.9868; -6.9649; -6.9563; -6.8359; ...
    -6.7097; -6.6211; -6.3014; -5.9746; -5.4875; -5.0964; -4.7653; -4.3603; -3.6474; ...
    -3.1056; -2.6439; -2.2799; -1.7218; -1.0987; -0.5180;  0.0326;  0.5958;  1.1052; ...
     1.8264;  2.3193;  2.9003;  3.4942;  4.2122;  4.6871;  5.1238;  5.5129;  6.0731; ...
     6.6401;  6.8797;  6.9531;  7.0979;  7.2823;  7.2941;  7.2998;  7.3008;  7.3191];

save sensore z Vz

plot(z,Vz,'o'), grid, title('Trasduttore di posizione'),
xlabel('Posizione{\it z} in m'), ylabel('Tensione{\it V_z} in V'),
print -deps sensore1.eps

%-----------------------------------------------------------------
% Stima dei parametri di un modello lineare con i minimi quadrati
%-----------------------------------------------------------------

% indici corrispondenti all'intervallo di linearita' della caratteristica
i1=11;
i2=36;

% identificazione dei parametri mediante i minimi quadrati;
L=[Vz(i1:i2), ones(i2-i1+1,1)];
p=L\z(i1:i2); % con l'operatore "\"
Kt=1/p(1)
Vo=-p(2)/p(1)

p_=inv(L'*L)*L'*z(i1:i2); % calcolando la pseudoinversa
Kt_=1/p_(1)
Vo_=-p_(2)/p_(1)

% calcolo degli intervalli di confidenza all'x %
% x=95 => k=2 ("2 sigma"); x=99.7 => k=3 ("3 sigma")
k_e=2; k_p=2;
rumore_max=5e-4;

% caso #1: varianza nota
sigma_e=rumore_max/k_e
Sigma_e=sigma_e^2*eye(i2-i1+1);
Sigma_p=inv(L'*inv(Sigma_e)*L);
sigma_p=sqrt(diag(Sigma_p));
delta_p=k_p*sigma_p;
p_min=p-delta_p;
p_max=p+delta_p;
Kt_min=1/p_max(1)
Kt_max=1/p_min(1)
Vo_min=-p_max(2)/p_min(1)
Vo_max=-p_min(2)/p_max(1)

% caso #2: varianza da stimare
sigma_e_mv=sqrt((z(i1:i2)-L*p)'*(z(i1:i2)-L*p)/(i2-i1+1))
Sigma_e=sigma_e_mv^2*eye(i2-i1+1);
Sigma_p=inv(L'*inv(Sigma_e)*L);
sigma_p=sqrt(diag(Sigma_p));
delta_p=k_p*sigma_p;
p_min=p-delta_p;
p_max=p+delta_p;
Kt_min=1/p_max(1)
Kt_max=1/p_min(1)
Vo_min=-p_max(2)/p_min(1)
Vo_max=-p_min(2)/p_max(1)

% Confronto dei risultati ottenuti

Vz0=linspace(-10,10,1000);
z_hat=p(1)*Vz0+p(2);
z_min=min([p_max(1)*Vz0; p_min(1)*Vz0])+p_min(2);
z_max=max([p_max(1)*Vz0; p_min(1)*Vz0])+p_max(2);

figure, plot(z,Vz,'o',z(i1)*[1,1],[-10,10],'-',z(i2)*[1,1],[-10,10],'-',z_hat,Vz0),
axis([min(z),max(z),-10,10]), grid, title('Trasduttore di posizione'),
xlabel('Posizione{\it z} in m'), ylabel('Tensione{\it V_z} in V'),
text(0.013,-9,'\fontsize{20} intervallo di linearita'''),
print -deps sensore2.eps

figure, plot(z,Vz,'o', z_min,Vz0,'--', z_hat,Vz0,'-', z_max,Vz0,'-.', ...
             z(i1)*[1,1],[-10,10],'-', z(i2)*[1,1],[-10,10],'-'),
axis([min(z),max(z),-10,10]), grid, title('Trasduttore di posizione'),
xlabel('Posizione{\it z} in m'), ylabel('Tensione{\it V_z} in V'),
legend('Dati sperimentali (z, V_z)_{ }^{  }', ...
       'z \approx min(p_{1}^{+} * V_{z} , p_{1}^{-} * V_{z}) + p_{2}^{-}', ...
       'z \approx p_{1} * V_{z} + p_2', ...
       'z \approx max(p_{1}^{+} * V_{z} , p_{1}^{-} * V_{z}) + p_{2}^{+}', 2),
text(0.013,-9,'\fontsize{20} intervallo di linearita'''),
print -deps sensore3.eps

%-------------------------------------------------------
% Modello polinomiale di ordine 3 con i minimi quadrati
%-------------------------------------------------------

% a) z = p(1)*Vz^3 + p(2)*Vz^2 + p(3)*Vz + p(4)
L=[Vz.^3, Vz.^2, Vz, Vz.^0];
p3a=L\z
z_pol=polyval(p3a,Vz0);

% b) Vz = p(1)*z^3 + p(2)*z^2 + p(3)*z + p(4)
L=[z.^3, z.^2, z, z.^0];
p3b=L\Vz
z0=linspace(min(z),max(z),1000);
Vz_pol=polyval(p3b,z0);

figure, plot(z,Vz,'o',z_pol,Vz0,'--',z0,Vz_pol,'-'),
axis([min(z),max(z),-10,10]), grid, title('Trasduttore di posizione'),
xlabel('Posizione{\it z} in m'), ylabel('Tensione{\it V_z} in V'),
legend('Dati sperimentali (z, V_z)_{ }^{  }', ...
       'z \approx p_1*V_z^3+p_2*V_z^2+p_3*V_z+p_4', ...
       'V_z \approx p_1*z^3+p_2*z^2+p_3*z+p_4',4),
print -deps sensore4.eps

%-------------------------------------------------------
% Modello polinomiale di ordine 3 calcolato con polyfit
%-------------------------------------------------------
modello_ordine3=polyfit(z,Vz,3);
Vz_modello_ordine3=polyval(modello_ordine3,z0);