
%Kalman Filter with outlier rejection


%function [x,K,P,M_D]= Kalman_Filter(P0, X, Rww, Rvv,MD_act,M_D_val)

%                       Returns:
%                      -x= Filtered Signal
%                      -K=Kalman Gain
%                      -P=Covariance
%                      -M_D=Mahalanbis Distance value

%                       Inputs:
%                      -P0=Initial Covariance (usually high)
%                      -X=Signal
%                      -Rww=Process Noise Covariance
%                      -Rvv=Measurement Noise Covariance
%                      -MD_act= 1-Activaded
%                                0-Desactivaded 
%                      -M_D_val= The treshold distance, if MD_act=1. 
%                       Otherwise, use 0.

function [x,K,P,M_D]= Kalman_Filter(P0, X, Rww, Rvv,MD_act,M_D_val)


T=length(X);
x=zeros(1,T);
%Transition Matrix 
A = [1];
%Measurement Matrix
C = [1];

%Initialization
x0 = 0;
X(1)=x0;
P = zeros(1,1,T);
P(1)=P0;




    for i=2:T
    
        %prediction
        x(i) = A*x(i-1);
        P(:,:,i) = A*P(:,:,i-1)*A' + Rww;
        
        %update
        e(:,i) = X(i)-C*x(i);
        Ree(:,:,i) = C*P(:,:,i)*C' + Rvv;
        %Mahalanobis Distance Calculation
        M_D(i)=sqrt(e(:,i)^2)/Ree(:,:,i);
        
        %1-Use Mahalanobis Distance, 0-Not use Mahalanobis Distance
        if MD_act==1
          if(M_D(i)>=M_D_val)  
              x(i)=x(i-1);
              P(:,:,i)= P(:,:,i-1);
          else
              K = P(:,:,i)*C'*(Ree(:,:,i)^-1);
              x(:,i) = x(:,i) + K*e(:,i);
              P(:,:,i) = (1-K*C)*P(:,:,i);
          end
        else
          K = P(:,:,i)*C'*(Ree(:,:,i)^-1);
          x(:,i) = x(:,i) + K*e(:,i);
          P(:,:,i) = (1-K*C)*P(:,:,i);          
        end    
    end
end

