% Set membership estimation of a position sensor model

clear all, close all, pack, clc
format compact, format short e

load sensor

% bound on the greatest measurement error of the position z
epsilon=5e-4

% indices corresponding to the linearity interval of the characteristic
i1=14; % use both 11 (at first) and 14 (at the end)
i2=36;

z_lin = z(i1:i2);
Vz_lin=Vz(i1:i2);

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

% parametric estimation by means of least squares algorithm
L=[Vz_lin, ones(i2-i1+1,1)];
A=inv(L'*L)*L';

nu_hat=L\z_lin   % by means of the operator "\"
nu_hat_=A*z_lin; % by means of the pseudoinverse matrix

%-----------------------------------------------------------------
% Evaluation of the uncertainty intervals EUI in l-infinity norm
%-----------------------------------------------------------------

for ind=1:length(nu_hat),
    nu_min(ind,1)=A(ind,:)*(z_lin-epsilon*sign(A(ind,:))');
end
nu_min
nu_max=2*nu_hat-nu_min

% check

nu_min_=[0;0]; nu_max_=[0;0];
for k=1:length(A),
    nu_min_(1)=nu_min_(1)+A(1,k)*(z_lin(k)-epsilon*sign(A(1,k)));
    nu_min_(2)=nu_min_(2)+A(2,k)*(z_lin(k)-epsilon*sign(A(2,k)));
    nu_max_(1)=nu_max_(1)+A(1,k)*(z_lin(k)+epsilon*sign(A(1,k)));
    nu_max_(2)=nu_max_(2)+A(2,k)*(z_lin(k)+epsilon*sign(A(2,k)));
end
nu_min_
nu_max_
nu_max__=2*nu_hat-nu_min_

EUI=[nu_min, nu_max]
Kt_min=1/EUI(1,2)
Kt_max=1/EUI(1,1)
Vo_min=-EUI(2,2)/EUI(1,1)
Vo_max=-EUI(2,1)/EUI(1,2)

% Comparison of the results

Vz0=linspace(-10,10,1000);
z_hat=nu_hat(1)*Vz0+nu_hat(2);
z_min=min([nu_max(1)*Vz0; nu_min(1)*Vz0])+nu_min(2);
z_max=max([nu_max(1)*Vz0; nu_min(1)*Vz0])+nu_max(2);

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}^{M} * V_{z} , \theta_{1}^{m} * V_{z}) + \theta_{2}^{m}', ...
       'z = \theta_{1} * V_{z} + \theta_2', ...
       'z \approx max(\theta_{1}^{M} * V_{z} , \theta_{1}^{m} * V_{z}) + \theta_{2}^{M}', 2),
text(0.015,-9,'\fontsize{20} linearity interval'),
print -deps sensore5.eps

%-----------------------------------------------------------------
% Evaluation of the uncertainty intervals PUI in l-infinity norm
%-----------------------------------------------------------------

% PUI_LP(1,1)=[1; 0]'*lp( [1; 0],[L; -L],[z_lin+epsilon; -z_lin+epsilon]); % Under Matlab 5.3
% PUI_LP(2,1)=[0; 1]'*lp( [0; 1],[L; -L],[z_lin+epsilon; -z_lin+epsilon]); % Under Matlab 5.3
% PUI_LP(1,2)=[1; 0]'*lp(-[1; 0],[L; -L],[z_lin+epsilon; -z_lin+epsilon]); % Under Matlab 5.3
% PUI_LP(2,2)=[0; 1]'*lp(-[0; 1],[L; -L],[z_lin+epsilon; -z_lin+epsilon]); % Under Matlab 5.3

options_old=optimset('linprog'); % the default value for the 'LargeScale' parameter is 'on'
options_new=optimset(options_old, 'LargeScale','off'); 
PUI(1,1)=[1; 0]'*linprog( [1; 0],[L; -L],[z_lin+epsilon; -z_lin+epsilon],[],[],[],[],[],options_new);
PUI(2,1)=[0; 1]'*linprog( [0; 1],[L; -L],[z_lin+epsilon; -z_lin+epsilon],[],[],[],[],[],options_new);
PUI(1,2)=[1; 0]'*linprog(-[1; 0],[L; -L],[z_lin+epsilon; -z_lin+epsilon],[],[],[],[],[],options_new);
PUI(2,2)=[0; 1]'*linprog(-[0; 1],[L; -L],[z_lin+epsilon; -z_lin+epsilon],[],[],[],[],[],options_new);

