clear all
close all
clc

% Sliding Mode Control Demos
% State Feedback vs Sliding Mode, Sliding Estimator, Smooth Switch
% Austin Gurley 2018
%
% Example adapted from 'Sliding Mode Control and Observation'
% > 2nd order mass
% > full state is measured and accessible
% > time varying disturbance force f(t) is applied

%% State Feedback Intro 
% > use pole placement to design a linear controller
% > observe the effect of changing gain on disturbance rejection
% > look at the phase plane to get a feel for the response
% > watch out the control effort lags behind the disturbance

dt = 0.001; % time step
t = 0:dt:8; % time vector

x1 = 1;     % position
x2 = 0;    % velocity
dx1 = x2;
dx2 = 0;
y1 = x1;    % position measurement
y2 = x2;    % velocity measurement
u = 0;      % control effort

f = @(t) sin(2*t); % disturbance (function of time)
noise = 0.0;         % std of measurement noise


% use pole placement for control design:
% pkg load control
A = [0 1;0 0];
B = [0;1];
C = [1,0];
D = 0;
G = 2; % higher gain = better disturbance rejection
eigs = G*[-1+1i, -1-1i];
K = place(A,B,eigs);
k1 = K(1);
k2 = K(2);
%k1 = 8; % placed at -2+-2i
%k2 = 4; % placed at -2+-2i
%k1 = 3; % unfair value from Schtessel
%k2 = 4; % unfair value from Schtessel


for i = 1:(length(t)-1)
  % plant simulation
  dx2(i+1) = u(i) + f(t(i));
  dx1(i+1) = x2(i);
   x2(i+1) = x2(i) + dx2(i+1)*dt;
   x1(i+1) = x1(i) + dx1(i+1)*dt;
   
  % control update
  y1(i+1) = x1(i+1) + noise*randn();
  y2(i+1) = x2(i+1) + noise*randn();
  
  u(i+1) = -k1*y1(i+1) -k2*y2(i+1);
  
end

figure

subplot(2,1,1)
plot(t,x1,'b')
hold on
plot(t,x2,'b--')
legend('x_1','x_2')
xlabel('time (s)')
title('Linear State Feedback')
grid on
ylim([-2 1])

subplot(2,2,3)
plot(t,u,'b')
xlabel('time (s)')
ylabel('control effort')
grid on

subplot(2,2,4)
plot(x1,x2,'b')
xlabel('x_1')
ylabel('x_2')
grid on


%% Sliding Control 1 - Basic
% > implement a sliding mode controller with switching part only
% > watch the controller drive sigma to zero, similar to Demo 1
% > watch the controller cancel the disturbance without lag
% > watch the switching control, when low passed, match the 'equivalent' control
% > see the estimate of the disturbance, with no delay except due to low-pass
% > observe the effect of changing the gain 'rho'
% > observe the effect of changing the desired dynamics pole 'c' 
% > [uncomment lines  in sim loop to run a controller designed using Lyapunov]

x1 = 1;    % position
x2 = 0;    % velocity
dx1 = x2;
dx2 = 0;
y1 = x1;    % position measurement
y2 = x2;    % velocity measurement
u = 0;      % control effort

rho = 2.0;  % switching law gain
c = 1.5;    % desired dynamics pole

tau = .05;  % low pass filter time constant used to estimate equivalent control
 dz = 0;
  z = 0;    % estimate of equivalent control
ueq = 0;    % exact equivalent control - cheating as if we know f(t)


for i = 1:(length(t)-1)
  % plant simulation
  dx2(i+1) = u(i) + f(t(i));
  dx1(i+1) = x2(i);
   x2(i+1) = x2(i) + dx2(i+1)*dt;
   x1(i+1) = x1(i) + dx1(i+1)*dt;
   
  % control update
  y1(i+1) = x1(i+1) + noise*randn();
  y2(i+1) = x2(i+1) + noise*randn();
  
  s(i+1) = y2(i+1) + c*y1(i+1);
  %u(i+1) = -c*y2(i+1) - rho*sign(s(i+1)); % from Lyapunov
  u(i+1) = -rho*sign(s(i+1));             % just the switch
  
  % equivalent control / estimator
  dz(i+1) = -1/tau*z(i) + 1/tau*sign(s(i+1));
   z(i+1) = z(i) + dz(i+1)*dt;
  % ueq(i+1) = -c*x2(i+1) - rho*z(i+1); % from Lyapunov
   ueq(i+1) = - rho*z(i+1);            % just the switch
  %   fhat(i+1) = rho*z(i+1);           % from Lyapunov
   fhat(i+1) = rho*z(i+1) - c*x2(i+1); % just the switch
  
end

x0 = x1(1);
xf = x1(length(t));
tss = t(length(t));
xss = x1(length(t));
for i = 1:length(t)
   if abs(x1(i)-xf)>(x0-xf)*0.01
      tss = t(i);
      xss = x1(i);
   end
