% TOGAC46_RGC_RealIdentification.m

% This file will analyze the input and output velocity data for the X Y and
% Z axis of the real galen robot for tests that are 10 seconds long on a
% chip input from 0.01 to 2 Hz


% Modified 05-4-2023
% Created by Brevin Banks

clc
close all

% True X data

% load the data
Outdata = load('RealSystemCartesianData/IdentificationOut_Xrun1_2023_05_05.txt');
Indata = load('RealSystemCartesianData/IdentificationIn_Xrun1_2023_05_05.txt');

% X data in position 2, more accurate time data in position 8
% Take the derivative of the output position data
Time_data = Outdata(:,8);
data = Outdata(:,2);
Vout = [];
for k=2:length(data(:))
Vout(k-1) = (data(k)-data(k-1))/(Time_data(k)-Time_data(k-1));
end
Vout =[Vout Vout(end)]; % Add lagging points lost due to finite difference method

% get the FFT of the data to determine the necessary cuttoff frequency wc
Ls = length(Vout);% Signal length
Fs = 200;% Sampling frequency
Ts = 1/Fs;% Sampling period
tv = (0:Ls-1)*Ts;        % Time vector

F = fft(Vout);% Calling fft() function for signal Xfv
PS2 = abs(F/Ls);% Double sampling plot
PS1 = PS2(1:Ls/2+1);% Single sampling plot
PS1(2:end-1) = 2*PS1(2:end-1);
f = Fs*(0:(Ls/2))/Ls;


% Appropriate cuttoff frequency for X
wc = 75;
Voutfilter = [0];
Ts = [];
% Apply a first order filter to the Vout data
for k = 2:length(Vout)-2
   
    Ts(k) = (Outdata(k,1)-Outdata(k-1,1)); % Calculate the timestep
    Voutfilter(k) = (Ts(k)*wc*(Vout(k)+Vout(k-1))-Voutfilter(k-1)*(Ts(k)*wc-2))/(2+Ts(k)*wc);% first order low pass filter from p/(s+p)
end
Voutfilter = [Voutfilter Voutfilter(end-1:end)]; % Add lagging points lost due to finite difference method


% Integrate the input velocity to get the position reference
In = cumtrapz(Indata(:,1),Indata(:,2));

figure(1)
subplot(3,1,1)
hold on
plot(Indata(:,1),In)
plot(Time_data,data-data(1))
ylabel('Position (mm)')
legend('Pref','Pout')

subplot(3,1,2)
hold on
plot(Indata(:,1),Indata(:,2))
plot(Time_data,Vout)
plot(Time_data,Voutfilter,'k')
xlabel('Time (s)')
ylabel('Velocity (mm/s)')
legend('Vref','Vout','VoutFilt75HzWc')

subplot(3,1,3)
hold on
plot(f,PS1)
xlabel('Hz')
ylabel('|Amplitude X fft|')

dt = mean(Ts);
Vin = Indata(:,2)';
[tf1,ss2] = IdentifySystem(Voutfilter,Vin,dt)

                          
% TFX = exp(-0.115*s) * 9.883e04/(s^2 + 12.47*s + 1.034e05)

%%
clc
close all

% True Y data

% load the data
Outdata = load('RealSystemCartesianData/IdentificationOut_Yrun1_2023_05_05.txt');
Indata = load('RealSystemCartesianData/IdentificationIn_Yrun1_2023_05_05.txt');

% Y data in position 3, more accurate time data in position 8
% Take the derivative of the output position data
Time_data = Outdata(:,8);
data = Outdata(:,3);
Vout = [];
for k=2:length(data(:))
Vout(k-1) = (data(k)-data(k-1))/(Time_data(k)-Time_data(k-1));
end
Vout =[Vout Vout(end)]; % Add lagging points lost due to finite difference method

% get the FFT of the data to determine the necessary cuttoff frequency wc
Ls = length(Vout);% Signal length
Fs = 200;% Sampling frequency
Ts = 1/Fs;% Sampling period
tv = (0:Ls-1)*Ts;        % Time vector

F = fft(Vout);% Calling fft() function for signal Xfv
PS2 = abs(F/Ls);% Double sampling plot
PS1 = PS2(1:Ls/2+1);% Single sampling plot
PS1(2:end-1) = 2*PS1(2:end-1);
f = Fs*(0:(Ls/2))/Ls;


% Appropriate cuttoff frequency for X
wc = 75;
Voutfilter = [0];
Ts = [];
% Apply a first order filter to the Vout data
for k = 2:length(Vout)-2
   
    Ts(k) = (Outdata(k,1)-Outdata(k-1,1)); % Calculate the timestep
    Voutfilter(k) = (Ts(k)*wc*(Vout(k)+Vout(k-1))-Voutfilter(k-1)*(Ts(k)*wc-2))/(2+Ts(k)*wc);% first order low pass filter from p/(s+p)
end
Voutfilter = [Voutfilter Voutfilter(end-1:end)]; % Add lagging points lost due to finite difference method


% Integrate the input velocity to get the position reference
In = cumtrapz(Indata(:,1),Indata(:,3));

figure(1)
subplot(3,1,1)
hold on
plot(Indata(:,1),In)
plot(Time_data,data-data(1))
ylabel('Position (mm)')
legend('Pref','Pout')
title('Y response from Chirp 0.01 to 0.5')

