%% ORR - lab 7 - Time-optimal control
% The topic of this' week seminar is time-optimal control and the associated implementation issues. Specifically, we will implement the time-optimal control law for the input constrained double integrator system. The goal is to get the double integrator system to the origin. Recall that the double integrator system is described by the following state-space model:
% \left\lbrack \begin{array}{c}
% \dot{y} \left(t\right)\\
% \dot{v} \left(t\right)
% \end{array}\right\rbrack =\left\lbrack \begin{array}{cc}
% 0 & 1\\
% 0 & 0
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% y\left(t\right)\\
% v\left(t\right)
% \end{array}\right\rbrack +\left\lbrack \begin{array}{c}
% 0\\
% 1
% \end{array}\right\rbrack u\left(t\right),\;\;\;\;\;u\left(t\right)\in \left\lbrack -1,1\right\rbrack \ldotp
% As derived in the lecture notes, the time-optimal control law for this partcular case is
% u\left(t\right)=\mathrm{sgn}\left(-\frac{1}{2}v\left(t\right)\left|v\left(t\right)\right|-y\left(t\right)\right)\ldotp
% Your task for this seminar is to implement this optimal control law in Simulink and simulate the response of the closed loop. Answer the following questions:
% Does the naive implementation (meaning, without any hacks and heuristics) work? Why not?
% Can you come up with a solution? Try to somehow approximate the optimal switching law by something more realizable. If you want to right away use a more systematic approach, you can implement a so-called Proximate Time-Optimal Servomechanism (PTOS) method [1].
% You can use the attached Simulink model in which the double integrator system is already implemented.
%% Motivational side note
% The problem of getting the double integrator system to the origin may seem to be somewhat artificial and useless. Nevertheless, that would be a wrong impression. First of all, one can easily bring the double integrator system to a reference position r\left(t\right) simply by using y_r \left(t\right)=y\left(t\right)-r\left(t\right) in place of y\left(t\right) in the control law. Second, frictionless mechanical systems can often be described by the double integrator model. For instance, if you have a motor with a torque controller--you set a reference torque and the motor almost immediately generates it--then the dynamics of the load attached to the motor is described by the double integrator model (again, if the friction is negligible). In other words, getting the double integrator to the origin as quickly as possible may actually be the same problem as rotating a shaft of a motor to a given angle as quickly as possible.
% If you are still not impressed, check this video where the world record in solving the Rubik's cube is recorded. The robot solves the Rubik's cube under 400 ms. Such an amazing speed was possible only because the authors of the robot used time-optimal control. In fact, they used quite a similar approach to the one you are about to practice. You can find the details about the controller they used here.
%
%
% [1] WORKMAN, Michael L.; KOSUT, Robert L.; FRANKLIN, Gene F. Adaptive proximate time-optimal servomechanisms: Continuous time case. In: 1987 American Control Conference. IEEE, 1987. p. 589-594.
%% Solution
% To get rid of the chattering the problem (frequent switchin of the control input), one can approximate the time-optimal control law in many ways. If the control input is constrained to the discrete values {-1, 1} then your options are limited. You can implement a deadzone around zero, i.e. you will not control the system when you get close to the switchin curve, or you can use hysteresis in pace of the sign function. If the control input can also have values in between the saturation limits (i.e. u\left(t\right)is from the interval [-1,1]) then you can for instance replace the sign function in the control law by a saturated linear function. Here, however, we implement the so-called Proximate Time-Optimal Servomechanism (PTOS) presented in [1].
% The optimal control law to be approximated is given by
% \begin{array}{l}
% u\left(t\right)=\mathrm{sgn}\left(f_{\mathrm{to}} \left(y\left(t\right)\right)-v\left(t\right)\right),\\
% f_{\mathrm{to}} \left(y\left(t\right)\right)=-\mathrm{sgn}\left(y\left(t\right)\right)\left(\sqrt{2|y\left(t\right)|}\right)\ldotp
% \end{array}
% Even though this looks different from the time-optimal control law stated in the lecture notes or at the begining of this worksheet, it is actually exactly the same. If you have doubts, go to the lecture notes and express the switching curve in Fig. 6 in terms of y\left(t\right) instead of v\left(t\right). PTOS does two things. It replaces the sign function in u\left(t\right) by a saturation function and it also replaces the switching function f_{\mathrm{to}} \left(y\left(t\right)\right)as follows:
% u\left(t\right)=\mathrm{sat}\left(k_2 \left(f\left(y\left(t\right)\right)-v\left(t\right)\right)\right),
% f\left(y\left(t\right)\right)=\left\lbrace \begin{array}{ll}
% -\frac{k_1 }{k_2 }y\left(t\right), & |y\left(t\right)|\le y_l ,\\
% -\mathrm{sgn}\left(y\left(t\right)\right)\left(\sqrt{2\alpha |y\left(t\right)|}-\frac{1}{k_2 }\right), & |y\left(t\right)|>y_l ,
% \end{array}\right.
% where k_1, k_2, y_l and \alpha are tuning parameters. Sufficient conditions for the closed loop to be stable are:
% k_2 =\sqrt{2\frac{k_1 }{\alpha \;}},\;\;\;\;\;\;\;\;y_l =\frac{1}{k_1 }\;\;\;\;\;\;\;\mathrm{and}\;\;\;\;\;\;0<\alpha <1.
% Examining the approximated control law, one notices that when y\left(t\right) is close to the origin (y\left(t\right)\le y_l), the control law is u\left(t\right)=\mathrm{sat}\left(-k_1 y\left(t\right)-k_2 v\left(t\right)\right).Hence, within the saturation limits, this control law is a state-feedback controller. That means, we can compute k_1 and k_2 by designing a state feedback controler. Having k_1 and k_2, we compute \alpha \;and y_l and check that \alpha \;is within the admissible limits. If so, we have a stabilizing approximate time-optimal controller. That is exactly what the following code does.

A =[0 1;0 0];
B = [0;1];
% Design a state-feedback controller by LQR
K = lqr(A, B, diag([10, .01]), .01);
k1 = K(1); k2 = K(2);
% Compute alpha and yl
alpha = 2*k1/k2^2;
yl = 1/k1;
% Check that alpha is smaller than 1
fprintf('alpha: %4.2f, yl: %4.2f\n', alpha, yl);

% Having all the neccesary parameters, we can simulate the response of the close loop.

sim('doubleIntgrator_ptos.slx')

figure(1)
clf

subplot(211)
plot(ScopeData.time, ScopeData.signals(2).values)
xlabel('Time [s]')
legend('x(t)', 'v(t)')

subplot(212)
plot(ScopeData.time, ScopeData.signals(1).values)
xlabel('Time [s]')
ylabel('u(t)')

% You can see that we got rid of the chattering problem while we still have a very quick response.