%% Ganancia / Condicionamiento numérico excesivamente altos: influencia en la precisión de los resultados de control (ejemplo Matlab)
% © 2021, 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/condnml.html
% Este código funcionó correctamente en Matlab R2020b
% Objetivo: ilustrar los inconvenientes de ganancias (escaladas) excesivamente altas en un problema de control multivariable sencillo (bucle abierto), tanto si es excesiva "en todas direcciones" como si la ganancia es excesiva en "algunas direcciones" y pequeña en "otras" (mal condicionamiento numérico).
% Tabla de Contenidos
% Modelado y objetivos de control
%     Casos a estudiar
% Análisis 1: cómo funcionan las cosas "en teoría"
% Análisis 2: cómo de mal podría funcionar esto en la realidad
%     2.1- Sin error de modelado, pero con ruido en entradas
%     2.2- Además, con error de modelado
% Conclusiones
%% Modelado y objetivos de control
% Sea un modelo y_{2\times 1}=G_{2\times 3}u_{3\times 1}+p_{2\times 1}, donde suponemos todo "escalado" (adimensionalizado) de modo que \|p\|\leq 1 y deseamos \|u\|\leq 1.
% Queremos cancelar la perturbación ( y\approx 0 )... si fuera medible, cancelaríamos con con u=-G^{\dagger}p, y conseguiríamos y=0.
% No obstante, existen errores de modelado,  G_{real}=G+\Delta_G, las medidas de perturbación no son exactas p_{medida}=p_{real}+\delta_{m}, y existe imprecisión (ruido, descalibración) en el posicionamiento de los actuadores, u_{real}=u+\delta_u.
% Con lo cual, calcularemos u=-G^\dagger p_{medida}, pero el resultado será y_{real}=G_{real}u_{real}+p_{real}.
% El objetivo es conseguir y_{real} pequeño, al menos pequeño comparado con la opción de no hacer nada u_{real}=0, y_{real}=p_{real}, se debería conseguir \frac{\|y_{real}\|}{\|p_{real}\|}<<1.
%% Casos a estudiar
% Revisaremos tres casos, viendo la sensibilidad a ruidos \delta_u y a errores de modelado \Delta_G (no consideramos \delta_m).

G1=[1 -0.5 1; ...
    0  1  1]; %bien condicionado, ganancias suficientes pero no excesivas
Greal1=[1.02 -0.49 1.03; ...
    0.01  1 1.03]; %para probar error de modelado

G2=[8  15 12; ...
    15 -12 10]; %bien condicionado, pero ganancias excesivamente altas
Greal2=[8.3 15 12.3;...
    15.4 -12 10.4];%para probar error de modelado

G3=[4  6  8; ...
    10 12 15]; %mal condicionado.
Greal3=[4.7 6 7.5;9.6 12 15.2]; %para probar error de modelado

caso=3;
switch caso
    case 1
        G=G1; %elegimos caso
        Greal=Greal1;
    case 2
        G=G2; %elegimos caso
        Greal=Greal2;
    case 3
        G=G3; %elegimos caso
        Greal=Greal3;
end
norm(Greal-G,'fro')/norm(G,'fro') 
%hay otras formas mejores de describir error... 
% esta es "media", pero no "worst-case"
[U,S,V]=svd(G,'econ')
cond(G) % es S(1,1) entre S(2,2)

% Vamos a hacer, por ejemplo, la maniobra "difícil" (perturbación tiene dirección de "baja ganancia"):

pert1=U(:,2)

%% Análisis 1: cómo funcionan las cosas "en teoría"
% Supondremos que la perturbación es medible, podemos hacer prealimentación.

u1=-pinv(G)*pert1

% La "simulación" (sin \delta_u ni \Delta_G) diría que se cancela la perturbación perfectamente.

errideal=G*u1+pert1
percenterrorsalida=norm(errideal)/norm(pert1)*100;
fprintf("Error residual %g%% amplitud perturb.",percenterrorsalida)

%% Análisis 2: cómo de mal podría funcionar esto en la realidad
%% 2.1- Sin error de modelado, pero con ruido en entradas

u1=-pinv(G)*pert1

% Cometo un error pequeño posicionando las entradas:

u1real=u1-0.03*V(:,1)
percenterrorentrada=norm(u1-u1real)/norm(u1)

% Entonces obtenemos:

err=G*u1real+pert1
norm(err)/norm(pert1)

%% 2.2- Además, con error de modelado
% Consideremos errores de modelado G_{real}\neq G, con lo que G_{real}\cdot G^{\dagger}\neq I:

err2=pert1+Greal*u1real
nn=norm(err2)/norm(pert1)
plot1pt=@(pt) plot(pt(1),pt(2),'x','LineWidth',2);
plot1pt(pert1)
hold on
plot(0,0,'ok','LineWidth',2);
plot1pt(err)
plot1pt(err2)
fimplicit(@(x,y) x.^2+y.^2-1,'-.')
fimplicit(@(x,y) x.^2+y.^2-nn^2,':')
hold off, grid on,xline(0),yline(0), axis equal, axis([-1.5 1.5 -1.5 1.5])
legend("pert. a cancelar","ideal", "error (con 3% ruido fondo esc.)","error (con ruido u y err. model)")

%% Conclusiones
% CASO 1: si la ganancia mínima está "cercana a 1" y el condicionamiento es "menor a 5", todo funciona razonablemente como se espera... un 4% de error en el posicionamiento del actuador, 2% de error modelado (en norma Frobenius) acaba traducido a un \approx6% de error en la cancelación de la perturbación.
% CASOS 2 y 3: Ganancias "excesivamente altas" (escaladas \pm1) no son buenas; en concreto:
% \bullet [CASO 2] Si son "excesivamente altas en todas direcciones", entonces las acciones de control son muy pequeñas y los errores en su posicionamiento hacen que la relación señal/ruido sea mala: un error \delta_u del 3% a fondo de escala acaba siendo un error del 60% en inputs con el pequeño valor de entrada necesario (caso 2), y de un 70% después de todos los errores de modelado, etc. en salida. Aparte, exceso de potencia indica que podría reducirse globalmente la potencia de los actuadores, posiblemente implicando ahorro económico.
% \bullet [CASO 3] Si son "excesivamente altas en unas direcciones y otras no", entonces amplifican los errores por el condicionamiento... un error relativo del 3% en \delta_u (entrada) y 4% en \Delta_G (modelado) acaba siendo un error del 122% en este ejemplo.
% En plantas mal condicionadas, cancelar perturbaciones en la dirección "difícil" es muy sensible a error en sensores o posicionamiento de actuadores, y la cosa es todavía peor teniendo en cuenta error de modelado si tenemos "mala suerte".
% Mejor rediseñar el proceso que intentar controlar en los casos 2 y 3, seguramente: todo simulará en Matlab, pero en la práctica hay alta probabilidad de que no funcione.