% Parametric estimation of a position sensor model

clear all, close all, pack, clc

% position measured in meters, with a position offset
z_misurata=(0.037:0.001:0.08)';

% position measured in meters, without position offset
z=z_misurata-z_misurata(1);

% output voltage measured in volts
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 sensor z Vz

plot(z,Vz,'o'), grid, title('Position transducer'),
xlabel('Position{\it z} in m'), ylabel('Voltage{\it V_z} in V'),
print -deps sensore1.eps

%-----------------------------------------------------------------
% Parametric estimation of a linear model using least squares
%-----------------------------------------------------------------

% indices corresponding to the linearity interval of the characteristic
i1=11;
i2=36;

% parametric estimation by means of least squares algorithm
L=[Vz(i1:i2), ones(i2-i1+1,1)];
p=L\z(i1:i2); % by means of the operator "\"
Kt=1/p(1)
Vo=-p(2)/p(1)

p_=inv(L'*L)*L'*z(i1:i2); % computing the pseudoinverse matrix
Kt_=1/p_(1)
Vo_=-p_(2)/p_(1)

% computation of the x% confidence intervals
% x=95 => k=2 ("2 sigma"); x=99.7 => k=3 ("3 sigma")
k_e=2; k_p=2;
rumore_max=5e-4;

% case #1: known variance
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)

% case #2: variance to be estimated
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)

% Comparison of the results

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('Position transducer'),
xlabel('Position{\it z} in m'), ylabel('Voltage{\it V_z} in V'),
text(0.013,-9,'\fontsize{20} linearity interval'),
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('Position transducer'),
xlabel('Position{\it z} in m'), ylabel('Voltage{\it V_z} in V'),
legend('Experimental data (z, V_z)_{ }^{  }', ...
       'z \approx min(\theta_{1}^{+} * V_{z} , \theta_{1}^{-} * V_{z}) + \theta_{2}^{-}', ...
       'z \approx \theta_{1} * V_{z} + \theta_2', ...
       'z \approx max(\theta_{1}^{+} * V_{z} , \theta_{1}^{-} * V_{z}) + \theta_{2}^{+}', 2),
text(0.013,-9,'\fontsize{20} linearity interval'),
print -deps sensore3.eps

%-------------------------------------------------------
% 3rd order polynomial model by means of least squares
%-------------------------------------------------------

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

% b) Vz = theta(1)*z^3 + theta(2)*z^2 + theta(3)*z + theta(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('Position transducer'),
xlabel('Position{\it z} in m'), ylabel('Voltage{\it V_z} in V'),
legend('Experimental data (z, V_z)_{ }^{  }', ...
       'z \approx \theta_1*V_z^3+\theta_2*V_z^2+\theta_3*V_z+\theta_4', ...
       'V_z \approx \theta_1*z^3+theta_2*z^2+\theta_3*z+\theta_4',4),
print -deps sensore4.eps

%-------------------------------------------------------
% 3rd order polymonial model by means of polyfit
%-------------------------------------------------------
modello_ordine3=polyfit(z,Vz,3);
Vz_modello_ordine3=polyval(modello_ordine3,z0);