%% Ejemplo filtro de Kalman para estimar temperatura del gas que circula por una tubería
% © 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/tubokal.html
% Este código ejecutó sin errores en Matlab R2019a
%% Modelado
%
% Suponiendo T_g (temperatura gas) y T_a (temperatura ambiente) como entradas, tendríamos unas constantes de transmisión de calor y capacidades caloríficas que darían lugar a la ecuación de estado:
% \frac{d}{dt}\left(\begin{array}{c}T_s \\ T_t \end{array}\right)=
% \left(\begin{array}{cc}(-k_{st}-k_{sa})/C_s &k_{st}/C_s \\ k_{st}/C_t &(-k_{st}-k_{ta}-k_{tg})/C_t \end{array}\right)
% \left(\begin{array}{c}T_s \\ T_t \end{array}\right)
% +
% \left(\begin{array}{cc}0 &k_{sa} /C_s\\ k_{tg}/C_t &k_{ta}/C_t\end{array}\right)
% \left(\begin{array}{c}T_g \\ T_a \end{array}\right)
% Nota: este es un modelo simplificado de parámetros concentrados; la transmisión de calor en una tubería se rige por ecuaciones en derivadas parciales fuera de los objetivos de este modelado. Supondremos, por tanto, que la tubería es "corta" y que las pérdidas de entalpía del gas (calor transmitido a la tubería) son despreciables.
% Vamos a dar valores numéricos

kst=.3; ktg=.25; kta=.08; ksa=0.01; Cs=.4; Ct=1.5;
A=diag([1/Cs 1/Ct])*[-kst-ksa kst; kst -ktg-kta-kst];
B=diag([1/Cs 1/Ct])*[0 ksa; ktg kta];
sys=ss(A,B,eye(2),0);
sys.InputName={'Tgas','Tambiente'};
sys.OutputName={'Tsonda','Ttuberia'};

%
%% Incorporación de modelo de perturbaciones

anchobandagas=0.16; s=tf('s');
GeneradorTgas=1/(s+anchobandagas)^2; %multipl. por ruido blanco...

% Por simplicidad, descartemos variaciones de Tamb...

sys2=ss([sys(:,1);1]*GeneradorTgas);
sys2.OutputName{3}='Tgas';

% Discretizamos:

Ts=0.2; sysd=c2d(sys2,Ts,'zoh'); 
%existe una forma teóricamente mejor de discretizar ruidos continuos,
%omitida por brevedad

%% Análisis de propiedades

Eba=eig(sysd.a)' %polos bucle abierto
Q=300; %varianza ruido de proceso
Zba=gram(sysd,'c')*Q %vzas-covarianzas estado en bucle abierto:
VzaSalidas=sysd.c*Zba*sysd.c'
dt_salidas=sqrt(diag(VzaSalidas))'
dt_Tgas=dt_salidas(3) %desviación. tip. temperatura gas a priori (sin sensores)

%% Simulación (bucle abierto)

N=1800;Tiempo=(0:(N-1))*Ts;
perturbacionRuidoBlanco=randn(1,N)*sqrt(Q);
Y=lsim(sysd,perturbacionRuidoBlanco);
plot(Tiempo,Y), grid on, legend('Temp. Sensor','Temp. Tubo','Temp. Gas','Location','best'), axis tight

%
%% Filtro de Kalman estacionario
% Con un sensor "horrible (vza 1e9)" coincide con la estimación del estado en bucle abierto

Csensor=sysd.c(1,:);  %medimos la salida 1 (sonda)
[Mhorrible,~,Zhorrible,Ehorrible]=dlqe(sysd.a,sysd.b,Csensor,Q,1e9);
Mhorrible'
Zhorrible
Zba %esto era lo que obtenía el gramiano
Ehorrible'
fliplr(Eba)

% con un sensor "perfecto" tendería a la estimación tiempo mínimo (polos muy rápidos)

[Mperfecto,~,Zperfecto,Eperfecto]=dlqe(sysd.a,sysd.b,Csensor,Q,4e-11);
Mperfecto %ganancia "enorme"

% La varianza del estado estimado con ese sensor perfecto:

Zperfecto %pero al ser de orden>1, el ruido de proceso no puede eliminarse

% La varianza del las tres salidas (sonda,tubo,gas) estimadas (medidas sólo es la primera) con ese sensor ideal:

VzaErrorEstimSalidasPerfecto=sysd.c*Zperfecto*sysd.c'

% Esta sería la desviación típica de las salidas estimadas (sonda,tubo,gas) en grados:

dt_perfecto=sqrt(diag(VzaErrorEstimSalidasPerfecto))' 
Eperfecto' %tres polos rápidos, 1 más lento

% Diseñemos un observador en bucle cerrado con un sensor "normal", ni muy bueno ni muy malo:

desvtipsensor=0.6;
R=desvtipsensor^2;%desv. típica ruido medida sensor al cuadrado
[M,~,Z,E]=dlqe(sysd.a,sysd.b,Csensor,Q,R); 
M %ganancia observador
Z %vza estado estimado
sysd.c*Z*sysd.c' %matriz vzas-cov salidas estimadas (sonda,tubo,gas)
E

% el comando kalman da el mismo resultado, y forma el observador:

[Observador,~,~,Mx,Z,~]=kalman(sysd(1,:),Q,R); %elegimos la primera salida para kalman
eig(Observador) %polos del observador
Mx %ganancia
Z %vza estado interno
VzaSalidasKalm=sysd.c*Z*sysd.c' %vza salidas
dt_Kalm=sqrt(diag(VzaSalidasKalm))' %de sonda, tubo, gas
dt_estimgas_consensor=dt_Kalm(3) %con ese sensor, esta es la d.t. del estimado
dt_estimgas_consensor/dt_Tgas %mejoramos 6 veces la precisión respecto a b.a.

%% Simulación del filtro de Kalman

size(Observador)
Observador.OutputName'
ObservadorEstado=Observador(2:end,:);
ObservadorTemperaturas=sysd.c*ObservadorEstado;
ObservadorTemperaturas.OutputName={'Tsonda_e','Ttubo_e','Tgas_e'};
%añadimos a la simulación de Tsonda el  ruido de medida
TgasEstim=lsim(ObservadorTemperaturas(3),Y(:,1)+randn(size(Tiempo'))*desvtipsensor);
plot(Tiempo,[Y(:,1) Y(:,3) TgasEstim]), grid on, legend('Sensor','T.Gas real','T gas estimada')
plot(Tiempo,Y(:,3)-TgasEstim), grid on, title('Error de estimación')
yline(-dt_estimgas_consensor*2);yline(dt_estimgas_consensor*2);

%
%