end
fprintf("Sliding control reaches SS at t = %2.2f\n",tss);

figure

subplot(2,1,1)
plot(t,x1,'b')
hold on
plot(t,x2,'b--')
plot(tss,xss,'b*')
legend('x_1','x_2')
xlabel('time (s)')
title('Conventional SMC')
grid on
ylim([-2 1])

subplot(2,2,3)
plot(t,u,'c')
hold on
plot(t,ueq,'b')
plot(t,-c*x2-f(t),'k')
xlabel('time (s)')
ylabel('control effort')
legend('u','u_e_q est','u_e_q ideal')
grid on

subplot(2,2,4)
plot(x1,x2,'b')
hold on
plot([-0.2 1],[0.2*c -1*c],'r--')
grid on
xlabel('x_1')
ylabel('x_2')

figure
plot(t,s,'b')
xlabel('time (s)')
ylabel('\sigma')
title('Reaching the Sliding Surface')
grid on


figure
plot(t,fhat,'b--')
hold on
plot(t,f(t),'r')
xlabel('time (s)')
ylabel('disturbance')
legend('f estimate','f(t)')
title('SMC Disturbance Estimator')
grid on


%% Sliding Control 2 - Smooth Switch
% > same as previous demo, but replaces sign(x) with a softer function
% > note that sigma = 0 is never reached exactly with this controller. 

x1 = 1;     % position
x2 = 0;     % velocity
dx1 = x2;
dx2 = 0;
y1 = x1;    % position measurement
y2 = x2;    % velocity measurement

%signum = @(x) x/(abs(x) + 0.01);
signum = @(x) tanh(20*x);
u = 0;
rho = 2;
c = 1.5;

for i = 1:(length(t)-1)
  % plant simulation
  dx2(i+1) = u(i) + f(t(i));
  dx1(i+1) = x2(i);
   x2(i+1) = x2(i) + dx2(i+1)*dt;
   x1(i+1) = x1(i) + dx1(i+1)*dt;
   
  % control update
  y1(i+1) = x1(i+1) + noise*randn();
  y2(i+1) = x2(i+1) + noise*randn();
  
  s(i+1) = y2(i+1) + c*y1(i+1);
  u(i+1) = -c*y2(i+1) - rho*signum(s(i+1)); % from Lyapunov
  %u(i+1) = -rho*signum(s(i+1)); % just the switch
  
end

figure

subplot(2,1,1)
plot(t,x1,'b')
hold on
plot(t,x2,'b--')
legend('x_1','x_2')
xlabel('time (s)')
title('Soft Switch SMC')
grid on
ylim([-2 1])

subplot(2,2,3)
plot(t,u,'b')
xlabel('time (s)')
ylabel('control effort')
grid on

subplot(2,2,4)
plot(x1,x2,'b')
hold on
plot([-0.2 1],[0.2*c -1*c],'r--')
grid on
xlabel('x_1')
ylabel('x_2')

%{
%% Sliding Control 3 - Embedded Integrator
% > smooth the control effort by analytically integrating
% > reaches sigma = 0 exactly AND has smoothed the control effort

x1 = 1;
x2 = 0;
dx1 = x2;
dx2 = 0;
y1 = x1;    % position measurement
y2 = x2;    % velocity measurement

du = 0;
 u = 0;     % control effort
 
rho = 30;   % control gain
c = 1.5;    % switching pole
cb = 10;    % filtering pole


for i = 1:(length(t)-1)
  % plant simulation
  dx2(i+1) = u(i) + f(t(i));
  dx1(i+1) = x2(i);
   x2(i+1) = x2(i) + dx2(i+1)*dt;
   x1(i+1) = x1(i) + dx1(i+1)*dt;
   
  % control update
  y1(i+1) = x1(i+1) + noise*randn();
  y2(i+1) = x2(i+1) + noise*randn();
  
  sig(i+1) = y2(i+1) + c*y1(i+1);
 dsig(i+1) = (sig(i+1)-sig(i))/dt;
    s(i+1) = dsig(i+1) + cb*sig(i+1);
  du(i+1) = -c*cb*y2(i+1) - (c+cb)*u(i) - rho*sign(s(i+1));
   u(i+1) = u(i) + du(i+1)*dt;
   
end

figure

subplot(2,1,1)
plot(t,x1,'b')
hold on
plot(t,x2,'b--')
legend('x_1','x_2')
xlabel('time (s)')
title('Higher Order SMC')
grid on
ylim([-2 1])

subplot(2,2,3)
plot(t,u,'b')
xlabel('time (s)')
ylabel('control effort')
grid on

subplot(2,2,4)
plot(x1,x2,'b')
hold on
plot([-0.2 1],[0.2*c -1*c],'r--')
grid on
xlabel('x_1')
ylabel('x_2')

%}
