%% Transfer Function of MIMO System
% Estimate the transfer function for a simple multi-input/multi-output system.
%
% An ideal one-dimensional oscillating system consists of two masses, m_1 and m_2, confined between two walls. The units are such that m_1 =1 and m_2 =\mu. Each mass is attached to the nearest wall by a spring with an elastic constant k. An identical spring connects the two masses. Three dampers impede the motion of the masses by exerting on them forces proportional to speed, with damping constant b. Sensors sample a_1 and a_2, the accelerations of the masses, at F_s =50 Hz.
%
%
% Generate 30000 time samples, equivalent to 600 seconds. Define the sampling interval \Delta t=1/F_s.

Fs = 50;
dt = 1/Fs;
N = 30000;
t = dt*(0:N-1);

%
% The system can be described by the state-space model
% \matrix{x(k+1)=Ax(k)+Bu(k),\cr y(k)=Cx(k)+Du(k),\cr}
% where x=\left[\matrix{r_1&v_1&r_2&v_2}\right]^T is the state vector, r_i and v_i are respectively the location and the velocity of the ith mass, u=\left[\matrix{u_1&u_2}\right]^T is the vector of input driving forces, and y=\left[\matrix{a_1&a_2}\right]^T is the output vector. The state-space matrices are
% A=\exp(A_c\Delta t),\quad B=A_c^{-1}(A-I)B_c,
% \quad
% C=\left[\matrix{-2k&-2b&k&b\cr k/\mu&b/\mu&-2k/\mu&-2b/\mu}\right],\quad D=\left[\matrix{1&0\cr0&1/\mu}\right],
% I is the 4\times 4 identity, and the continuous-time state-space matrices are
% A_c=\left[\matrix{0&1&0&0\cr-2k&-2b&k&b\cr0&0&0&1\cr k/\mu&b/\mu&-2k/\mu&-2b/\mu}\right],
% \quad
% B_c=\left[\matrix{0&0\cr1&0\cr0&0\cr0&1/\mu}\right].
% Set k=400, b=0, and \mu =1/10.

k = 400;
b = 0;
m = 1/10;

Ac = [0 1 0 0;-2*k -2*b k b;0 0 0 1;k/m b/m -2*k/m -2*b/m];
A = expm(Ac*dt);
Bc = [0 0;1 0;0 0;0 1/m];
B = Ac\(A-eye(4))*Bc;
C = [-2*k -2*b k b;k/m b/m -2*k/m -2*b/m];
D = [1 0;0 1/m];

%
% The masses are driven by random input throughout the measurement. Use the state-space model to compute the time evolution of the system starting from an all-zero initial state.

rng default
u = randn(2,N);

x = [0;0;0;0];
for kk = 1:N
    y(:,kk) = C*x + D*u(:,kk);
    x = A*x + B*u(:,kk);
end

%
% Use the input and output data to estimate the transfer function of the system as a function of frequency. Specify the 'mimo' option to produce all four transfer functions. Use a 5000-sample Hann window to divide the signals into segments. Specify 2500 samples of overlap between adjoining segments and 2^{14} DFT points. Plot the estimates.

wind = hann(5000);
nov = 2500;

[q,fq] = tfestimate(u',y',wind,nov,2^14,Fs,'mimo');

% Compute the theoretical transfer function as the Z-transform of the time-domain transfer function, evaluated at the unit circle.

nfs = 2^14;

fz = 0:1/nfs:1/2-1/nfs;
z = exp(2j*pi*fz);

[b1,a1] = ss2tf(A,B,C,D,1);
[b2,a2] = ss2tf(A,B,C,D,2);

frf(1,:,1) = polyval(b1(1,:),z)./polyval(a1,z);
frf(1,:,2) = polyval(b1(2,:),z)./polyval(a1,z);

frf(2,:,1) = polyval(b2(1,:),z)./polyval(a2,z);
frf(2,:,2) = polyval(b2(2,:),z)./polyval(a2,z);

%
% Plot the theoretical transfer functions and their corresponding estimates.

for jk = 1:2
    for kj = 1:2
        subplot(2,2,2*(jk-1)+kj)
        plot(fq,20*log10(abs(q(:,jk,kj))))
        hold on
        plot(fz*Fs,20*log10(abs(frf(jk,:,kj))))
        hold off
        grid
        title(['Input ' int2str(kj) ', Output ' int2str(jk)])
        axis([0 Fs/2 -50 100])
    end
end

%
% The transfer functions have maxima at the expected values, \omega_{1,2} /2\pi, where the \omega are the eigenvalues of the modal matrix.

sqrt(eig(k*[2 -1;-1/m 2/m]))/(2*pi)

%
% Add damping to the system by setting b=0\ldotp 1. Compute the time evolution of the damped system with the same driving forces. Compute the H_2 estimate of the MIMO transfer function using the same window and overlap. Plot the estimates using the tfestimate functionality.

b = 0.1;

Ac = [0 1 0 0;-2*k -2*b k b;0 0 0 1;k/m b/m -2*k/m -2*b/m];
A = expm(Ac*dt);
B = Ac\(A-eye(4))*Bc;
C = [-2*k -2*b k b;k/m b/m -2*k/m -2*b/m];

x = [0;0;0;0];
for kk = 1:N
    y(:,kk) = C*x + D*u(:,kk);
    x = A*x + B*u(:,kk);
end

clf
tfestimate(u',y',wind,nov,[],Fs,'mimo','Estimator','H2')
legend('I1, O1','I1, O2','I2, O1','I2, O2')
yl = ylim;

% Compare the estimates to the theoretical predictions.

[b1,a1] = ss2tf(A,B,C,D,1);
[b2,a2] = ss2tf(A,B,C,D,2);

frf(1,:,1) = polyval(b1(1,:),z)./polyval(a1,z);
frf(1,:,2) = polyval(b1(2,:),z)./polyval(a1,z);

frf(2,:,1) = polyval(b2(1,:),z)./polyval(a2,z);
frf(2,:,2) = polyval(b2(2,:),z)./polyval(a2,z);

plot(fz*Fs,20*log10(abs(reshape(permute(frf,[2 1 3]),[nfs/2 4]))))
legend('I1, O1','I1, O2','I2, O1','I2, O2')
ylim(yl)
grid

% Copyright 2012 The MathWorks, Inc.