% TOGAC38_AMBF_CartesianSystemIdent.m

% This file was used to analyze the input and and output data for the galen
% surgical robot in AMBF. This was done with the Cartesian test data from
% the chirp signals to get the transfer functions
% The systems were identified using the tfest method and ssest methods from
% matlab's system identification toolbox
% Created by Brevin Banks
% Modified April 15 2023

%% Chirp input

% load the data
dt = 0.0231; % measured time step per sampling loop
Vout = load('AMBFCartesianData/CartChirp0d001TO5HzXVOUT.mat');
Vin = load('AMBFCartesianData/CartChirp0d001TO5HzXVIN.mat');
Voutx = Vout.cv(1,:);
Vinx = Vin.rv(1,:);

Vout = load('AMBFCartesianData/CartChirp0d001TO5HzYVOUT.mat');
Vin = load('AMBFCartesianData/CartChirp0d001TO5HzYVIN.mat');
Vouty = Vout.cv(2,:);
Viny = Vin.rv(2,:);

Vout = load('AMBFCartesianData/CartChirp0d001TO5HzZVOUT.mat');
Vin = load('AMBFCartesianData/CartChirp0d001TO5HzZVIN.mat');
Voutz = Vout.cv(3,:);
Vinz = Vin.rv(3,:);

Vout = load('AMBFCartesianData/CartChirp0d001TO5HzWxVOUT.mat');
Vin = load('AMBFCartesianData/CartChirp0d001TO5HzWxVIN.mat');
Voutwx = Vout.cv(4,:);
Vinwx = Vin.rv(4,:);

Vout = load('AMBFCartesianData/CartChirp0d001TO5HzWyVOUT.mat');
Vin = load('AMBFCartesianData/CartChirp0d001TO5HzWyVIN.mat');
Voutwy = Vout.cv(5,:);
Vinwy = Vin.rv(5,:);
%%
IdentifySystem(Voutx,Vinx,0.0284)
%%
IdentifySystem(Vouty,Viny,0.0296)
IdentifySystem(Voutz,Vinz,0.0221)
IdentifySystem(Voutwx,Vinwx,0.0222)
IdentifySystem(Voutwy,Vinwy,0.0219)

%%


s = tf('s');
% tfest Transfer resultm for chirp on 0.01 to 20Hz for 180s simultaneous
% X tfest 26.1% match
Xtf = tf([2282],[1  72.45 2360]);
% X ssest 19.75% match
Xest = exp(-0.0231*s) * 37.61/(s + 59.96);
% Y tfest 30.86 % match
Ytf = 1682/(s^2 + 57.13*s + 1761);
% Y ssest 26.4% match
Yest = exp(-0.0231*s) * (24.88*s^2 + 1419*s - 207.8)/(s^3 + 79.87*s^2 + 1998*s + 6.701);
% Z tfest 27.43% match

Ztf = 2205/(s^2 + 67.05*s + 2285);
% Z ssest 22.07% match

Zest =  exp(-0.0231*s) * 41.64/(s + 64.6);
% WX tfest 14.69% match

Wxtf =   2771/(s^2 + 87.77*s + 2804);
% WX ssest 24.13% match


Wxest = exp(-0.0231*s)*(244.2*s^6 + 2.66e04*s^5 + 8.088e06*s^4 + 6.18e08*s^3 + 2.995e10*s^2 + 7.436e11*s + 1.128e13)/(s^7 + 464.1*s^6 + 8.301e04*s^5 + 1.517e07*s^4 + 1.422e09*s^3 + 6.897e10*s^2 + 1.793e12*s+ 2.751e13);

% WY tfest -0.3697% match
Wytf = -1.777/(s^2 + 1.339*s + 0.7451);
% WY ssest 23.74% match

Wyest = exp(-0.0231*s) *  (34.21*s^4 + 1522*s^3 + 5.851e04*s^2 + 1.637e06*s + 8.274e05)/(s^5 + 93.08*s^4 + 3811*s^3 + 1.315e05*s^2 + 2.366e06*s + 1.673e06);



%%
s = tf('s');
% tfest Transfer resultm for chirp on 0.001 to 5Hz for 60s indiviudally
% X tfest -72.16%
Xtf = -0.4909/(s^2 + 1.045*s + 1.541e-12);
% X ssest 77.93%
Xest = 132.8/(s + 145.8);

% Y tfest -1.981%
Ytf = -6.848e04/(s^2 + 1.716e05*s + 3.164e05);
% Y ssest 92.15%
Yest = 131.8/(s + 132.4);

% Z tfest 92.58%
Ztf = 1.973e06/(s^2 + 1.558e04*s + 1.975e06);
% Z ssest 92.58%
Zest = 127.2/(s + 127.3);

% WX tfest 89.39%
Wxtf = 2.542e05/(s^2 + 153.1*s + 2.561e05);
% WX ssest -489.1% unstable
Wxest = exp(-0.0231*s)*(1.326e04*s^6 + 4.967e05*s^5 + 7.575e08*s^4 + 4.319e10*s^3 + 3.464e12*s^2 + 1.253e14*s + 1.506e15)/(s^7 + 2962*s^6 + 4.305e05*s^5 + 1.825e08*s^4 + 2.164e10*s^3 + 1.248e12*s^2 + 6.95e13*s + 1.493e15);
                                                                          
% WY tfest 91.04%
Wytf = 1.587e04/(s^2 + 230.7*s + 1.584e04);
% WY ssest 91.03%
Wyest = 80.55/(s + 80.06);

%%
% Plot out the quality of the fit. Output vs Identified Transfer function
% response

%Close figure if already open
if ishandle(1)
close 1
end
%Asign input and output
u = Vinx';
yreal = Voutx';
data = iddata(yreal,u,dt);
figure(1)
opt = compareOptions;
opt.InitialCondition = 'z'; %discretize
compare(data,Xtf,opt)% plot the quality comparison
set(findall(gca, 'Type', 'Line'), 'LineWidth', 4)
xlabel("Time (s)")
subtitle("Identified System Comparison")
ylabel('Velocity (m/s)')
grid on


%%

% This function will take input and output joint command data for the galen
% carriage joints. It only needs the data for one joint as the
% identification was made with the assumption that the galen was a lumped
% mass. It uses the tfest and ssest methods from the system identification
% toolbox.
%
% 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)

close all

figure(1)
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
ylim([-0.06 0.06])


% 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(2)
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
ylim([-0.06 0.06])


end