PUI

Kt_min=1/PUI(1,2)
Kt_max=1/PUI(1,1)
Vo_min=-PUI(2,2)/PUI(1,1)
Vo_max=-PUI(2,1)/PUI(1,2)

%-------------------------------------------------------------------
% Approximation of the uncertainty intervals PUI in l-infinity norm
%-------------------------------------------------------------------

n=16;
theta_vect=[];
for k=0:n-1,
    theta=linprog([sin(2*pi*k/n), cos(2*pi*k/n)],[L; -L],[z_lin+epsilon; -z_lin+epsilon],[],[],[],[],[],options_new);
    theta_vect=[theta_vect, theta];
end

%-----------------------------------------------------------------
% Evaluation of the central estimate
%-----------------------------------------------------------------

nu_central=[(PUI(1,1)+PUI(1,2))/2; (PUI(2,1)+PUI(2,2))/2]
Kt_central=1/nu_central(1)
Vo_central=-nu_central(2)/nu_central(1)

% Comparison of the results

clear z_min z_max
nu_vect=[];
for k=1:length(Vz_lin),
    % nu=lp([Vz_lin(k); 1],[L; -L],[z_lin+epsilon; -z_lin+epsilon]); % Under Matlab 5.3
    nu=linprog([Vz_lin(k); 1],[L; -L],[z_lin+epsilon; -z_lin+epsilon],[],[],[],[],[],options_new);
    nu_vect=[nu_vect, nu];
    z_min(k)=[Vz_lin(k), 1]*nu;
    % nu=lp(-[Vz_lin(k); 1],[L; -L],[z_lin+epsilon; -z_lin+epsilon]); % Under Matlab 5.3
    nu=linprog(-[Vz_lin(k); 1],[L; -L],[z_lin+epsilon; -z_lin+epsilon],[],[],[],[],[],options_new);
    nu_vect=[nu_vect, nu];
    z_max(k)=[Vz_lin(k), 1]*nu;
end

figure, plot(z,Vz,'o')
child=get(get(gcf,'Child'),'Child'); set(child(1),'MarkerSize',3);
hold on,
plot(z_hat,Vz0,'-', z_min,Vz_lin,'--', z_max,Vz_lin,'-.', ...
     z(i1)*[1,1],[-10,10],'-', z(i2)*[1,1],[-10,10],'-'),
plot_dx(z_lin,Vz_lin,epsilon,epsilon*0)
hold off,
axis([min(z),max(z),-10,10]), grid on, zoom on, title('Position transducer'),
xlabel('Position{\it z} in m'), ylabel('Voltage{\it V_z} in V'),
legend('Experimental data (z, V_z)_{ }^{  }', ...
       'z = \theta_{1} * V_{z} + \theta_2', 2),
text(0.015,-9,'\fontsize{20} linearity interval'),
print -deps sensore6.eps

k=convhull(nu_vect(1,:),nu_vect(2,:));
figure, plot(nu_vect(1,k),nu_vect(2,k),'-',nu_hat(1),nu_hat(2),'*', ...
   [PUI(1,1),PUI(1,:),PUI(1,2:-1:1)], [PUI(2,2:-1:1),PUI(2,:),PUI(2,2)],'--', ...
   nu_central(1),nu_central(2),'*k'),
grid on, text(nu_hat(1)*0.9975,nu_hat(2),'\phi^{LS}(y)'),
text(nu_central(1)*0.9975,nu_central(2)*0.9997,'\phi^{C}(y)'),
title('Feasible Parameter Set {\it FPS^\infty}'), 
xlabel('\theta_{1}'), ylabel('\theta_{2}')
print -deps sensore7.eps

k1=convhull(theta_vect(1,:),theta_vect(2,:));
figure, plot(theta_vect(1,k1),theta_vect(2,k1),'-',nu_hat(1),nu_hat(2),'*', ...
   [PUI(1,1),PUI(1,:),PUI(1,2:-1:1)], [PUI(2,2:-1:1),PUI(2,:),PUI(2,2)],'--', ...
   nu_central(1),nu_central(2),'*k'),
grid on, text(nu_hat(1)*0.9975,nu_hat(2),'\phi^{LS}(y)'),
text(nu_central(1)*0.9975,nu_central(2)*0.9997,'\phi^{C}(y)'),
title('Feasible Parameter Set {\it FPS^\infty}'), 
xlabel('\theta_{1}'), ylabel('\theta_{2}')
print -deps sensore8.eps