% TOGAC31_AMBF_GalenSystemIdent.m
% This file was used to analyze the input and and output data for the galen
% surgical robot in AMBF. This was used for the assumption the the robot
% could be evaluated as a mass spring damper where the joints are all held
% at their 0 positions, and the 3 carriage joints are controlled with
% velocity control simulatneously. The input for the 3 carriage joints was
% identical, lumping their response together. They should opperate in
% parallel and move the robot like a mass linearly along the z axis. 
% The response of a step input of magnitud 0.5 was applied for the step
% respone, with the intial pose of the carriage joints at the bottom of the base.
% The response for a chirp input from 100Hz to 1hz was applied for 50
% seconds with the carriage joints in the middle of the joint limits
% (0.21m)
% The systems were identified using the tfest method and ssest methods from
% matlab's system identification toolbox
% Created by Brevin Banks
% Modified April 1 2023

%% Chirp input

% load the data
dt = 0.1087; % measured time step per sampling loop
Vout100hzto1hzChirp = load('Vout100hzto1hzChirp.mat');
Vin100hzto1hzChirp = load('Vin100hzto1hzChirp.mat');

Vout = (Vout100hzto1hzChirp.Vout100hzto1hz(1,:) + Vout100hzto1hzChirp.Vout100hzto1hz(2,:) + Vout100hzto1hzChirp.Vout100hzto1hz(3,:))/3;
Vin = Vin100hzto1hzChirp.Vin100hzto1hz(1,:);

IdentifySystem(Vout,Vin,dt)

% tfest Transfer result

% tf_chirp_no_grav =
%          1.413e05
%   -----------------------
%   s^2 + 1465 s + 1.417e05
% 83.45% match

% ssest Transfer result
% tf_chirp_no_grav = 
%      4.894 s + 630.6
%   ---------------------
%   s^2 + 26.33 s + 641.6
% 84.14% match



%% Step Input No Gravity

% load the data
dt = 0.0746;% measured time step per sampling loop
VoutStep0_05Nograv = load('VoutStep0_05Nograv.mat');
VinStep0_05 = load('VinStep0_05.mat');

Vout = (VoutStep0_05Nograv.cv(1,:) + VoutStep0_05Nograv.cv(2,:) + VoutStep0_05Nograv.cv(3,:))/3;
Vin = VinStep0_05.rv(1,:);

IdentifySystem(Vout,Vin,dt)


% Step input only has tfest result as ssest was a very poor match

% tf_step_no_grav = 
  %         5262
  % --------------------
  % s^2 + 138.6 s + 5265
% 84.15% match




%% Step Input Gravity

% load the data
dt = 0.0971;% measured time step per sampling loop
VoutStep0_05Grav = load('VoutStep0_05Grav.mat');
VinStep0_05 = load('VinStep0_05Grav.mat');

Vout = (VoutStep0_05Grav.cv(1,:) + VoutStep0_05Grav.cv(2,:) + VoutStep0_05Grav.cv(3,:))/3;
Vin = VinStep0_05.rv(1,:);

IdentifySystem(Vout,Vin,dt)

% tfest result

% tf_step_grav =
%                         1124
%   exp(-0.0971*s) * -------------------
%                    s^2 + 50.9 s + 1274
% 98.5% match


%%

% 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