clc
clear all
close all

% This code is used to perform linear system identification using
% State Space Subspace approach

load suspension.mat
Ts = 0.001; N = 100000; opt = n4sidOptions('N4Weight','CVA');

roadx = {hxA, hxB, hxC, hxD, hxE};
roads = {hsA, hsB, hsC, hsD, hsE};

bodyx = {bodyxsuppressedA, bodyxsuppressedB, bodyxsuppressedC, bodyxsuppressedD, bodyxsuppressedE};
bodys = {bodysuppressedA, bodysuppressedB, bodysuppressedC, bodysuppressedD, bodysuppressedE};

tyrex = {tirexsuppressedA, tirexsuppressedB, tirexsuppressedC, tirexsuppressedD, tirexsuppressedE};
tyres = {tiresuppressedA, tiresuppressedB, tiresuppressedC, tiresuppressedD, tiresuppressedE};

% best order selected based on FIT_val_diff and FIT_val_lin 
bodynx = {9,8,8,6,10};  
tyrenx = {9,8,10,9,10};

% find excited harmonics
X = abs(fft(hsA)); c = find(X > 0.00001);

% body
for i = 1:5

    u_train = [roadx{i};roadx{i}]; y_train = bodyx{i}(N+1:3*N); y_train = y_train - mean(y_train);
    u_val_same = [roadx{i}]; y_val_same = bodyx{i}(3*N+1:4*N); y_val_same = y_val_same - mean(y_val_same);
    u_val_diff = [roads{i};roads{i};roads{i}]; y_val_diff = bodys{i}(N+1:4*N); y_val_diff = y_val_diff - mean(y_val_diff);
    Z2 = fft(bodys{i}(N+1:2*N)); Y2 = zeros(N,1); Y2(c) = Z2(c); y2 = ifft(Y2);
    Z3 = fft(bodys{i}(2*N+1:3*N)); Y3 = zeros(N,1); Y3(c) = Z3(c); y3 = ifft(Y3);
    Z4 = fft(bodys{i}(3*N+1:4*N)); Y4 = zeros(N,1); Y4(c) = Z4(c); y4 = ifft(Y4);
    y_val_lin = [y2;y3;y4];

    tic
    sysbody{i} = n4sid(u_train,y_train,bodynx{i},'Ts',Ts,opt);
    time_taken = toc;
    yest_train = lsim(sysbody{i},[roadx{i};roadx{i};roadx{i}],[0:3*N-1]*Ts); yest_train(1:N) = [];
    FIT_train = 100*(1 - sqrt(sum((y_train - yest_train).^2)/sum((y_train - mean(y_train)).^2)));
    MSE_train = mean((y_train - yest_train).^2);

    yest_val_same = lsim(sysbody{i},[roadx{i};roadx{i}],[0:2*N - 1]*Ts); yest_val_same(1:N) = [];
    FIT_val_same = 100*(1 - sqrt(sum((y_val_same - yest_val_same).^2)/sum((y_val_same-mean(y_val_same)).^2)));
    MSE_val_same = mean((y_val_same - yest_val_same).^2);

    yest_val_diff = lsim(sysbody{i},[roads{i};roads{i};roads{i};roads{i}],[0:4*N-1]*Ts); yest_val_diff(1:N) = [];
    FIT_val_diff = 100*(1 - sqrt(sum((y_val_diff - yest_val_diff).^2)/sum((y_val_diff - mean(y_val_diff)).^2)));
    MSE_val_diff = mean((y_val_diff - yest_val_diff).^2);

    FIT_val_lin = 100*(1 - sqrt(sum((y_val_lin - yest_val_diff).^2)/sum((y_val_lin-mean(y_val_lin)).^2)));
    MSE_val_lin = mean((y_val_lin - yest_val_diff).^2);

    [FIT_train, FIT_val_same, FIT_val_diff, FIT_val_lin, time_taken]

end

