% Kalman filtering demo

clear all
close all

% Arbejds signal oprettes

% Xa            Idielt signal
% X             Signal med støj
% var_          Fornavn for variabel der indeholder variansen
% S_hat         Estimeret signal med moving varianse estimat
% Xvf           Estimeret signal med fixed varianse
% abs_error     Fornavn for variabel der indeholder den absolutte fejl


noise_level=5.0;

for i=1:5000
Xa(i)=13.0;
X(i)=Xa(i)+(noise_level*rand(1)-(0.5));
end

for i=5000:10000
Xa(i)=50.0;
X(i)=Xa(i)+(noise_level*rand(1)-(0.5));
end

for i=10000:15000
Xa(i)=25.0*sin(i/100);
X(i)=Xa(i)+(noise_level*rand(1)-(0.5));
end
% Ardejds signal oprettet
for n=2:15000
    Xdiff(n)=X(n)-X(n-1);
end;


% initialiseringer til kalman filteret og rekursiv variance estimerende karlmanfilter
var_v=var(X);
var_xfv=var_v;
var_w=1.0;
a=1.0;

P(1)=1.0;
S_hat(1)=0.0;
for n=1:15000
    if(n>50)
        XX=var(X(n-50:n));
        var_v=XX;
    end;
    
    kr(n)=P(n)/(P(n)+var_v);
    S_hat(n+1)=a*(S_hat(n)+kr(n)*(X(n)-S_hat(n)));
    P(n+1)=a*a*(1-kr(n))*(1-kr(n))*P(n)+kr(n)*kr(n)*var_v*var_v+var_w;
end
%Slut


% initialiseringer til kalman filteret og ikke-rekursiv variance estimerende karlmanfilter
P(1)=1.0;
Xfv(1)=0.0;
for n=1:15000
    k(n)=P(n)/(P(n)+var_v);
    Xfv(n+1)=a*(Xfv(n)+k(n)*(X(n)-Xfv(n)));
    P(n+1)=a*a*(1-k(n))*P(n)+var_w;
end
%Slut


%Absolut fejl udregning
for n=1:15000
abs_error_Xa(n)=abs(Xa(n)-X(n));
abs_error_S_hat(n)=abs(Xa(n)-S_hat(n));
abs_error_Xfv(n)=abs(Xa(n)-Xfv(n));
end
%Absolut fejl udregning slut




%Presentations plot
axis manual

subplot(4,4,1);plot(Xa);axis([0 15000 -75 75]);
title('Optimal signal')

pause

subplot(4,4,2);plot(X);axis([0 15000 -75 75]);
title('X with noise')

pause

subplot(4,4,6);plot(Xfv);axis([0 15000 -75 75]);
title('kalman filter with fixed variance')

pause

subplot(4,4,10);plot(S_hat);axis([0 15000 -75 75]);
title('kalman filter with a moving calculation of the variance')

pause

subplot(4,4,3);plot(abs_error_Xa);axis([0 15000 0 15]);
title('abs error')

subplot(4,4,7);plot(abs_error_S_hat);axis([0 15000 0 15]);
title('abs error')

subplot(4,4,11);plot(abs_error_Xfv);axis([0 15000 0 15]);
title('abs error')

pause

subplot(4,4,8);plot(k);axis([0 15000 0 1]);
title('k non recursive')

subplot(4,4,12);plot(kr);axis([0 15000 0 1]);
title('k recursive')

pause

subplot(4,4,13);plot(Xdiff);axis([4990 5090 -75 75]);
title('X difference');

subplot(4,4,14);plot(S_hat);axis([4990 5090 -75 75]);
title('Estimated values');

subplot(4,4,15);plot(abs_error_S_hat);axis([4990 5090 0 15 ]);
title('abs error of estimated value');

subplot(4,4,16);plot(kr);axis([4990 5090 0 1]);
title('k recursive');

pause

mean(abs_error_S_hat)
mean(abs_error_Xfv)

close all

%Presentations plot slut