%% Eliminación de tendencias (derivas, rampas) en una señal: suavizado Rauch-Tung-Striebel
% © 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/ramprts.html
% Este código ejecutó correctamente en Matlab R2019a
% Tabla de Contenidos
%     Motivación y objetivos:
%     Generación de los datos
% Smoother (RTS)
% Conclusiones
%% Motivación y objetivos:
% Algunas señales físicas son contaminadas con una deriva, o incluso una deriva en la velocidad (rampa). Esta deriva o pendiente pueden variar en el tiempo.
% Objetivo: comprender cómo eliminar ese tipo de perturación con un filtro no causal (RTS smoother) que mejora las predicciones en un instante incorporando medidas pasadas (Kalman) y futuras (paso hacia atrás en el tiempo).
%
%% Generación de los datos

%ejecutar partes 1, 2, 3!

%
%% Smoother (RTS)

PsmoothRTS{N}=ZZ{N};%inicializamos backward smooth
xrts(:,N)=x_kalman(:,N);
la_stdK=graf_sigm_est';
la_stdRTS(:,N)=sqrt(diag(ZZ{N}));
for muestra=(N-1):-1:1
        G=ZZ{muestra}*Abig'/(PP{muestra+1});
        cosa=ZZ{muestra}-G*(PP{muestra+1}-PsmoothRTS{muestra+1})*G';
        PsmoothRTS{muestra}=cosa; 
        xrts(:,muestra)=x_kalman(:,muestra)+G*(xrts(:,muestra+1)-Abig*x_kalman(:,muestra));
        
        la_stdRTS(:,muestra)=sqrt(diag(cosa));%para gráfica
end
Y_smoothRTS=(Climpia*xrts)';
plot(Tiempos,[senyal Y_Kalman Y_smoothRTS]), grid on, legend('limpia','causal Kalman','RTS','Location','southwest')
plot(Tiempos,[senyal Y_Kalman Y_smoothRTS]), xlim([48.5,53.5]), grid on, legend('limpia','causal','RTS','Location','southwest')
plot(Tiempos,[senyal Y_Kalman Y_smoothRTS]), xlim([48.5,53.5]), grid on, legend('limpia','causal','RTS','Location','southwest')
xlim([62 74])
plot(Tiempos,([la_stdK(1,:);  la_stdRTS(1,:)])), grid on
plot(Tiempos,([la_stdK(2,:);  la_stdRTS(2,:)])), grid on
plot(Tiempos,([la_stdK(3,:);  la_stdRTS(3,:)])), grid on
errk=norm(senyal-Y_Kalman) %con filtro Kalman causal
err_rts=norm(senyal-Y_smoothRTS) %con RTS smoother no causal
errordetrend1
errordetrend3

%% Conclusiones
% Si las señales "limpias" tienen media cero y no tienen componentes de frecuencia INFERIOR a 0.1 rad/s todo esto funcionaría bien... En caso contrario, es dificil separar "deriva" de "trozo de señal con forma de escalón".