%% Data from ultrasound sensors

close all; clear all; clc;


%% Filtro media - 5 elementos

[time,dist_s1,dist_s2,dist_s3,dist_filt_s1,dist_filt_s2,dist_filt_s3,vel_s1,vel_s2,vel_s3,vel_filt_s1,vel_filt_s2,vel_filt_s3] = textread('rcdu_data_7-7-10_20Hz_5elem_1','%d %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);
dist_s1=filter(ones(1,windowSize)/windowSize,1,dist_s1);
dist_s2_posfilter=filter(ones(1,windowSize)/windowSize,1,dist_s2);
dist_s3=filter(ones(1,windowSize)/windowSize,1,dist_s3);

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

dist_s2_posfilter(1:10)=dist_s2_posfilter(11);

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

subplot(1,3,2); plot(time, vel_s1, 'r');
TITLE('velocidade instantanea (m/s)');
xlabel('time');
ylabel('velocidade instantanea (m/s)');
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 5 elementos (m/s)');
xlabel('time');
ylabel('velocidade media 5 elementos (cm)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

figure();	%------

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

subplot(1,2,2); plot(time, dist_s2_posfilter, 'r');
TITLE('Distância Filtro Média 10 Elementos');
xlabel('Iterações');
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 5 elementos (m/s)');
% xlabel('time');
% ylabel('velocidade media 5 elementos (cm)');
% legend('Sensor 2')
% axis([x_min x_max y_min y_max]);

figure()    %------

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

subplot(1,3,2); plot(time, vel_s3, 'r');
TITLE('velocidade instantanea (m/s)');
xlabel('time');
ylabel('velocidade instantanea (m/s)');
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 5 elementos (m/s)');
xlabel('time');
ylabel('velocidade media 5 elementos (cm)');
legend('Sensor 3')
axis([x_min x_max y_min y_max]);

%% Filtro media - 10 elementos

[time,dist_s1,dist_s2,dist_s3,dist_filt_s1,dist_filt_s2,dist_filt_s3,vel_s1,vel_s2,vel_s3,vel_filt_s1,vel_filt_s2,vel_filt_s3] = textread('rcdu_data_7-7-10_20Hz_10elem_0','%d %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_filt_s1, 'b');
TITLE('Distancia (m)');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

subplot(1,3,2); plot(time, vel_s1, 'r');
TITLE('velocidade instantanea (m/s)');
xlabel('time');
ylabel('velocidade instantanea (m/s)');
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 (m/s)');
xlabel('time');
ylabel('velocidade media 10 elementos (cm)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

figure();

subplot(1,3,1); plot(time, dist_filt_s2, 'b');
TITLE('Distância Original (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 (m/s)');
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('Velocidade com filtro média 10 elementos (m/s)');
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_filt_s3, 'b');
TITLE('Distancia (m)');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 3')
axis([x_min x_max y_min y_max]);

subplot(1,3,2); plot(time, vel_s3, 'r');
TITLE('velocidade instantanea (m/s)');
xlabel('time');
ylabel('velocidade instantanea (m/s)');
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 (cm)');
legend('Sensor 3')
axis([x_min x_max y_min y_max]);


%% Filtro media - 10 elementos - myway

[time,dist_s1,dist_s2,dist_s3,dist_filt_s1,dist_filt_s2,dist_filt_s3,vel_s1,vel_s2,vel_s3,vel_filt_s1,vel_filt_s2,vel_filt_s3] = textread('rcdu_data_10e_mway_0','%d %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_filt_s1, 'b');
TITLE('Distância (m)');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

subplot(1,3,2); plot(time, vel_s1, 'r');
TITLE('velocidade instantanea (m/s)');
xlabel('time');
ylabel('velocidade instantanea (m/s)');
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 (cm)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

figure();

subplot(1,3,1); plot(time, dist_filt_s2, 'b');
TITLE('Distância Filtrada');
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('Velocidade média 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_filt_s3, 'b');
TITLE('Distancia (m)');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 3')
axis([x_min x_max y_min y_max]);

subplot(1,3,2); plot(time, vel_s3, 'r');
TITLE('velocidade instantanea (m/s)');
xlabel('time');
ylabel('velocidade instantanea (m/s)');
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 (cm)');
legend('Sensor 3')
axis([x_min x_max y_min y_max]);

%% Filtro mÃ©dia - 10Hz - 10 elementos

[time,dist_s1,dist_s2,dist_s3,dist_filt_s1,dist_filt_s2,dist_filt_s3,vel_s1,vel_s2,vel_s3,vel_filt_s1,vel_filt_s2,vel_filt_s3] = textread('rcdu_data_9-7-10_10hz_10elem_1','%d %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_filt_s1, 'b');
TITLE('Distancia (m)');
xlabel('time');
ylabel('DistÃ¢ncia (m)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

subplot(1,3,2); plot(time, vel_s1, 'r');
TITLE('velocidade instantanea (m/s)');
xlabel('time');
ylabel('velocidade instantanea (m/s)');
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 (m/s)');
xlabel('time');
ylabel('velocidade media 10 elementos (cm)');
legend('Sensor 1')
axis([x_min x_max y_min y_max]);

figure();

subplot(1,3,1); plot(time, dist_filt_s2, 'b');
TITLE('Distancia (m)');
xlabel('time');
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 instantanea (m/s)');
xlabel('time');
ylabel('velocidade instantanea (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('velocidade mÃ©dia 10 elementos (m/s)');
xlabel('time');
ylabel('velocidade media 10 elementos (cm)');
legend('Sensor 2')
axis([x_min x_max y_min y_max]);


figure()

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

subplot(1,3,2); plot(time, vel_s3, 'r');
TITLE('velocidade instantanea (m/s)');
xlabel('time');
ylabel('velocidade instantanea (m/s)');
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 (cm)');
legend('Sensor 3')
axis([x_min x_max y_min y_max]);

