%% Linealización (acotación) de sector en un modelo de depósito no lineal
% © 2021, Antonio Sala Piqueras. Universitat Politècnica de València. Todos los derechos reservados.
% Presentaciones en vídeo:
% http://personales.upv.es/asala/YT/V/dsnl1.html , http://personales.upv.es/asala/YT/V/dsnl2.html .
% Este código funcionó sin errores en Matlab R2021a
% Objetivos: Acotar una no-linealidad dentro de un "sector incierto" delimitado por dos modelos lineales, para posterior diseño de controladores lineales robustos (o planificación de ganancia). Compararlo con otras opciones de tratar la no-linealidad (linealización, cancelación). Escalar a incertidumbre normalizada.
%%
% Tabla de Contenidos
% Modelado
% Otras opciones de linealización (sin "incertidumbre")
%     Linealización (cancelación) por realimentación
%     Linealización Taylor
%         Punto de funcionamiento
%         Modelo incremental
% Acotación de sector (incremental) de una no-linealidad
%     Escalado a [-1,+1] de la incertidumbre (incertidumbre "normalizada")
% Conclusiones
%% Modelado
% Sea un depósito:
% \frac{dh}{dt}=-\sqrt{h}+q

syms h q %estado y entrada
deriv_estado=-sqrt(h)+q

% Existe la nolinealidad \mu(h)=\sqrt{h} que deseamos de alguna forma "eliminar" para poder usar técnicas lineales, garantizando estabilidad si es posible ante el proceso no lineal original.
%
%% Otras opciones de linealización (sin "incertidumbre")
% Aunque no son objetivo de este material, se revisan brevemente.
%% Linealización (cancelación) por realimentación
% Podríamos hacer linealización por realimentacion, si h fuera medible, con q=\sqrt{h}+\nu, resultando \frac{dh}{dt}=\nu. Esto, junto con un controlador para el modelo resultante \frac{1}{s}, resultaría en un controlador "no lineal" en su conjunto. No hay ninguna "aproximación" de la no-linealidad, sino una "cancelación".
%
%% Linealización Taylor
% Es el primer paso, usualmente, para diseñar controladores lineales... pero sólo tienen garantía de funcionamiento en un entorno "pequeño" (teóricamente infinitesimal) alrededor de un punto de funcionamiento.
%% Punto de funcionamiento

pf=solve(deriv_estado==0,q==2);
pfhnum=eval(pf.h)
pfqnum=eval(pf.q)
SubstJaco=@(expr,var) eval(subs(jacobian(expr,var),{h,q},{pf.h,pf.q}));
A=SubstJaco(deriv_estado,h) %h es la variable de estado
B=SubstJaco(deriv_estado,q) %q es la variable de entrada

% Comprobemos el error de linealización si nos alejamos del punto de funcionamiento:

rangoh=[pfhnum-4 pfhnum+6]
mu=sqrt(h);
mupf=eval(sqrt(pf.h))
aproxlineal=subs(jacobian(mu),pf.h)
fplot(mu,rangoh,LineWidth=3), grid on
hold on
fplot(@(h) aproxlineal*(h-pfhnum) + mupf, rangoh,'-.' )
plot(pfhnum,0,'o')
hold off,xline(4,'m'),yline(mupf,'m')
legend("no lineal"," aprox. linealizado (Taylor)",Location="best")
ylabel("caudal salida si varia h")
xlabel('h (absoluta)')

%% Modelo incremental
% Introduzcamos \phi(\tilde h)=\mu(\tilde h+h_{pf})-\mu(h_{pf}), el incremento de caudal de salida respecto al punto de funcionamiento en función de la altura incremental, o sea \phi(\tilde h)=\sqrt{\tilde h+4}\ -\ \sqrt{4}. Verifica \phi(0)=0.

incmu=mu-mupf; %la no-linealidad "incremental", para que en h_pf valga cero:
syms inc_h
phi=subs(incmu,h,inc_h+pf.h)

% Podemos expresar el depósito como
% \frac{d\tilde h}{dt}=-\phi(\tilde h)+\tilde q
%  porque
% \frac{d\tilde h}{dt}=\frac{d h}{dt}=-(\phi(\tilde h)+\mu(h_{pf}))+\tilde q+q_{pf}=-\mu(h)+q=-\sqrt{h}+q
% Por el momento, es sólamente un cambio de variable, es una reescritura exacta, de modo que el punto de equilibrio ante \tilde q=0 es \tilde h=0.
% La linealización aproxima \phi(\tilde h)\approx \frac{1}{4}\tilde h, con lo que queda \frac{d\tilde h}{dt}\approx -0.25\cdot \tilde h+\tilde q.

