clc
clearvars %emol
close all

% parameters of a two-degree-of-freedom system (emol)
A1=20; A2=30; 
f1=5; f2=15; 
wn1=2*pi*f1; wn2=2*pi*f2; 
zeta1=0.05; zeta2=0.03;
wd1=sqrt(1-zeta1^2)*wn1; wd2=sqrt(1-zeta2^2)*wn2;

% impulse response of a two-degree-of-freedom system (emol)
fs=50; % sampling frequency (emol)
T1=10; % time length (emol)
t1=0:1/fs:T1-1/fs;
h=(A1/wd1)*exp(-zeta1*wn1*t1).*sin(wd1*t1)+...
  (A2/wd2)*exp(-zeta2*wn2*t1).*sin(wd2*t1);

% white noise sequence as input (emol)
T=50000;
t=0:1/fs:T-1/fs; %emol
rng('default') %emol
x=randn(1,T*fs); 

% output sequence (lines 26 and 27 are equivalent) (emol)
y=conv(h,x); y=y(1:end-length(h)+1); %emol
% y=filter(h,1,x); % not scaled for convenience; see use below (emol)

% Y = filter(B,A,X) filters the data in vector X 
%     with the filter described by vectors A and B 
%     to create the filtered data Y.  
%     The filter is a implementation of the standard difference equation:
%     a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb)
%                           - a(2)*y(n-1) - ... - a(na+1)*y(n-na)
% In line 26, h acts as a FIR filter.(emol)

% uncorrelated input and output measurement noises (emol)
rng(10); % to produce a different sequence of numbers than before (emol)
nx=0.5*randn(size(x));
% nx=0; % for Case (a)
rng(20); % see explanation above (emol)
ny=0.5*randn(size(y));
% ny=0; % for Case (b)

x=x+nx; y=y+ny;
clear nx ny % to save computer memory (emol)

% 1st stage: time functions (emol)
figure(1)
subplot(2,2,1)
plot(t1,h,'r-')
xlabel('time')
ylabel('h(t)')
grid on
subplot(2,2,2)
plot(t1(1:150),h(1:150),'r-')
xlabel('time')
ylabel('h(t)')
grid on
subplot(2,2,3)
plot(t,x,'b-')
xlabel('time')
ylabel('x(t)')
axis([0 60000 -15 15]) %emol
grid on
subplot(2,2,4)
plot(t,y,'b-')
xlabel('time')
ylabel('y(t)')
axis([0 60000 -15 15]) %emol
grid on

% spectral density functions via Welch's (segment averaging) method (emol)
[Gxx,f]=cpsd(x(1:T*fs),x(1:T*fs),hanning(T1*fs),T1*fs/2,T1*fs,fs);
[Gyy,f]=cpsd(y(1:T*fs),y(1:T*fs),hanning(T1*fs),T1*fs/2,T1*fs,fs);
[Gxy,f]=cpsd(y(1:T*fs),x(1:T*fs),hanning(T1*fs),T1*fs/2,T1*fs,fs);
[Gyx,f]=cpsd(x(1:T*fs),y(1:T*fs),hanning(T1*fs),T1*fs/2,T1*fs,fs);

% Pxy = cpsd(X,Y) returns the Cross Power Spectral Density estimate, Pxy,
%       of two discrete-time signals, X and Y, using Welch's averaged,
%       modified periodogram method. By default, X and Y are divided into
%       eight sections with 50% overlap, each section is windowed with a
%       Hamming window and eight modified periodograms are computed and
%       averaged.
% Pxy = cpsd(X,Y,WINDOW), when WINDOW is a vector, divides each column of
%       X and Y into overlapping sections of length equal to the length of
%       WINDOW, and then windows each section with the vector specified in
%       WINDOW.
% Pxy = cpsd(X,Y,WINDOW,NOVERLAP) uses NOVERLAP samples of overlap from
%       section to section.
% Pxy = cpsd(X,Y,WINDOW,NOVERLAP,NFFT) specifies the number of FFT points
%       used to calculate the Cross PSD estimate.
% [Pxy,F] = cpsd(X,Y,WINDOW,NOVERLAP,NFFT,Fs) returns the Cross PSD as a
%           function of physical frequency. Fs is the sampling frequency 
%           specified in Hertz, whereas F is the vector of frequencies
%           (in hertz) at which Pxy is estimated.
% The same command can be used for all the spectral density funcions.(emol)

% frequency response function estimates (emol)
H1=Gxy./Gxx; 
H2=Gyy./Gyx;
HT=(Gyy-Gxx + sqrt((Gxx-Gyy).^2 + 4*abs(Gxy).^2))./(2*Gyx);
% Above, no correction factors are applied to any FRF estimate,
% as they can then be compared directly.

% frequency response function via FFT of impulse response sequence (emol)
H=fft(h); 

% 2nd stage: magnitude spectra of estimators (emol)
figure (2) % H1 x H (emol)
plot(f,20*log10(abs(H1)),'b-',f,20*log10(abs(H(1:length(f)))),'r--') %emol
xlabel('Frequency (Hz)'); ylabel('|\itH\rm_1(\itf\rm)| (dB)')
axis([0 25 -35 25]) %emol
grid on %emol
legend('H_{1}','H') %emol

figure(3) % H2 x H (emol)
plot(f,20*log10(abs(H2)),'b-',f,20*log10(abs(H(1:length(f)))),'r--') %emol
xlabel('Frequency (Hz)'); ylabel('|\itH\rm_2(\itf\rm)| (dB)')
axis([0 25 -30 30]) %emol
grid on %emol
legend('H_{2}','H') %emol

figure(4) % HT x H (emol)
plot(f,20*log10(abs(HT)),'b-',f,20*log10(abs(H(1:length(f)))),'r--') %emol
xlabel('Frequency (Hz)'); ylabel('|\itH_T(\itf\rm)| (dB)')
axis([0 25 -30 30]) %emol
grid on %emol
legend('H_{T}','H') %emol

% Comments 1: phase spectrum
% (phase spectrum of all three estimators is the same)
 
figure(5) %emol
plot(f,unwrap(angle(H1)),'r-',f,unwrap(angle(H2)),'g-',...
     f,unwrap(angle(HT)),'b-',f,unwrap(angle(H(1:length(f)))),'k-')
xlabel('Frequency (Hz)'); ylabel('Phase spectrum (rad)')
grid on %emol
legend('H_{1}','H_{2}','H_{T}','H')

% % Comments 2: ARMA model
% 
% [b,a]=invfreqz(HT, 2*pi*f/fs, 4,4, [],30); 
% Hz=freqz(b,a,length(f),fs);
% 
% figure(6) %emol
% plot(f,20*log10(abs(Hz)),f,20*log10(abs(H(1:length(f)))), 'r:')
% xlabel('Frequency (Hz)'); ylabel('Magnitude spectrum (dB)')
% axis([0 25 -35 25])
% grid on %emol
% 
% figure(7) %emol
% plot(f,unwrap(angle(Hz)),f,unwrap(angle(H(1:length(f)))), 'r:')
% xlabel('Frequency (Hz)'); ylabel('Phase spectrum (rad)')
% grid on %emol