% system_id_models.m --- 
% continuous time system models and input-output data for LTI systems
% some models are from Applied System Identification, Jer-Nan Juang, 1994
%
% Index 
% MODEL == 1	: lightly damped simple oscillator with feed-through
% MODEL == 2	: lightly damped 3 mode system with feed-through, J-N.Juan 6.3
% MODEL == 3    : 4-coordinate lightly damped SIMO MCK system, no feed-through
% MODEL == 4	: base-excited lightly damped SIMO MCK system with accel msmnts
% MODEL == 5	: Boeing 707, speed and vertical flight dynamics
% MODEL == 6	: some aircraft, lateral flight dynamics
% MODEL == 7    : pollutant concentrations in plants, fish, water
% DEFAULT	: a random under damped MIMO system without feed-through
%
% INPUT == 'CHIRP'    : sine sweep
% INPUT == 'IMPUSE'   : unit impuse
% INPUT == 'POISSON'  : bi-polar Poisson Noise
% INPUT == 'FILTER' : second order linear system filter
% INPUT == 'FLICKER' : 1/f (pink) noise input   (default)
%
% relative intensities of various types of measurement noise
%
% SNRu 			%  input msmnt signal-to-noise ratio
% SNRy 			% output msmnt signal-to-noise ratio
%
% independent filtered white noise ("flicker noise") model 
% NOISE_WEIGHT = [ white noise , flicker noise , Poisson noise ];
%
% PLOTS = 1 : draw plots, 0 : do not draw plots

if NEW_MODEL % ----------------------------------------------------

% DEFAULTS ==================================================================
% may be over-ridden in any model, below

dt = 0.01;			% time-step, sec
N  = 2048;			% number of data points in the time seris

w = 2*pi* linspace(0.1,10.0,500);        % frequencies for Bode plot 

p       = 40;	  % number of observer Markov parameters for OKID model
svt_io  = 0.050;  % singular value ratio threshold for input-output data
svt_mpc = 0.050;  % singular value ratio threshold for Markov param. correlation
mact    = 0.9;	  % threshold for modal amplitude correlations

k       = 32;	  % number of block rows in data Hankel matrix
svt     = 0.05;	  % singular value threshold 
cct     = 0.6;    % cannonical correlation threshold 

switch  MODEL
case 1 % MODEL 1 : lightly damped simple oscillator with feed-through

 Ks = 1; Ms = 1; Cs = 0.02;

 Ac = [    0          1    ;			% dynamic matrix
        -Ms\Ks     -Ms\Cs  ];

 Bc = [ 0 ; 1 ];
 C  = [ 0   1  ;
       Ac(2,:) ];
 D  =  [ 0 ; 1 ];

 [n,r,m] = abcddim(Ac,Bc,C,D);			% dimensions of the system

 dt  = 0.20;			% time-step, sec
 N   = 512;			% number of data points in the time seris
 tau = 2;                       % characteristic time scale of input process

 p = round(2.0*2*pi*sqrt(Ms/Ks)/dt)   % number of observer Markov parameters
 k = 8;
 w = 2*pi*[ 0.01 : 0.01 : 1 ];
 leg = {'displ','accel'};
 leg_fig = {'y_{true}','','y_{data}','','y_{id}',''};


case 2 % MODEL 2 : lightly damped 3 mode system with feed-through, J-N.Juan 6.3

% discrete time model

 A1 = [ 0.9856 0.1628 ; -0.1628 0.9856 ];
 A2 = [ 0.8976 0.4305 ; -0.4305 0.8976 ];
 A3 = [ 0.8127 0.5690 ; -0.5690 0.8127 ];

 A = [ A1 zeros(2,4) ; zeros(2) A2 zeros(2) ; zeros(2,4) A3 ];
 B = [ 0.0011 ; 0.0134 ; -0.0016 ; -0.0072 ; 0.0011 ; 0.0034 ]; 
 C = [ 1.5119  0   2   0   1.5119  0 ;
       1.3093  0   0   0  -1.3093  0 ];
 D = [ 0 ; 0 ];

 [n,r,m] = abcddim(A,B,C,D);			% dimensions of the system

 dt = 0.10;					% 10 Hz sample frequency
 N  = 1024;					% number of data points

 Ac = logm(A)/dt;  Bc = (A-eye(n))\(Ac*B);

