%% Eliminación de tendencias (derivas, rampas) en una señal: fusión sensorial, muestreo no convencional.
% © 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/rampNC.html
% Este código ejecutó correctamente en Matlab R2019a
% Tabla de Contenidos
%     Motivación y objetivos:
%     Generación de los datos
% 3.- Filtro de Kalman no estacionario con 2 sensores
% 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. En ciertos casos, se dispone de otras medidas sin estar afectadas de esa deriva.
% Ejemplo: en navegación inercial, la estimación de velocidad y posición integrando la aceleración tiene cada vez más incertidumbre (doble integral de ruido blanco --y eso sin contar el componente constante de gravedad que el acelerómetro también debería compensar--). Sin embargo, la ventaja de estas medidas es que pueden ser obtenidas con alta frecuencia de muestreo, y de ese modo servir para la estabilización de una aeronave, por ejemplo.
% Para navegación, existen medidas GPS de posición que no están afectadas por esa deriva/rampa, pero su ruido de medida es diferente y la frecuencia de muestreo con la que pueden ser obtenidas es menor que las de la navegación incercial. Este es un problema clásico de fusión sensorial que el filtro de Kalman (caso causal) o el RTS smoother (no causal) pueden incorporar con modificaciones muy sencillas.
%
% Objetivo: comprender cómo fusionar información de aceleración (equiv. medidas de posición obtenidas por integración de acelerómetro) a frecuencia de muestreo alta con información posicional directa obtendia a una frecuencia de muestreo más baja. Usaremos filtro de Kalman y RTS smoother con muestreo no convencional (dual-rate o aleatorio).
%
%% Generación de los datos
% *Se tiene que haber ejecutado las partes 1, 2 y 3 para que haya determinadas variables en memoria
%
%% 3.- Filtro de Kalman no estacionario con 2 sensores

xest=[0;0;0];
graf_yest=zeros(N,1); x_kalman=zeros(3,N); 
graf_sigm_est=zeros(N,3); 
graf_M=zeros(N,3);
Pk=diag([4^2 1^2 0.2^2]);%poca informacion a priori, pero algo es algo
muestraslimpias=[];
for muestra=1:N
    
     %hay dos sensores, con deriva o sin deriva:
    if(rand()<0.01)  %(mod(muestra,12*20)==0)
        Csens=[Cbig;Climpia];
        y=[ymed(muestra);senyal(muestra)];Rsens=blkdiag(R,R/64);%8x precision el limpia...
        muestraslimpias=[muestraslimpias muestra];
    else
        Csens=Cbig;y=ymed(muestra);Rsens=R;
    end
   
    
    M=Pk*Csens'/(Csens*Pk*Csens'+Rsens);
    graf_M(muestra,:)=M(:,end)';
    xest=xest+M*(y-Csens*xest);%corrector (observador)
    Zk=(eye(3)-M*Csens)*Pk;Zk=(Zk+Zk')/2;
    PP{muestra}=Pk; %priori
    ZZ{muestra}=Zk; %guardamos,  posteriori
    x_kalman(:,muestra)=xest;%tambien guardo para RTS.
    yest= Climpia*xest;
    graf_sigm_est(muestra,:)=sqrt(diag(Zk))';
    graf_yest(muestra)=yest;
    xest=Abig*xest; %ejecutamos modelo para calcular suguiente estado
    Pk=Abig*Zk*Abig'+Gbig*Q*Gbig';%varianza antes de medir para la siguiente iteración
end
muestraslimpias*Ts %instantes con muestreos sin deriva:
graf_Xest=x_kalman';
Y_Kalman=graf_Xest(:,1);

plot(Tiempos,graf_M), grid on, axis([0 30 0 1.1])
plot(Tiempos,[senyal graf_yest Y_Kalman1sens]),legend('real','2sensores','1sensor'), grid on
hold on
plot(muestraslimpias*Ts,senyal(muestraslimpias),'ok')
hold off
plot(Tiempos,[ycorregida3 Y_Kalman]), hold  on
plot(Tiempos,senyal,'g')
plot(Tiempos,[Y_Kalman+2.6*graf_sigm_est(:,1) Y_Kalman-2.6*graf_sigm_est(:,1)],'-.r'), grid on, hold off
axis([80 94 -1.6 1.6]), legend('1sens','2sens','limpia','int.c.','int.c.')
plot(Tiempos,graf_sigm_est(:,1))
plot(Tiempos,[graf_Xest(:,2) graf_Xest(:,2)+2.6*graf_sigm_est(:,2) graf_Xest(:,2)-2.6*graf_sigm_est(:,2)]), grid on
ylim([-4 6])
plot(Tiempos,[graf_Xest(:,3) graf_Xest(:,3)+2*graf_sigm_est(:,3) graf_Xest(:,3)-2*graf_sigm_est(:,3)]), grid on
ylim([-.45 .45])

%
%% 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+cosa')/2;
        la_stdRTS(:,muestra)=sqrt(diag(cosa));
        xrts(:,muestra)=x_kalman(:,muestra)+G*(xrts(:,muestra+1)-Abig*x_kalman(:,muestra));

end
Y_smoothRTS=(Climpia*xrts)';
plot(Tiempos,[senyal Y_Kalman Y_smoothRTS]), grid on, legend('limpia','causal2sens','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)
err_rts=norm(senyal-Y_smoothRTS)
errordetrend1
errordetrend3
norm(senyal-Y_Kalman1sens)

%% Conclusiones
% Esta es la señal finalmente reconstruida con suavizado no causal y fusión de 2 sensores, que en una aplicación real no podría ser comparada con la señal "original":

plot(Tiempos,Y_smoothRTS), grid on

%