%% Filtering Gyro and Acc Data with Low pass and High pass ButterWorth filters
% ----------------------Low Pass Filt---------------------
Gyro_LPF_data = []; 
Acc_LPF_data = [];

Acc_HPF_data = [];
Gyro_HPF_data = [];
l = linspace(1,length(Acc_data), length(Acc_data));
% %Accelerometer data filtering ButterWorth
Wp = 4/50;
Ws = 45/50;
[n,Wn] = buttord(Wp,Ws,2,40);
[z,p] = butter(n,Wn); %2,4
Acc_LPF_data = filter(z,p,Acc_data);
figure;
plot(l,Acc_data(:,1),l,Acc_LPF_data(:,1));
legend('Acc Original Signal Order 15 and Cutoff Freq 40','ButterWorth Filtered Signal');
figure;
plot(l,Acc_data(:,2),l,Acc_LPF_data(:,2));
legend('Acc Original Signal Order 15 and Cutoff Freq 40','ButterWorth Filtered Signal');

%Gyro data filtering ButterWorth
l1 = linspace(1,length(gyro_data), length(gyro_data));
Wp = 2/50;
Ws = 49/50;
[n,Wn] = buttord(Wp,Ws,2,60);
[b,a] = butter(n,Wn); %2,4
% [m n]=size(A.data);
Gyro_LPF_data = filter(b,a,gyro_data);
figure;
plot(l1,gyro_data(:,1),l1,Gyro_LPF_data(:,1));
legend('Gyro Original Signal Order 2 Cutoff Freq 0.5179','ButterWorth Filtered Signal');

% % % ------------------HIGH Pass Filt------------------
Wp = 1/50;
Ws = 45/50;
[n,Wn] = buttord(Wp,Ws,1,45);
[z,p] = butter(n,Wn,'high');
Acc_HPF_data = filter(z,p,Acc_data);
figure;

plot(l,Acc_data(:,1),l,Acc_HPF_data(:,1));
legend('Acc Original Signal Order 15 and Cutoff Freq 40','ButterWorth Filtered Signal');
figure;
plot(l,Acc_data(:,2),l,Acc_HPF_data(:,2));
legend('Acc Original Signal Order 15 and Cutoff Freq 40','ButterWorth Filtered Signal');
%
Wp = 2/50;
Ws = 25/50;
[n,Wn] = buttord(Wp,Ws,2,60);
[z,p] = butter(n,Wn,'high');
Gyro_HPF_data = filter(z,p,gyro_data);
figure;
plot(l1,gyro_data(:,1),l1,Gyro_HPF_data(:,1));
legend('Gyro Original Signal Order 15 and Cutoff Freq 40','ButterWorth Filtered Signal');
figure;
plot(l1,gyro_data(:,2),l1,Gyro_HPF_data(:,2));
legend('Gyro Original Signal Order 15 and Cutoff Freq 40','ButterWorth Filtered Signal');


filename = 'D:\MATLAB\4th Conf\Acc_HPF_data';
save( filename,'Acc_HPF_data' );
filename = 'D:\MATLAB\4th Conf\Acc_LPF_data';
save( filename,'Acc_LPF_data' );
filename = 'D:\MATLAB\4th Conf\Gyro_HPF_data';
save( filename,'Gyro_HPF_data' );
filename = 'D:\MATLAB\4th Conf\Gyro_LPF_data';
save( filename,'Gyro_LPF_data' );

% Add the Acc+Gyro Angles together
for i = 1:length(Acc_Axr)
   Acc_Gyro_Angles_Added_together_x(i,:) =  (Acc_Axr(i,:) + Gyro_Rx(i,:))/2;
   Acc_Gyro_Angles_Added_together_y(i,:) =  (Acc_Ayr(i,:) + Gyro_Ry(i,:))/2;
   Acc_Gyro_Angles_Added_together_z(i,:) =  (Acc_Azr(i,:) + Gyro_Rz(i,:))/2;
    
end
filename = 'D:\MATLAB\4th Conf\Acc_Gyro_Angles_Added_together_x';
save( filename,'Acc_Gyro_Angles_Added_together_x' );
filename = 'D:\MATLAB\4th Conf\Acc_Gyro_Angles_Added_together_y';
save( filename,'Acc_Gyro_Angles_Added_together_y' );
filename = 'D:\MATLAB\4th Conf\Acc_Gyro_Angles_Added_together_z';
save( filename,'Acc_Gyro_Angles_Added_together_z' );