subplot(3,1,2)
hold on
plot(Indata(:,1),Indata(:,3))
plot(Time_data,Vout)
plot(Time_data,Voutfilter,'k')
xlabel('Time (s)')
ylabel('Velocity (mm/s)')
legend('Vref','Vout','VoutFilt75HzWc')

subplot(3,1,3)
hold on
plot(f,PS1)
xlabel('Hz')
ylabel('|Amplitude Y fft|')

dt = mean(Ts);
Vin = Indata(:,3)';
[tf1,ss2] = IdentifySystem(Voutfilter,Vin,dt)

                            
% TFY = exp(-0.0599*s) * 1.607e07/(s^2 + 1.971e05*s + 1.655e07)

%%
clc
close all

% True Z data

% load the data
Outdata = load('RealSystemCartesianData/IdentificationOut_Zrun1_2023_05_05.txt');
Indata = load('RealSystemCartesianData/IdentificationIn_Zrun1_2023_05_05.txt');

% Z data in position 4, more accurate time data in position 8
% Take the derivative of the output position data
Time_data = Outdata(:,8);
data = Outdata(:,4);
Vout = [];
for k=2:length(data(:))
Vout(k-1) = (data(k)-data(k-1))/(Time_data(k)-Time_data(k-1));
end
Vout =[Vout Vout(end)]; % Add lagging points lost due to finite difference method

% get the FFT of the data to determine the necessary cuttoff frequency wc
Ls = length(Vout);% Signal length
Fs = 200;% Sampling frequency
Ts = 1/Fs;% Sampling period
tv = (0:Ls-1)*Ts;        % Time vector

F = fft(Vout);% Calling fft() function for signal Xfv
PS2 = abs(F/Ls);% Double sampling plot
PS1 = PS2(1:Ls/2+1);% Single sampling plot
PS1(2:end-1) = 2*PS1(2:end-1);
f = Fs*(0:(Ls/2))/Ls;


% Appropriate cuttoff frequency for X
wc = 75;
Voutfilter = [0];
Ts = [];
% Apply a first order filter to the Vout data
for k = 2:length(Vout)-2
   
    Ts(k) = (Outdata(k,1)-Outdata(k-1,1)); % Calculate the timestep
    Voutfilter(k) = (Ts(k)*wc*(Vout(k)+Vout(k-1))-Voutfilter(k-1)*(Ts(k)*wc-2))/(2+Ts(k)*wc);% first order low pass filter from p/(s+p)
end
Voutfilter = [Voutfilter Voutfilter(end-1:end)]; % Add lagging points lost due to finite difference method


% Integrate the input velocity to get the position reference
In = cumtrapz(Indata(:,1),Indata(:,4));

figure(1)
subplot(3,1,1)
hold on
plot(Indata(:,1),In)
plot(Time_data,data-data(1))
ylabel('Position (mm)')
legend('Pref','Pout')
title('Z response from Chirp 0.01 to 0.5')

subplot(3,1,2)
hold on
plot(Indata(:,1),Indata(:,4))
plot(Time_data,Vout)
plot(Time_data,Voutfilter,'k')
xlabel('Time (s)')
ylabel('Velocity (mm/s)')
legend('Vref','Vout','VoutFilt75HzWc')

subplot(3,1,3)
hold on
plot(f,PS1)
xlabel('Hz')
ylabel('|Amplitude Z fft|')

dt = mean(Ts);
Vin = Indata(:,4)';
[tf1,ss2] = IdentifySystem(Voutfilter,Vin,dt)

                          
% TFZ = exp(-0.015*s)*2.075e04/(s^2 + 749.4*s + 2.098e04)

%%

% This function will take input and output data from a control response
%
% Inputs:
%   Vout - output data from control response
%   Vin - input data from control
%   dt - the estimated sampling time used for control
% Outputs:
%   Gest - the estimated transfer function using the tfest method with 2nd
%   order guess
%   Gest2 - the estimated transfer functino using the ssest method
function [Gest, Gest2] = IdentifySystem(Vout,Vin,dt)
s = tf('s');
u = Vin(1,:)';
yreal = Vout';

data = iddata(yreal,u,dt);
Gest = tfest(data,2,0,NaN)


figure(2)
opt = compareOptions;
opt.InitialCondition = 'z';
compare(data,Gest,opt)
set(findall(gca, 'Type', 'Line'), 'LineWidth', 4)
xlabel("Time (s)")
subtitle("2nd Order Est with no Zeros")
ylabel('Velocity (m/s)')
grid on



% estimate the delay term
delay_samples = delayest(data);

% Remove delay from output
yreal_no_delay = yreal(delay_samples+1:end);

% Repackage offset data
data_offset = iddata(yreal_no_delay, u(1:end-delay_samples), dt);

% Fit data to a state space model of unknown oder
Gss = ssest(data_offset, 1:10);
% Convert to tf and add in the delay term
Gest2 = tf(Gss)*exp(-delay_samples * dt * s)


figure(3)
opt = compareOptions;
opt.InitialCondition = 'z';
compare(data,Gest2,opt)
set(findall(gca, 'Type', 'Line'), 'LineWidth', 4)
xlabel(["Time (s)"])
subtitle("SS Estimate")
ylabel('Velocity (m/s)')
grid on



end