%Kalman Filter with outlier rejection

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

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

%                       Inputs:
%                      -A=Transition Matrix
%                      -C=Measurement Matrix
%                      -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]= m_KF(A,C,P0,X,Rww,Rvv,MD_act,M_D_val)


%Initialization
T=length(X);
M_D=zeros;
DIM_A_s=size(A,1);
x0 = zeros(DIM_A_s,1);
x = zeros(DIM_A_s,T);
x(:,1)=x0;
P = zeros(DIM_A_s,DIM_A_s,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) = (eye(2)-K*C)*P(:,:,i);
            end
          else
            K = P(:,:,i)*C'*(Ree(:,:,i)^-1);
            x(:,i) = x(:,i) + K*e(:,i);
            P(:,:,i) = (eye(DIM_A_s)-K*C)*P(:,:,i);
          end
    end