% This script:
% 1) Runs labFitter to convert frequency sweep data into a state-space model.  
% 2) SS model used to design CT measured-state and DT measured-state- and 
%    observed-state-feedback (Msf and Obsf) controllers.  
% 4) The DT Msf and Obsf controller simulations are compared 
%    (with noise and disturbances).
% 5) The observer-based controller and associated gains are written to 
%    file includable in appropriate Teensy sketch.
%

close all; % close all the plots.
clearvars;
dt = 1000e-6;        % DT sample period (1000 samples per second)

% Number of poles and zeros for the fitter to use.
nPoles = 3;  % ******* Why Three Poles?
nZeros = 0;  % ******* Why no zeros?

% Simulation Inputs
inAmp = 0.15;             % Input Amplitude for Simulation.

% Noise Inputs
noiseVec = [1 1e6 1e9]';   % Noise vector, How is this being used?
noiseAmp = 1e-2;    % Noise amplitude (set by experiment)

% Disturbance Inputs 
disturbScaleVec= [1,100,1000]; % Scale factors for position, vel, accel.
disturbEqn = 1;  % *******CHANGE THIS FOR TESTING********!!
disturbAmp = 0.1;
disturbAmp= disturbAmp*disturbScaleVec(disturbEqn);

% Createfrequency sweep data matrix M
% M: real, five columns and num rows = num measurements;
% M: Real, row per measurement, 5 Cols: Freq,re(U),imag(U),re(Y),imag(Y)
% M = synthData(0.25, 5, 40); (uncomnment for testing)
% M from File, Format: Line = row in M, comma-sep-vals with semicolon EOL.
M = readmatrix('sweep.csv', 'LineEnding', ';'); 

% Fit state-space model to frequency sweep data
sysOpenLoop = labFitter(nPoles,nZeros,M);
display(pole(sysOpenLoop))
A = sysOpenLoop.a; 
B = sysOpenLoop.b; 
C = sysOpenLoop.c; 
D = sysOpenLoop.d;

% Find the gains for a measured-state and estimated-state DT controller
% Examine state-space A, B, C matrices to determine what weights to use.
Q = diag([100, 1, 1]);           % ********OPTIMIZE THIS***********                     
R = 1;                             % ********OPTIMIZE THIS***********

% Compute state-feedback gains using lqr.
K = lqr(A,B,Q,R);                     

disp('***CT Controller Gains********');
disp(['K: ',num2str(K)]);
disp('***CT Control and Error-Estimate Poles**');
disp(['controller poles: ', num2str(eig(A-B*K).')]);

% Create input matrices for two additional inputs, disturbance and noise.
Bdist = zeros(size(A,1),1);
Bdist(disturbEqn) = disturbAmp;
Bnoise = noiseAmp*noiseVec(1:nPoles);  % Relative noise for state measurements.

%%%%%%%%%%%%%%%%%% Switch to Discrete-Time ***********************

% Convert system to discrete time (exact for ZOH).
Ad = expm(A*dt);  % Matrix Exponential           
B2Bd = (Ad - eye(size(Ad)))/A; % Matrix for scaling B to Bd.
Bd = B2Bd*B;
inBdistd = B2Bd * Bdist; % Scale CT state disturbance for DT
inBnoised = B2Bd * Bnoise;

Cd = C;
Dd = D;

% Recompute the controller gains (K's) for DT.
Kd = dlqr(Ad, Bd, Q, R); % Kd should be same as K if sampling fast enough


% Calc closed-loop matrices.
Adcl = Ad - Bd*Kd;
Krd = 1/(Cd*((eye(size(Ad)) - Adcl)\Bd));
Byd = Bd*Krd; 

% Compute the observer gains
Ld = place(Ad',Cd',[0.99, 0.992, 0.995])';  % Optimize this!

disp(' ');
disp('***DT Controller Gains********');
disp(['Kd Gains: ',num2str(Kd), '      Krd:',num2str(Krd)]);
disp('***DT Observer Gains********');
disp(['Ld Gains: ',num2str(Ld')]);
disp('*****||DT Controller Poles||*****');
disp(['controller poles: ', num2str(sort(abs(eig(Adcl)).'))]);
disp('*****||DT observer Poles||*****');
disp(['observer poles: ', num2str(sort(abs(eig(Ad - Ld*Cd)).'))]);



% *****Simulate Measured-State and Observed-State Feedback (Msf,Obsf)

% Discrete time inputs 
inByd = inAmp*Byd;  
inKrd = inAmp*Krd;
[yMsf,yObsf,t,uDist] = ...
    simMsfObsf(Ad,Bd,Cd,Kd,inKrd,Ld,inByd,inBdistd,inBnoised,dt);

% Plot DT Measured and Observed State.
nPlots = size(yMsf,2);  % Measured-state and Observed-State, same num plots.
plotSys(t, yMsf, nPlots, nPoles, uDist, disturbEqn, 'DT Measured State Feedback');
plotSys(t, yObsf, nPlots, nPoles, uDist, disturbEqn, 'DT Observed State Feedback');

% Output gains and model to a file compiled by Teensyduino
fname = 'obs6310.h'; % If you change this name, change it in sketch!!!
printTeensyObserver(Ad,Bd,Cd,Krd,Kd,Ld,fname)

 