figure(1)
i = 1;
[Mag,Phase] = bode(sysbody{i},[0.01:0.01:10]*20*2*pi);
for k = 1:1000
    m(:,k) = Mag(1,1,k);
end
loglog([0.01:0.01:10],m)
hold
for i = 2:5
    [Mag,Phase] = bode(sysbody{i},[0.01:0.01:10]*20*2*pi);
    for k = 1:1000
        m(:,k) = Mag(1,1,k);
    end
    loglog([0.01:0.01:10],m)
end
xlabel('Spatial Frequency [cycles/m]')
ylabel('Frequency Response Magnitude')
legend('Class A','Class B','Class C','Class D','Class E')

%tyre
for i = 1:5

    u_train = [roadx{i};roadx{i}]; y_train = tyrex{i}(N+1:3*N); y_train = y_train - mean(y_train);
    u_val_same = [roadx{i}]; y_val_same = tyrex{i}(3*N+1:4*N); y_val_same = y_val_same - mean(y_val_same);
    u_val_diff = [roads{i};roads{i};roads{i}]; y_val_diff = tyres{i}(N+1:4*N); y_val_diff = y_val_diff - mean(y_val_diff);
    Z2 = fft(tyres{i}(N+1:2*N)); Y2=zeros(N,1); Y2(c) = Z2(c); y2 = ifft(Y2);
    Z3 = fft(tyres{i}(2*N+1:3*N)); Y3=zeros(N,1); Y3(c) = Z3(c); y3 = ifft(Y3);
    Z4 = fft(tyres{i}(3*N+1:4*N)); Y4=zeros(N,1); Y4(c) = Z4(c); y4 = ifft(Y4);
    y_val_lin = [y2;y3;y4];

    tic
    systyre{i} = n4sid(u_train,y_train,tyrenx{i},'Ts',Ts,opt);
    time_taken = toc;

    yest_train = lsim(systyre{i},[roadx{i};roadx{i};roadx{i}],[0:3*N-1]*Ts); yest_train(1:N) = [];
    FIT_train = 100*(1-sqrt(sum((y_train - yest_train).^2)/sum((y_train - mean(y_train)).^2)));
    MSE_train = mean((y_train-yest_train).^2);

    yest_val_same = lsim(systyre{i},[roadx{i};roadx{i}],[0:2*N-1]*Ts); yest_val_same(1:N) = [];
    FIT_val_same = 100*(1-sqrt(sum((y_val_same - yest_val_same).^2)/sum((y_val_same-mean(y_val_same)).^2)));
    MSE_val_same = mean((y_val_same - yest_val_same).^2);

    yest_val_diff = lsim(systyre{i},[roads{i};roads{i};roads{i};roads{i}],[0:4*N-1]*Ts); yest_val_diff(1:N) = [];
    FIT_val_diff = 100*(1 - sqrt(sum((y_val_diff - yest_val_diff).^2)/sum((y_val_diff - mean(y_val_diff)).^2)));
    MSE_val_diff = mean((y_val_diff - yest_val_diff).^2);

    FIT_val_lin = 100*(1 - sqrt(sum((y_val_lin - yest_val_diff).^2)/sum((y_val_lin - mean(y_val_lin)).^2)));
    MSE_val_lin = mean((y_val_lin - yest_val_diff).^2);

    [FIT_train, FIT_val_same, FIT_val_diff, FIT_val_lin, time_taken]

end

figure(2)
i = 1;
[Mag,Phase] = bode(systyre{i},[0.01:0.01:10]*20*2*pi);
for k = 1:1000
    m(:,k) = Mag(1,1,k);
end
loglog([0.01:0.01:10],m)
hold
for i = 2:5
    [Mag,Phase] = bode(systyre{i},[0.01:0.01:10]*20*2*pi);
    for k = 1:1000
        m(:,k) = Mag(1,1,k);
    end
    loglog([0.01:0.01:10],m)
end
xlabel('Spatial Frequency [cycles/m]')
ylabel('Frequency Response Magnitude')
legend('Class A','Class B','Class C','Class D','Class E')