clear all;
close all;
clc;

load posizioniNM 
load stimeC
load valori

for t=1:length(r)
    
    res_x = fstar_x-test_Y_x;
    res_y = fstar_y-test_Y_y;
    mse_x1(t) = mean(res_x.^2);
    mse_y1(t) = mean(res_y.^2);
    em_x1(t) = mean(abs(res_x)); 
    em_y1(t) = mean(abs(res_y)); 
    max_x1(t) = max(abs(res_x));
    max_y1(t) = max(abs(res_y));
    
    res_x = stima_x21(:,t)-test_Y_x;
    res_y = stima_y21(:,t)-test_Y_y;
    mse_x21(t) = mean(res_x.^2);
    mse_y21(t) = mean(res_y.^2);
    em_x21(t) = mean(abs(res_x)); 
    em_y21(t) = mean(abs(res_y)); 
    max_x21(t) = max(abs(res_x));
    max_y21(t) = max(abs(res_y));
    
    res_x = stima_x22(:,t)-test_Y_x;
    res_y = stima_y22(:,t)-test_Y_y;
    mse_x22(t) = mean(res_x.^2);
    mse_y22(t) = mean(res_y.^2);
    em_x22(t) = mean(abs(res_x)); 
    em_y22(t) = mean(abs(res_y)); 
    max_x22(t) = max(abs(res_x));
    max_y22(t) = max(abs(res_y));
    
    res_x = stima_x31(:,t)-test_Y_x;
    res_y = stima_y31(:,t)-test_Y_y;
    mse_x31(t) = mean(res_x.^2);
    mse_y31(t) = mean(res_y.^2);
    em_x31(t) = mean(abs(res_x)); 
    em_y31(t) = mean(abs(res_y)); 
    max_x31(t) = max(abs(res_x));
    max_y31(t) = max(abs(res_y));
    
    res_x = stima_x32(:,t)-test_Y_x;
    res_y = stima_y32(:,t)-test_Y_y;
    mse_x32(t) = mean(res_x.^2);
    mse_y32(t) = mean(res_y.^2);
    em_x32(t) = mean(abs(res_x)); 
    em_y32(t) = mean(abs(res_y)); 
    max_x32(t) = max(abs(res_x));
    max_y32(t) = max(abs(res_y));
end

figure
subplot(3,1,1)
plot(r,em_x1,'o-')
hold on;
plot(r,em_x21,'yo-')
hold on;
plot(r,em_x22,'ro-')
hold on;
plot(r,em_x31,'go-')
hold on;
plot(r,em_x32,'ko-')
grid on;
title('Asse x: Errore medio di stima al variare di r');
xlabel('r [m]');
ylabel('Errore medio [m]');
legend('Stima centralizzata','Stima decentralizzata, metodo 21','Stima decentralizzata, metodo 22','Stima decentralizzata, metodo 31','Stima decentralizzata, metodo 32');
subplot(3,1,2)
plot(r,mse_x1,'o-')
hold on;
plot(r,mse_x21,'yo-')
hold on;
plot(r,mse_x22,'ro-')
hold on;
plot(r,mse_x31,'go-')
hold on;
plot(r,mse_x32,'ko-')
grid on;
title('Asse x: Scarto quadratico medio di stima al variare di r');
xlabel('r [m]');
ylabel('Scarto quadratico medio [m^2]');
subplot(3,1,3)
plot(r,max_x1,'o-')
hold on;
plot(r,max_x21,'yo-')
hold on;
plot(r,max_x22,'ro-')
hold on;
plot(r,max_x31,'go-')
hold on;
plot(r,max_x32,'ko-')
grid on;
title('Asse x: Errore massimo di stima al variare di r');
xlabel('r [m]');
ylabel('Errore massimo [m]');

figure
subplot(3,1,1)
plot(r,em_y1,'o-')
hold on;
plot(r,em_y21,'yo-')
hold on;
plot(r,em_y22,'ro-')
hold on;
plot(r,em_y31,'go-')
hold on;
plot(r,em_y32,'ko-')
grid on;
title('Asse y: Errore medio di stima al variare di r');
xlabel('r [m]');
ylabel('Errore medio [m]');
legend('Stima centralizzata','Stima decentralizzata, metodo 21','Stima decentralizzata, metodo 22','Stima decentralizzata, metodo 31','Stima decentralizzata, metodo 32');
subplot(3,1,2)
plot(r,mse_y1,'o-')
hold on;
plot(r,mse_y21,'yo-')
hold on;
plot(r,mse_y22,'ro-')
hold on;
plot(r,mse_y31,'go-')
hold on;
plot(r,mse_y32,'ko-')
grid on;
title('Asse y: Scarto quadratico medio di stima al variare di r');
xlabel('r [m]');
ylabel('Scarto quadratico medio [m^2]');
subplot(3,1,3)
plot(r,max_y1,'o-')
hold on;
plot(r,max_y21,'yo-')
hold on;
plot(r,max_y22,'ro-')
hold on;
plot(r,max_y31,'go-')
hold on;
plot(r,max_y32,'ko-')
grid on;
title('Asse y: Errore massimo di stima al variare di r');
xlabel('r [m]');
ylabel('Errore massimo [m]');
