%% Optimización del transitorio hasta punto de equilibrio de un sistema de segundo orden (péndulo): ode45+fminunc
% © 2019, Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados.
% Presentación en vídeo: http://personales.upv.es/asala/YT/V/optode1.html , http://personales.upv.es/asala/YT/V/optode2.html , http://personales.upv.es/asala/YT/V/optode3.html .
% Este código ejecutó correctamente en Matlab R2019b.
%
% Motivación y objetivos: uno de los problemas más importantes de control es el de "estabilización" o "alcanzar" un punto de funcionamiento (equilibrio) dado de una forma "adecuada" desde ciertas condiciones iniciales. De hecho, es casi equivalente al cambio de referencia constante (ir de un punto de equilibrio a otro).
% En este material, se discute cómo dar una solución a dicho problema en un modelo de péndulo invertido inestable, mediante optimización de un índice de coste donde se ponderan las desviaciones de la posición, la velocidad (suavidad del movimiento) y la fuerza necesaria (que en la práctica está limitada por la saturación y, por tanto, no puede ser arbitrariamente grande), en un controlador tipo PD (control es función de posición y velocidad).
%
%                                                     *VIDEO 1: Modelado, Ecuaciones de bucle cerrado, función objetivo.
%                                                     *VIDEO 2: Simulación y Optimización.
%                                                     *VIDEO 3: Modificación índice de coste. Comparación con solución lineal.
% Tabla de Contenidos
% 1.- Modelado
%     Estructura del controlador
% 2.- Modelo en bucle cerrado y función objetivo
% 3.- Simulación con ODE45 del modelo y de la acumulación de coste
% 4.- Obtención de los parámetros óptimos del controlador con fminunc
%     Comparación con regulador óptimo LQR (linear quadratic regulator) sobre el modelo linealizado
% Conclusiones
% Funciones auxiliares
%
%
%
%% 1.- Modelado
% Modelemos la planta a controlar (ecuaciones de estado) de un péndulo invertido ( p=0 indica equilibrio superior) sin rozamiento.
%         \frac{dp}{dt}=v,     \frac{dv}{dt}=g\sin(p)+\frac{1}{M}u
% El vector de estado lo definimos con x=(p,v)^T.

dxdtPlanta = @(x, u)   [  x(2); ...
                        9.81*sin(x(1)) + u/0.33 ];

% Luego usaremos el modelo linealizado en el origen, sustituyendo \sin(x_1)\approx x_1, para comparaciones \frac{dx}{dt}=Ax+Bu:

A=[0 1;9.81 0]; B=[0;1/0.33];
eig(A) %es inestable

%% Estructura del controlador
% El controlador utilizado será tipo "PD", u=-K_pp-K_v\frac{dp}{dt}=-K_pp-K_vv, con el objetivo de estabilizar al sistema y alcanzar la posición cero de equilibrio con cierta rapidez.
% Como la derivada de la posición es la segunda variable de estado, podemos expresar el control como u=-K_px_1-K_vx_2=-[K_p\ K_v]\cdot x, con lo que tendremos un vector de "parámetros ajustables del controlador" [K_p \ K_v] de dimensión 1\times 2.
% A esta expresión se le denomina en teoría de control "realimentación del estado", pero es básicamente equivalente a un control PD en sistemas de segundo orden  (excepto modificaciones para filtrar ruidos de medida y estimar la velocidad por derivada numérica de posición si no se dispusiera de sensor físico de la misma).
% Programaremos la función que devuelve la acción de control dado el estado (posición, velocidad) y parámetros (a decidir por el optimizador) mediante:

Control = @(x, params)   -params(1)*x(1,:)-params(2)*x(2,:); 
%el x(1,:) en vez de x(1) están pensado para poder calcular en una linea "muchas" acciones de control,
% dada una trayectoria de estados en las gráficas.

%% 2.- Modelo en bucle cerrado y función objetivo
% Las ecuaclones de estado bucle cerrado se obtienen sustituyendo el control en el modelo de la planta (necesitan saber las ganancias del controlador, claro):

dxdt_BucleCerrado = @(x, params)   dxdtPlanta(x,Control(x,params));

% Vamos a intentar obtener los parámetros que minimizan la integral (coste acumulado) de la función:

coste = @(x, u)   x(1)^2 + 0.1*x(2)^2 + 0.06*u^2;

%
% Objetivo: minimizar la integral de coste(x,u) durante un tiempo de simulación a partir de unas condiciones iniciales prefijadas, buscando los parámetros del controlador adecuados.
%
%%
%% 3.- Simulación con ODE45 del modelo y de la acumulación de coste
% Para ello, tendremos que añadir el integrador del coste a las ecuaciones del bucle cerrado, formando un sistema de orden 3 con una ecuación adicional \frac{dJ}{dt}=coste(x,u), sustituyendo u por su expresión:

deriv_EstadoBC_y_derivJ = @(posvel, params) ...
    [ dxdt_BucleCerrado(posvel, params); ...
    coste( posvel, Control(posvel, params) ) ];

% Vamos, por ejemplo, a hacer una simulación con ode45  con un valor arbitrario inicial de los parámetros:

Kpprueba=20;
Kvprueba=1.5;
paramsprueba = [Kpprueba Kvprueba];

Tspan=[0 5]; %tiempo inicial y final de simulación

posicion_inicial = 15*pi/180; velocidad_inicial = 0;
x_inicial = [posicion_inicial;velocidad_inicial];

resul = SimulaEjemplo(Tspan, x_inicial, deriv_EstadoBC_y_derivJ, paramsprueba);
DibujaResultadoSimulacion(resul)

% El valor final es lo que se desea que sea suficientemente pequeño (buscar los parámetros que lo minimizan)
% Veamos la acción de control:

DibujaSimulacionControl(resul,Control,paramsprueba)

%% 4.- Obtención de los parámetros óptimos del controlador con fminunc
% Ahora vamos a optimizar usando la optimization toolbox de Matlab. Necesitamos "envolver" todo en una función a la que le entren los parámetros, simule, y le salga un único número:

J = @(params)  SimulaEjemplo(Tspan, x_inicial, deriv_EstadoBC_y_derivJ, params).J_simulado(end);

% En efecto, el resultado anterior es idéntico a:

J(paramsprueba)

% Si los parámetros iniciales nos han parecido satisfactorios, comenzaremos la búsqueda alrededor de ellos... cronometraremos el tiempo de ejecución.

tic,paramsoptimos=fminunc(J,paramsprueba),toc

% Simulemos el resultado:

resul2=SimulaEjemplo(Tspan,x_inicial,deriv_EstadoBC_y_derivJ,paramsoptimos);
DibujaResultadoSimulacion(resul2)

% La acción de control del regulador óptimo resulta en:

DibujaSimulacionControl(resul2,Control,paramsoptimos)

%% Comparación con regulador óptimo LQR (linear quadratic regulator) sobre el modelo linealizado
% El problema con coste cuadrático y modelo lineal tiene una solución computacionalmente eficiente a gran escala que se denomina "control LQR por realimentación del estado".
% Los parámetros óptimos se obtendrían con:

tic, K_lqr= lqr(A, B, diag([1 0.1]), 0.06), toc

% que son muy parecidos al resultado de la optimización numérica no-lineal (cuya solución es mejor, claro, pero mucho más lenta de calcular... y las diferencias de tiempo de cómputo son mucho más acentuadas en sistemas de orden mayor)

paramsoptimos

%% Conclusiones
% En este ejemplo de código se ha presentado cómo definir un índice de coste y preparar la simulación en ode45 de un modelo en bucle cerrado y de la acumulación de dicho índice de coste. Asimismo, se ha generado un "wrapper" para optimizar parámetros de un controlador con fminunc. Los resultados son similares a los del modelo linealizado (control óptimo LQR) si las condiciones iniciales están cerca del punto de equilibrio donde se ha linealizado.
% *Soluciones alterantivas similares podrían haberse obtenido con Simulink Response Optimisation o/y Simulink Control Design, fuera de los objetivos de el material aquí considerado.
%
%
%
%
%% Funciones auxiliares

function resultado=SimulaEjemplo(Tspan, x_ini, deriv_Estado_y_J, parametros)
    xJ_inicial = [x_ini; 0];
    opts = odeset('RelTol',1e-4,'AbsTol',1e-4);
    [resultado.t_simul,xJ_simulado] = ...
        ode45( @(t,xJ) deriv_Estado_y_J(xJ(1:2), parametros), Tspan, xJ_inicial, opts);
    resultado.x_simulado = xJ_simulado(:,1:2);
    resultado.J_simulado = xJ_simulado(:,3);
end

function DibujaResultadoSimulacion(resul)
    figure()
    plot(resul.t_simul, resul.x_simulado(:,1),'LineWidth',3), grid on, title('Posición (m)')
    figure()
    plot(resul.t_simul, resul.x_simulado(:,2),'-.r','LineWidth',2), grid on, title('Velocidad (m/s)')
    figure()
    plot(resul.t_simul, resul.J_simulado,'k','LineWidth',2), grid on
    title('Coste Acumulado conforme pasa tiempo')
    fprintf('El valor final del coste acumulado es: %g', resul.J_simulado(end))
end

function DibujaSimulacionControl(resul,Controlador,params)
    plot(resul.t_simul,Controlador(resul.x_simulado',params),'-.','LineWidth',3), grid on
    title('Acción de control: Fuerza (N)')
end