% independent filtered white noise ("flicker noise") excitation on each input
  tau = 5;   % characteristic time scale of the input process

  w = 2*pi*[ 0.01 : 0.01 : 1.5];

  leg = {'y_1','y_2'};
  leg_fig = {'y_{true}','','y_{data}','','y_{id}',''};

 p = 70;	  % number of observer Markov parameters
 svt_mpc = 0.010; % singular value ratio threshold for Markov param. correlation
 mact = 0.8;	  % threshold for modal amplitude correlations

 k = 24;	  % number of block rows in the data Hankel matrix
 svt = 0.05;	  % specified ratio of singular values


case 3 % MODEL 3 : 4-coordinate lightly damped MIMO MCK system, no feed-through

  Ac = [ zeros(4)                        eye(4);
        [ -2000  1000    0    0    -5    1   0   0 ;
            500 -1000  500    0     1   -3   1   0 ;
              0   333 -667  333     0    0  -2   0 ;
              0     0  250 -500     0    0   0  -1 ] ];
  Bc = [ 0 0 ; 0 0 ; 0 0 ; 0 0 ;  -100 0 ; -50 0 ; -33  -10 ;  -25 10 ];
  C  = [ 0  0  0  0  -1   1  0  0 ;
         0  0  0  0   0  -1  1  0 ];
  D  = zeros(2);

  [n,r,m] = abcddim(Ac,Bc,C,D);			% dimensions of the system
     
  dt = 0.01;
  N  = 4096;					% number of data points

  % independent filtered white noise ("flicker noise") excitation on each input
  INPUT = 'FLICKER';
  tau = 1;        % characteristic time scale of the input process

  leg = {'ifd 1';'ifd 2'};
  leg_fig = {'y_{data}','','y_{true}','','y_{okid}','','y_{ssid}',''};

  p       = 80;	   % number of observer Markov parameters
  svt_io  = 0.010; % singular value ratio threshold for input-output data
  svt_mpc = 0.050; % singular value ratio threshold for Markov param. correl.
  mact    = 0.7;	  % threshold for modal amplitude correlations

  k = 24;	  % number of block rows in the data Hankel matrices
  svt  =  0.05;	  % specified ratio of singular values


case 4 % MODEL 4 : base-excited lightly damped SIMO MCK system with accel msmnts

  nc =   7;                   % number of displacement coordinates
