%% Linealización alrededor de una trayectoria de un péndulo
% © 2018 Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados.
% Este código ha sido ejecutado en Matlab R2018a.
% Presentación en vídeo en: http://personales.upv.es/asala/YT/V/ltraym.html
%
%% Modelo físico
% El modelo del péndulo es:
% \frac{d\alpha}{dt}=\omega \\
% \frac{d\omega}{dt}=\frac{1}{J}(MgL\cdot \sin\alpha-\beta\cdot \omega+u)
% donde \alpha es la posición angular, \omega es la velocidad angular, u es la entrada (variable) manipulada.
% Los parámetros constantes son:

M=2;L=1;%masa y longitud
beta=0.15; %rozamiento viscoso
J=M*L^2; %momento de inercia: masa puntual en el extremo.
g=9.81; %gravedad

% La representación interna normalizada \dot x=f(x,u) será introducida como:

syms alpha omega u real
f=[omega; 1/J*(M*g*L*sin(alpha)-beta*omega+u)]

%% Análisis de puntos de equilibrio
% Ante u=0, el sistema tiene dos puntos de equilibrio:

sol=solve([f==0,u==0],'ReturnConditions', true)
sol.alpha, sol.conditions
sol.omega

%% Linealización "clásica" en los puntos de equilibrio (constantes)
% Linealizamos en los puntos 0 y pi:

Alin=jacobian(f,[alpha,omega]) %matriz de derivadas parciales
%evaluadas sobre la trayectoria de referencia
Aarriba=eval(subs(Alin,{alpha,omega},{0,0}))
Aabajo=eval(subs(Alin,{alpha,omega},{pi,0}))
Blin=eval(jacobian(f,u))
%como sale constante, el sistema es "lineal en la entrada"
%no hace falta sustituir ángulos en los puntos de equilibrio.
eig(Aarriba)
eig(Aabajo)

% El punto de equilibrio estable (polos con parte real negativa) es \alpha=\pi.
%
%% Generación de trayectoria de referencia e inversión de la dinámica
% Se desea seguir una trayectoria del ángulo prefijada \alpha_{ref}(t), dada por

syms t real
alpha_ref=pi*exp(-t) 
alpha_ref_inicial=subs(alpha_ref,t,0)
fplot(alpha_ref,[0 6]), grid on

% Con lo que la velocidad angular en función del tiempo \omega_{ref}(t) deberá ser su derivada:

omega_ref=diff(alpha_ref,t)
omega_ref_inicial=subs(omega_ref,t,0)

% A partir del modelo puede calcularse la entrada u_{ref}(t) que consigue la trayectoria angular deseada, SI INCIALMENTE el estado YA TIENE LA POSICIÓN Y VELOCIDAD de la trayectoria (si no, hace falta CONTROL para que converja a la trayectoria "rápidamente"):
% u_{ref}=J \frac{d\omega_{ref}}{dt}-Mg\sin \alpha_{ref}+\beta\cdot\omega_{ref}
% Esto es:

u_ref=J*diff(omega_ref,t)-M*g*sin(alpha_ref)+beta*omega_ref
fplot(u_ref,[0 6]), grid on

% Si el modelo fuera perfecto, introduciendo ese par en función del tiempo u_{ref}(t), la trayectoria sería seguida con precisión.
%
%
%% Linealización (modelo incremental) alrededor de trayectoria

Alin=jacobian(f,[alpha,omega]) %matriz de derivadas parciales
%evaluadas sobre la trayectoria de referencia
Atray=subs(Alin,{alpha,omega},{alpha_ref,omega_ref}) 
Blin=eval(jacobian(f,u)) %no depende de ningún parámetro, es lineal en la entrada

% Por tanto, el modelo linealizado es un modelo variante en el tiempo \frac{d\Delta x}{dt}=A(t)\Delta x+B\Delta u donde
% A(t)=\left(\begin{array}{cc}1 &0 \\ a_{21}(t) &-3/40 \end{array}\right)
% siendo el coeficiente a_{21}(t)\in [-9.81, +9.81].
%% Comprobación de la aproximación:
% Vamos a simular ante la entrada u=u_{ref}+0.12\cdot\sin t, y comparar las soluciones dadas por el sistema no-lineal original y sus aproximaciones linealizadas( 1. constante alrededor de \pi y 2. variante en el tiempo alrededor de la trayectoria).

Delta_u_simulado=0.12*sin(t);
ControlSimulado=u_ref+Delta_u_simulado
modeloSimulaNoLinealSimbolico=subs(f,u,ControlSimulado)
modeloSimulaNoLinealNumerico=matlabFunction(modeloSimulaNoLinealSimbolico);
syms deltaalpha deltaomega deltau real
uPF=0;
modeloSimulaIncrementLTISimbolico=subs(Aabajo*[deltaalpha;deltaomega]+Blin*deltau,deltau,ControlSimulado-uPF)
modeloSimulaIncrementLTINumerico=matlabFunction(modeloSimulaIncrementLTISimbolico);
modeloSimulaIncrementLTVSimbolico=subs(Atray*[deltaalpha;deltaomega]+Blin*deltau,deltau,ControlSimulado-u_ref) %eso da Delta_u, claro!
modeloSimulaIncrementLTVNumerico=matlabFunction(modeloSimulaIncrementLTVSimbolico);
tfinal=4;
xinicial=[pi+0.02;-pi+0.03];%posición y velocidad iniciales
xpf=[pi;0];
solNoLineal = ode45(@(t,x) modeloSimulaNoLinealNumerico(x(1),x(2),t) ,[0 tfinal],xinicial);%tolerancias por defecto
incXinicial=xinicial-xpf;
sol_inc_LinealizPunto = ode45(@(t,x) modeloSimulaIncrementLTINumerico(x(1),x(2),t) ,[0 tfinal],incXinicial);%tolerancias por defecto
incXinicial=eval(xinicial-[alpha_ref_inicial;omega_ref_inicial]);
sol_inc_LinealizTrayect = ode45(@(t,x) modeloSimulaIncrementLTVNumerico(x(1),x(2),t) ,[0 tfinal],incXinicial);%tolerancias por defecto

% Representemos gráfica:

tiempos=linspace(0,tfinal,100);
aa=matlabFunction(alpha_ref);
anguloideal=aa(tiempos);
aaw=matlabFunction(omega_ref);
velocidadideal=aaw(tiempos);
xNL_simulado=deval(solNoLineal,tiempos);
xLinPto_simulado=xpf*ones(1,100)+deval(sol_inc_LinealizPunto,tiempos);
xLinTraj_simulado=[anguloideal;velocidadideal]+deval(sol_inc_LinealizTrayect,tiempos);

plot(tiempos,[xNL_simulado(1,:); xLinPto_simulado(1,:); xLinTraj_simulado(1,:)]), grid on
hold on
plot(tiempos,anguloideal,'m:')
hold off
legend('PosicionNL','Pos LinPTO','Pos LinTray','alpharef','Location','Best'), axis tight

% Se observa que al aproximarse y superar \pi/2, las simulaciones se alejan de la trayectoria de referencia (en esas posiciones el péndulo es "inestable", con lo que las diferencias se amplian con el tiempo incluso con el modelo correcto --linea azul, simulación NL--: mantener el péndulo en la parte superior requiere control en bucle cerrado que estabilice). Una vez alejadas de la trayectoria de referencia, la simulación no lineal y la simulación LTV se separan. La linealización en el punto de equilibrio inferior \alpha_{eq}=\pi se separa pronto de las trayectorias del sistema no-lineal.