%% Data from ultrasound sensors

close all; clear all; clc;


%% Distancia (filtro mediana-9 elementos) Velocidade (filtro media-10 elementos)

[time,dist_s1,dist_s2,dist_s3,dist_filt_s1,dist_filt_s2,dist_filt_s3,dist_filt2_s1,dist_filt2_s2,dist_filt2_s3, vel_s1,vel_s2,vel_s3,vel_filt_s1,vel_filt_s2,vel_filt_s3] = textread('rcdu_data_vel_mean_10elem_dist_median_9elem','%d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f ','headerlines',1);

time=linspace(1, length(dist_s1), length(dist_s1));
% windowSize = 10;

tmax=max(time);
% s1=filter(ones(1,windowSize)/windowSize,1,s1);
% s2=filter(ones(1,windowSize)/windowSize,1,s2);
% s3=filter(ones(1,windowSize)/windowSize,1,s3);

x_min=0;
x_max=tmax;
y_min=0;
y_max=8;

subplot(1,3,1); plot(time, dist_s1, 'b');
TITLE('DistÃ¢ncia - raw');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

subplot(1,3,2); plot(time, dist_filt2_s1, 'r');
TITLE('DistÃ¢ncia - filtro mediana 9elem');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

subplot(1,3,3); plot(time, vel_filt_s1, 'g');
TITLE('Velocidade mÃ©dia 10 elementos');
xlabel('time');
ylabel('Velocidade media 10 elementos (m/s)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

figure();

subplot(1,3,1); plot(time, dist_filt2_s2, 'b');
TITLE('Distância (m)');
xlabel('Iterações');
ylabel('Distância (m)');
legend('Sensor 2')
axis([x_min x_max y_min y_max]);

subplot(1,3,2); plot(time, vel_s2, 'r');
TITLE('Velocidade Instantânea');
xlabel('Iterações');
ylabel('Velocidade (m/s)');
legend('Sensor 2')
axis([x_min x_max y_min y_max]);

subplot(1,3,3); plot(time, vel_filt_s2, 'g');
TITLE('Filtro média de 10 elementos');
xlabel('Iterações');
ylabel('Velocidade (m/s)');
legend('Sensor 2')
axis([x_min x_max y_min y_max]);

figure()

subplot(1,3,1); plot(time, dist_s3, 'b');
TITLE('DistÃ¢ncia - raw');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 3')
axis([x_min x_max y_min y_max]);

subplot(1,3,2); plot(time, dist_filt2_s3, 'r');
TITLE('DistÃ¢ncia - filtro mediana 9elem');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 3')
axis([x_min x_max y_min y_max]);

subplot(1,3,3); plot(time, vel_filt_s3, 'g');
TITLE('Velocidade mÃ©dia 10 elementos (m/s)');
xlabel('time');
ylabel('Velocidade media 10 elementos (m/s)');
legend('Sensor 3')
axis([x_min x_max y_min y_max]);


%% Distancia (filtro mediana-9 elementos) Velocidade (filtro media-10 elementos)

[time,dist_s1,dist_s2,dist_s3,dist_filt_s1,dist_filt_s2,dist_filt_s3,dist_filt2_s1,dist_filt2_s2,dist_filt2_s3, vel_s1,vel_s2,vel_s3,vel_filt_s1,vel_filt_s2,vel_filt_s3] = textread('rcdu_data_vel_mean_10elem_dist_median_9elem_1','%d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f ','headerlines',1);

time=linspace(1, length(dist_s1), length(dist_s1));
windowSize = 10;

tmax=max(time);
% s1=filter(ones(1,windowSize)/windowSize,1,s1);
% s2=filter(ones(1,windowSize)/windowSize,1,s2);
% s3=filter(ones(1,windowSize)/windowSize,1,s3);

x_min=0;
x_max=tmax;
y_min=0;
y_max=10;

subplot(1,3,1); plot(time, dist_s1, 'b');
TITLE('DistÃ¢ncia - raw');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

subplot(1,3,2); plot(time, dist_filt2_s1, 'r');
TITLE('DistÃ¢ncia - filtro mediana 9elem');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

subplot(1,3,3); plot(time, vel_filt_s1, 'g');
TITLE('Velocidade mÃ©dia 10 elementos');
xlabel('time');
ylabel('Velocidade media 10 elementos (m/s)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

figure();

subplot(1,3,1); plot(time, dist_s2, 'b');
TITLE('DistÃ¢ncia - raw');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 2')
axis([x_min x_max y_min y_max]);

subplot(1,3,2); plot(time, dist_filt2_s2, 'r');
TITLE('distancia - filtro mediana 9elem');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 2')
axis([x_min x_max y_min y_max]);

subplot(1,3,3); plot(time, vel_filt_s2, 'g');
TITLE('Velocidade mÃ©dia 10 elementos');
xlabel('time');
ylabel('Velocidade media 10 elementos (m/s)');
legend('Sensor 2')
axis([x_min x_max y_min y_max]);

figure()

subplot(1,3,1); plot(time, dist_s3, 'b');
TITLE('DistÃ¢ncia - raw');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 3')
axis([x_min x_max y_min y_max]);

subplot(1,3,2); plot(time, dist_filt2_s3, 'r');
TITLE('DistÃ¢ncia - filtro mediana 9elem');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 3')
axis([x_min x_max y_min y_max]);

subplot(1,3,3); plot(time, vel_filt_s3, 'g');
TITLE('Velocidade mÃ©dia 10 elementos (m/s)');
xlabel('time');
ylabel('Velocidade media 10 elementos (m/s)');
legend('Sensor 3')
axis([x_min x_max y_min y_max]);