rango_inc_h=rangoh-pfhnum;
fplot(phi,rango_inc_h,LineWidth=3) 
hold on
fplot(@(inc_h) aproxlineal*inc_h,rango_inc_h,'-.r'), plot(0,0,'o'),hold off
xline(0,'m'),yline(0,'m'), grid on, ylim([0 3.5]-mupf)
xlabel("h incremental"), ylabel("Caudal salida incremental")
legend("Incremento caudal \phi","Aproximación linealizada",Location="best")

%% Acotación de sector (incremental) de una no-linealidad
% Este es el objetivo principal del material, acotar el modelo incremental \phi(\tilde h) entre dos rectas que pasan por el origen, A_1\tilde h y A_2\tilde h. Las pendientes son:

A1=0.5; A2=0.191; %pendientes de las rectas, prueba y error.

% Dibujemos:

fplot(phi,rango_inc_h,'LineWidth',3), grid on %función a acotar
hold on
fplot(@(inc_h) A1*inc_h, rango_inc_h, '-.', LineWidth=2 )%cota 1
fplot(@(inc_h) A2*inc_h, rango_inc_h, '-.', LineWidth=2 )%cota 2
plot(0,0,'o')
xline(0,'m'),yline(0,'m')
fplot(@(inc_h) Amedio*inc_h, rango_inc_h, '-.' ) %BISECTRIZ
%fplot(@(inc_h) aproxlineal*inc_h, rango_inc_h, '-.' ) %linealizado (NO hace falta)
legend("\phi no lineal","límite sector A1", "límite sector A2",Location="best"), ylabel("incremento caudal respecto a pf. si varia h")
hold off
xlabel('h incremental')
title("Linealización (acotación) de sector de un depósito")

%
%% Escalado a [-1,+1] de la incertidumbre (incertidumbre "normalizada")
% El modelo "nominal" será el centro de la incertidumbre:

Amedio=(A1+A2)/2 %no es la linealización "Taylor", es la BISECTRIZ del sector

% Y ese centro (bisectriz), se desviará simétricamente hacia arriba y hacia abajo, con un incremento de inclinación (pendiente) dado por:

inc_pendiente=A1-Amedio %le llamaremos "l" en la fórmula a continuación

% Definamos \Delta(\tilde h)=\frac{\phi(\tilde h)-A_{medio}\tilde h}{\ell\cdot \tilde h}, con lo que \phi(\tilde h)=(\ell\cdot \Delta(\tilde h)+A_{medio})\cdot \tilde h.

Delta=(phi-Amedio*inc_h)/(inc_pendiente*inc_h);

% Vamos a dibujar la función \Delta(\tilde h):

fplot(Delta,rango_inc_h,LineWidth=2,Color=[1 .5 .2]), grid on, yline(-1,'-.y'),yline(+1,'-.r'), ylim([-1.1 1.1])
title("Incertidumbre escalada \Delta \in [-1,+1]")
xlabel("h incremental")

%% Conclusiones
% Para controlar alrededor de un punto de funcionamiento podemos pasar a variables "incrementales"  el modelo no-lineal.
% Siendo \phi(\tilde h):=\mu(\tilde h+h_{pf})-\mu(h_{pf})=\sqrt{h_{pf}+\tilde h}-\sqrt{h_{pf}}, podemos expresar el depósito como
% \frac{d\tilde h}{dt}=-\phi(\tilde h)+\tilde q
% Es sólamente un cambio de variable, es una reescritura exacta, de modo que el punto de equilibrio ante \tilde q=0 es \tilde h=0.
%
% Aproximaciones/acotaciones de \phi:
% Linealización Taylor:   \phi(\tilde h)\approx 0.25\tilde h
% Acotación Sector:      \phi(\tilde h)\in [0.19\tilde h, 0.5\tilde h], sin utilizar la "forma" concreta de \phi dentro de ese intervalo en control posterior (se denominarán "shape-independent solutions").
% Nota: para ser correctos formalmente, si \tilde h<0 deberían "cambiarse" los extremos del intervalo para que el más grande esté a la derecha, formalmente sería más correcto acotar 0.19\tilde h^2\leq \phi(\tilde h)\cdot h\leq 0.5\tilde h^2, pero "informalmente" se entiende lo de arriba.
% Acotación Sector normalizado (escalado a \pm 1):    \phi(\tilde h)=(0.345+0.155\Delta(\tilde h))\cdot \tilde h, siendo -1\leq \Delta(\tilde h)\leq 1. En control posterior también descartaremos la expresión concreta de \Delta(\tilde h), será shape-independent: únicamente consideraremos \Delta como un "elemento incierto de norma máxima 1", para hacer control robusto o planificación de ganancia.
%