%% Optimal Semi-Active Control
% "Determining the physical limits on semi-active control performance: A tutorial"
% submitted for publication in Struct Control Health Monit
% Harvey, Gavin, Scruggs, & Rinker
%
% Written:  PSH     5-Sept-2012
% Editted:  PSH     6-Feb-2013

clear; clc; close all
global dt nT n
global A B Bw Q R T S umax cmax

%% Structural Properties
mu     = 0.10;              % mass ratio mt/ms, []
omegas = 1.0;               % primary structure natural frequency, [rad/s]
omegat = omegas/(1+mu);     % TMD natural frequency, [rad/s] ... Den Hartog value
ms     = 1e6;               % primary structure mass, [kg]
zetat  = 0.01;              % TMD inherent damping ratio, [ ]

% stiffnesses and damping
zetas  = 0.01;              % primary structure damping ratio
ks = omegas^2*ms;           % primary structure stiffness, N/m
cs = 2*zetas*omegas*ms;     % primary structure damping rate, N-s/m
mt = mu*ms;                 % TMD mass (rounded), kg
kt = omegat^2*mt;           % TMD stiffness, N/m
ct = 2*zetat*omegat*mt;     % TMD damping rate, N-s/s

% Den Hartog values
omegat_DH = omegas/(1+mu);              % Den Hartog TMD natural frequency, [rad/s]
zetat_DH  = sqrt(3*mu/(8*(1+mu)));      % Den Hartog TMD damping ratio, []
ct_DH     = 2*zetat_DH*omegat_DH*mt;  	% Den Hartog TMD damping rate, [N-s/m]

%% State-space matrices .... eq'n (17)
n  = 4;                             % number of states
A  = [        zeros(2)                          eye(2)    % dynamics matrix
      -omegas^2    mu*omegat^2    -2*zetas*omegas   mu*2*zetat*omegat
       omegas^2  -(1+mu)*omegat^2   2*zetas*omegas  -(1+mu)*2*zetat*omegat];
B  = [0 0 mu/mt -(1+mu)/mt]';       % control input matrix
Bw = [0 0 -1 0]';                   % disturbance input matrix

%% Constraints ... g = [ u^2-umax^2 ; u*(u-cmax*T'*x) ]
umax = 5e4;                     % max semi-active force, [N]
zeta_max = 0.18;                % max damping rate, []
cmax = 2*zeta_max*omegat*mt;    % max damping coefficient, [N-s/m]
T = [0 0 0 1]';                 % velocity extraction vector

%% Performance matrices ... Section 3.3
b1 = [0 0 1 0];                 % acceleration extraction vector
Q = A'*(b1'*b1)*A;              % state weighting matrix
S = A'*(b1'*b1)*B;              % state-control weighting matrix
R = B'*(b1'*b1)*B;              % control weighting matrix

%% Disturbance ... Section 3.2
Tp = 2*pi/omegas;               % pulse period, [s]
Vp = 0.8;                       % peak velocity, [m/s]
Nc = 2.0;                       % number of strong cycles, [ ]
t0 = 2.0;                       % start time, [s]
dt = 0.05;                      % time step, [s]
tf = 60;                        % terminal time, [s]
t = 0:dt:tf;                    % time, [s]
[w,xgd,xg] = pulseVx(Vp,Tp,Nc,t,t0,0); % pulse-like disturbance
nT = length(t);                 % number of time steps
figure; plot(t,w/9.81,'linewidth',2); ylabel('acceleration x_g" (g)')

%% Solve BVP
x0 = zeros(n,1); pf = zeros(n,1);   % initial states and terminal co-states
options = bvpset('NMax',100*nT,'Stats','on');   % set options
solinit = bvpinit(t,5*eps*ones(2*n,1));         % initialize solution
sol = bvp5c(@(t,z) zdot(t,z,w),@(z0,zf) bcfun(z0,zf,x0,pf,n),solinit,options);

%% Post-processing
z = deval(sol,t);           % extract states and co-states
x = z(1:n,:);               % x(:,1) = x0;
p = z(n+1:2*n,:);           % p(:,end) = pf;
u = nan(1,nT); lambda = nan(2,nT); % initialize control u and multipliers lambda
for ii = 1:nT; [u(ii),lambda(:,ii)] = sat(x(:,ii),-R\(S'*x(:,ii) + B'*p(:,ii)),p(:,ii)); end
g = [u.^2-umax^2 ; u.*(u - cmax*T'*x)]; % evaluate contraints
J = cumsum(diag(x'*Q*x)'+2*diag(x'*S*u)'+R*u.^2)*dt/2;  % calculate cost functional
accel = b1*A*x + b1*B*u;    % total acceleration, [m/s/s]
      
%% Plot
figure(12); plot(t,u,'linewidth',2); ylabel('control u(t)'); ylim(1.2*umax*[-1,1])
figure(22); plot(T'*x,u,'linewidth',2); 
    ylabel('control u(t)'); xlabel('velocity v(t)');ylim(1.2*umax*[-1,1])
figure(52); plot(t,J,'linewidth',2); ylabel('cost J(t)')
figure(39); plot(t,accel,'linewidth',2); ylabel('total accel')