% mc = [1:nc];                % full response measurements
  mc = [1:3:nc];              % sparse response measurements
  m  = length(mc);

  k1 = 2e3; k2 = 5e1;  ki = 1e2;   % inter-story, double-story, base stiffness
  c1 = 1e0; c2 = 5e-1; ci = 1e1;   % inter-story, double-story, base damping

  Ms = 10*eye(nc)/nc;
  Ks = toeplitz( [ 2*k1+2*k2, -k1, -k2, zeros(1,nc-3) ] );
  Ks(1,1) = ki+k1+k2; Ks(2,2) = 2*k1+k2; Ks(nc-1,nc-1) = 2*k1+k2; Ks(nc,nc) = k1+k2;
  Cs = toeplitz( [ 2*c1+2*c2, -c1, -c2, zeros(1,nc-3) ] );
  Cs(1,1) = ci+c1+c2; Cs(2,2) = 2*c1+c2; Cs(nc-1,nc-1) = 2*c1+c2; Cs(nc,nc) = c1+c2;

  [V,E] = eig(Ks,Ms);
  mpf = abs(V'*Ms*ones(nc,1))    % mode participation factors for seismic input

  Ac = [ zeros(nc)    eye(nc) ;
         -Ms \ Ks    -Ms \ Cs ]; 
  Bc = [ zeros(nc,1) ; -ones(nc,1) ];
  Cc = [ -Ms \ Ks    -Ms \ Cs ];	% acceleration response msmsnts
  Cc = Cc(mc,:);			% sparse acceleration response msmnts
  Dc = zeros(m,1);

  w = 2*pi*linspace( 0.05 , 12 , 200 ); % frequencies for Bode plots

  [n,r,m] = abcddim(Ac,Bc,Cc,Dc);	% dimensions of the system

  % independent filtered white noise ("flicker noise") excitation on each input
  tau = 0.5;        % characteristic time scale of the input process

  leg = {'floor accel'};
  leg_fig = {'y_{true}','','y_{data}','','y_{id}',''};

  p       = floor(3/dt);  % number of observer Markov parameters
  svt_io  = 0.050;% singular value ratio threshold for input-output data
  svt_mpc = 0.001;% singular value ratio threshold for Markov param. correlation
  svt_mpc = 2*nc; % singular value ratio threshold for Markov param. correlation
  mact    = 0.7;  % threshold for modal amplitude correlations

  k    = 120;	  % number of block rows in the data Hankel matrices
  svt  = 0.010;	  % specified order of the system for SSID
  svt  = 2*nc;	  % specified order of the system for SSID
  cct  = 8;     % cannonical correlation threshold 

case 5 % MODEL 5 : Boeing 707 - linearized about a nominal steady condition

%  System inputs:  (1) thrust   and (2) elevator angle.
%  System outputs: (1) airspeed and (2) pitch angle.

% R. Brockhaus: {Flugregelung} (Flight Control), Springer, 1994.

  Ac = [-0.46E-01,            0.10681415316, 0.0,   -0.17121680433;
        -0.1675901504661613, -0.515,         1.0,    0.6420630320636088E-02;
         0.1543104215347786, -0.547945,     -0.906, -0.1521689385990753E-02;
         0.0,                 0.0,           1.0,    0.0];

  Bc = [0.1602300107479095,      0.2111848453E-02;
        0.8196877780963616E-02, -0.3025E-01;
        0.9173594317692437E-01, -0.75283075;
        0.0,                     0.0];

  C = [1.0, 0.0, 0.0, 0.0;
       0.0, 0.0, 0.0, 1.0];

  D = zeros (2, 2);

  [n,r,m] = abcddim(Ac,Bc,C,D);  

  dt = 0.50;					% time-step, sec
  N  = 1024;					% number of data points

  tau = 10;   % characteristic time scale of the input process

  w = 2*pi*[ 0.01 : 0.01 : 1 ];
  leg = {'airspeed','pitch'};

  p = 200;	  % number of observer Markov parameters
  svt_io  = 0.05; % singular value threshold of Markov Parameter Correlation
  svt_mpc = 0.01; % singular value threshold of Markov Parameter Correlation
  svt_mpc = 6;    % specified order of the model
  mact    = 0.7;  % threshold for modal amplitude correlations

  k   = 20;	  % number of block rows in the aata Hankel matrices
  svt = 4;        % specified order of the model


case 6 % MODEL 6 : lateral aircraft dynamics (linearized)    % Scruggs 2014 3.11

% x_1  =  roll rate           (rad/s)
% x_2  =   yaw rate           (rad/s)
% x_3  =  side-slip angle     (rad)
% x_4  =  roll attitude       (rad)
% u_1  =  aileron position    (rad)
% u_2  =  rudder  position    (rad)
% y_1  = x_1
% y_2  = x_2
% y_3  = x_3
% y_4  = x_4

Ac = [ -2.6   ,  0.25  , -38.0 ,   0    ;
       -0.075 , -0.27  ,   4.4 ,   0    ;
        0.078 , -0.99  ,  -0.23 , 0.052 ;
        1.0   ,  0.078 ,    0   ,  0    ];

Bc = [ 17    ,   7    ;
        0.82 , -3.2   ;
         0   ,  0.046 ;
         0   ,   0    ];

C  = eye(4);
D  = zeros(4,2);

  [n,r,m] = abcddim(Ac,Bc,C,D);

  dt = 0.50;                                    % time-step, sec
  N  = 1024;                                    % number of data points

  % independent filtered white noise ("flicker noise") excitation on each input
  tau = 5;    % characteristic time scale of the input process
  
  w = 2*pi*[ 0.001 : 0.01 : 1 ];
  leg = {'roll rate','yaw rate','yaw','roll'};

  p = 500;	   % number of observer Markov parameters
  svt_io  = 0.05;  % singular value threshold of Markov Parameter Correlation
  svt_mpc = 0.001; % singular value threshold of Markov Parameter Correlation
  svt_mpc = 6;     % specified order of the model
  mact    = 0.9;   % threshold for modal amplitude correlations

  k   = 10;	   % number of block rows in the data Hankel matrices 
  svt =  4;       % specified order of the model


case 7 % MODEL 7 : pollutant concentrations in plants, fish, water

% Accumulation and transformation of an insecticide in an aquatic environment.
%    Compartment 1:  Insecticide in pond water
%    Compartment 2:  Insecticide in plants and soil
%    Compartment 3:  Insecticide in fish
%    Compartment 4:  Metabolite in pond water
%    Compartment 5:  Metabolite in plants and soil
%    Compartment 6:  Stored insecticide in fish
% Note: X(i) refers to amount in compartment i expressed
%       as a percentage of the initial applied insecticide.
%
% d/dt(X[1]) = k21*X[2]-k12*X[1]-k13*X[1]
% d/dt(X[2]) = k12*X[1]-k21*X[2]
% d/dt(X[3]) = k13*X[1]+k63*X[6]-k36*X[3]-k34*X[3]
% d/dt(X[4]) = k34*X[3]-k45*X[4]+k54*X[5]
% d/dt(X[5]) = k45*X[4]-k54*X[5]
% d/dt(X[6]) = k36*X[3]-k63*X[6]

% These concentrations must be positive-valued

% Blau GE, Neely, WB, Branson, DR (1975)
% Ecokinetics: A study of the fate and distribution of chemicals in laboratory ecosystems. 
% AIChE Journal 21:854-861

k12 = 0.388                 % uptake of insecticide by plants
k21 = 0.0515                % release of insecticide by plants
k13 = 0.136                 % uptake of insecticide by fish
k36 = 0.067                 % storage of insecticide
k63 = 0.0254                % mobilisation of insecticide
k34 = 0.0788                % excretion of metabolite 
k45 = 0.0068                % uptake of metabolite by plants
k54 = 0.001                 % release of metabolite by plants

Ac = [ -k12-k13 ,  k21 ,  0       ,  0   ,  0   ,  0   ;
         k12   , -k21 ,  0       ,  0   ,  0   ,  0   ;
         k13   ,  0   , -k36-k34 ,  0   ,  0   ,  k63 ;
         0     ,  0   ,  k34     , -k45 ,  k54 ,  0   ;
         0     ,  0   ,  0       ,  k45 , -k54 ,  0   ;
         0     ,  0   ,  k36     ,  0   ,  0   , -k63 ];
 
Bc = [ 1 ; 0 ; 0 ; 0 ; 0 ; 0 ];
 
C  = [ 0 , 0 , 1 , 0 , 0 , 1 ;    % fish
       0 , 1 , 0 , 0 , 1 , 0 ;    % plants
       1 , 0 , 0 , 1 , 0 , 0 ];   % water

D  = [ 0 ; 0 ; 0 ];

 dt = 1;   % time step, hours
 N  = 1024; 

  tau = 0.2;    % INPUT = 'FLICKER';

  w  = 2*pi*linspace( 0.01*f_min , 1.2*f_max , 200 );

  leg = {'y_i'};
  leg_fig = {'y_{true}','','y_{data}','','y_{id}',''};

  p       = 200;   % number of observer Markov parameters
  svt_io  = 0.005; % singular value ratio threshold for input-output data
  svt_mpc = 0.100; % singular value ratio threshold for Markov param. correl.
  svt_mpc = 14;    % specified number of states
  mact    = 0.7;   % threshold for modal amplitude correlations

  k    =  20;  % number of block rows in data Hankel matrix
  svt  =  6;   % specified number of states to identify


otherwise %  a random under damped MIMO system without feed-through

  % Specify an LTI system used to simulate some input-output data ...
  n = 10;    % number of internal states (even) ... complex-conjugate poles
  r =  2;    % number of inputs
  m =  3;    % number of outputs

  % generate a random diagonalized LTI system with under-damped dynamics 
  f_min = 0.50;  f_max = 5.0;            % range of natural frequencies, Hz
  z_min = 0.05;  z_max = 0.15;           % range of damping ratios
  wn = 2*pi*(f_min + (f_max-f_min)*rand(1,n/2));
  z  = z_min+(z_max-z_min)*rand(1,n/2);
  [Ac, Bc, C, D] = modalLTI(wn,z,m,r);     % generate a continuous time LTI

  % --- generate records of clean input/output data ...
  dt = 0.01;                          % time-step increment
  N  = 8192;                          % number of points in the data record

  tau = 0.2;    % INPUT = 'FLICKER';

  w  = 2*pi*linspace( 0.01*f_min , 1.2*f_max , 200 );

  leg = {'y_i'};
  leg_fig = {'y_{true}','','y_{data}','','y_{id}',''};

  p       = 1.5/f_min/dt;   % number of observer Markov parameters
  svt_io  = 0.005; % singular value ratio threshold for input-output data
  svt_mpc = 0.100; % singular value ratio threshold for Markov param. correl.
  svt_mpc = 14;    % specified number of states
  mact    = 0.7;   % threshold for modal amplitude correlations

  k    =  80;	  	% number of block rows in data Hankel matrix
  svt  =   0.02;	% specified ratio of singular values
  svt  =  10; 	% specified number of states to identify

end % switch
end % NEW_MODEL -----------------------------------------------------------

if ~exist('INPUT','var')
  return;
end

t  = [0:N-1]*dt;				% time

%if  NEW_DATA 				% simulate data and add msmnt noise ...

%% SIMULATE INPUT SEQUENCES ================================================
% compute input sequences and their theoretical RMS (if possible) 
INPUT

[t,u_true,N,RMSu,Au,Bu,Cu,Du] = inputs( INPUT, r, dt, N, tau  ); 

plot(t,u_true)

%% SIMULATE OUTPUT SEQUENCES ===============================================
 y_true = lsym(Ac,Bc,C,D,u_true,t);

%% RMS of each output SEQUENCE =============================================
 switch INPUT
   case 'CHIRP'
       RMSy = sqrt(var(y_true'));
   case 'IMPULSE'
       RMSy = sqrt(var(y_true'));
   case 'POISSON'
       RMSy = sqrt(var(y_true'));
   otherwise 
       nu = size(Au,1);
       As = [ Au , zeros(nu,n) ; Bc*Cu , Ac ]; 
       Bs = [ Bu ; zeros(n,r) ];
       Cs = [zeros(m,nu) C ]; 
       Ds = zeros(m,r); 
       RMSy = sqrt(diag(real(Cs*liap(As,Bs)*Cs')));  
 end

%% SIMULATE UNCORRELATED INPUT AND OUTPUT NOISE SEQUENCES ===================
% measurement noise modeling : sum of white, "1/f", and Poisson noise
 rm = r+m;                   % uncorrelated noise on "n" inputs and "m" outputs 

SNRu
SNRy

% WHITE NOISE ("Nyquist")  -------------------------------------------------
 n_wn   = randn(rm,N)/sqrt(dt);                   % white noise
 RMS_wn = sqrt(1/dt); 

% FLICKER NOISE ("1/f" or "pink")  -----------------------------------------
 wc =  0.5*(pi/dt);     % frequency at 1/2 power  ( <= pi/dt )
 an = -sqrt(2 + wc^2);  % power of "1/f" noise is -50% at w = wc
 An = an*eye(rm);  Bn = eye(rm);  Cn = eye(rm);  Dn = zeros(rm);
 RMS_fn = sqrt(-1/(4*an));                         % RMS of each flicker noise
 n_fn = lsym(An,Bn,Cn,Dn,randn(rm,N)/sqrt(dt),t);  % flicker noise

% POISSON NOISE zero-mean, bi-polar ("shot")  ------------------------------
 T_pn = 1;
 n_pn = Poisson_rnd(T_pn,rm,N).*(2*(rand(rm,N)>0.5)-1);
 RMS_pn = sqrt(2/T_pn);                % ... for bi-polar Poisson noise.   

% weighted sum of noise such that var(noise) = 1 ..........................
%   more fn: less White ,  more pn: less Gaussean 
%                     wn   fn   pn
%noise_wght = abs ( [ 10 , 50 , 20 ] );
 noise_wght = sqrt ( NOISE_WEIGHT.^2 / sum(NOISE_WEIGHT.^2) );
 noise_wght = noise_wght ./ [ RMS_wn , RMS_fn , RMS_pn ];

 noise = noise_wght(1)*n_wn + noise_wght(2)*n_fn + noise_wght(3)*n_pn; 

% add scaled noise to true input / output data  ............................
 u = u_true + diag(RMSu(:)./SNRu(:))*noise( 1 :r ,:);  % noise on  input msmnts
 y = y_true + diag(RMSy(:)./SNRy(:))*noise(r+1:rm,:);  % noise on output msmnts

 sqrt_u_err_psd = diag(RMSu./SNRu(:))*sqrt(dt); 
 sqrt_y_err_psd = diag(RMSy./SNRy(:))*sqrt(dt);
 Q = (Bc*sqrt_u_err_psd)'*(Bc*sqrt_u_err_psd);    % process noise covariance
 R = sqrt_y_err_psd*sqrt_y_err_psd';              %  msmnt  noise covariance

%end % NEW_DATA ---------------------------------------------------------
%}

if PLOTS % ------------------------------------------------------------- 

 fprintf('\n True Model Dynamics: ')
 damp(Ac)	  % check the eigenvalues of the true dynamics matrix

 [mag] = mimoBode(Ac,Bc,C,D,w,[],1000,'y',leg); 

 % input/output time series data
 plotEnsemble(t, u, u_true, [], 1001 , min(t), max(t), 'u')
 plotEnsemble(t, y, y_true, [], 1002 , min(t), max(t), 'y')

 figure(1003)   % PSD of input data and noise
  clf
   [Puu,fuu] = psd(u_true(1,:),1/dt);
   [Pnn,fnn] = psd((RMSu(1)/SNRu(1))*noise(1,:),1/dt);
   loglog(fnn(2:end),Pnn(2:end),fuu(2:end),Puu(2:end));
   axis([ fnn(2), fnn(end), min(Pnn)/5, 2*max(Puu) ])
     xlabel('frequency, Hz')
     ylabel('input and noise PSD')

 figure(1004)   % PSD of output data and noise
  clf
   [Pyy,fyy] = psd(y_true(1,:),1/dt);
   [Pnn,fnn] = psd((RMSy(1)/SNRy(1))*noise(1,:),1/dt);
   loglog(fnn(2:end),Pnn(2:end),fyy(2:end),Pyy(2:end));
   axis([ fnn(2), fnn(end), min(Pnn)/5, 2*max(Pyy) ])
     xlabel('frequency, Hz')
     ylabel('output and noise PSD')

end % PLOTS ------------------------------------------------------------

% SYSTEM_ID_MODELS ------------------------------------------------------- 
% H.P. Gavin, 2009-03-31, 2013-11-7, 2015-08-4, 2015-10-09, 2018-01-07
