clear
close all

order = 1
butter_coef = .6
first = 60;
last = 270;

load id.dat
[a,b] = butter( order, butter_coef )
filtered = filtfilt( a, b, id );

figure
plot_index = 1;
plot(id(first:last,plot_index))
hold
plot(filtered(first:last,plot_index),'r')
hold

figure
plot_index = 2;
plot(id(first:last,plot_index))
hold
plot(filtered(first:last,plot_index),'r')
hold

figure
plot_index = 3;
plot(id(first:last,plot_index))
hold
plot(filtered(first:last,plot_index),'r')
hold

figure
plot_index = 4;
plot(id(first:last,plot_index))
hold
plot(filtered(first:last,plot_index),'r')
hold

figure
plot_index = 5;
plot(id(first:last,plot_index))
hold
plot(filtered(first:last,plot_index),'r')
hold

figure
plot_index = 6;
plot(id(first:last,plot_index))
hold
plot(filtered(first:last,plot_index),'r')
hold

figure
plot_index = 7;
plot(id(first:last,plot_index))
hold
plot(filtered(first:last,plot_index),'r')
hold

id = filtered;

%use the eq of motion
%Define A and B
A(:,1) = id(first:last,5); %command
A(:,2) = id(first:last,4); %sin
A(:,3) = id(first:last,2); %vel
B(:,1) = id(first:last,3); %acc

% x1=(A'*A)^-1*A'*B
temp1 = A' * A;
temp2 = inv(temp1);
temp3 = temp2 * A';
temp4 = temp3 * B;
x1 = temp4 

clear A   
clear B
clear temp1
clear temp2
clear temp3
clear temp4


%integrate
%Define C and D
C(:,1) = id(first:last,7); %int_command
C(:,2) = id(first:last,6); %int_sin
C(:,3) = id(first:last,1) - id(first,1); %pos
D(:,1) = id(first:last,2) - id(first,2); %vel

% x2=(C'*C)^-1*C'*D
temp1 = C' * C;
temp2 = inv(temp1);
temp3 = temp2 * C';
temp4 = temp3 * D;
x2 = temp4 

clear C
clear D
clear temp1
clear temp2
clear temp3
clear temp4

