
% KALMAN FILTER SIMULATION
close all; 

% Variable we want to find, we assume it is constant.
x = -0.37727;

% Number of iterations or timestep we run to find the value of x
Nsteps = 100;

% Generate Nsteps measurements with noise variance sigma according to z = x + N(0,sigma)
sigma = 0.25;
z = normrnd(x,sigma,1,Nsteps);

% Measurement variance - how sure are we of our measurements.
R = (1E-1)^2;

% A - Previous state dependency
A = 1;
% B - No control input.
B = 0; 
% Q - process noise covariance
Q = 1E-5;
% w - Process noise.
w = normrnd(0,Q,1,Nsteps);
% H - z is just corrupted by white noise
H = 1;

% Error covariance estimates of P
tP = zeros(1,Nsteps);
% Estimates of x 
tX = zeros(1,Nsteps);

% Set initial guess of x to be 0
tX(1) = 0;

% Set initial guess of P to be 0
tP(1) = 1;


% Do iterations 
for t = 2:Nsteps
  
  % A priori estimates
  tXNew = A * tX(t-1) + w(t-1);
  tPNew = tP(t-1) + Q;
  
  % Posteriori estimates
  K = tPNew/(tPNew+R);
  tX(t) = tXNew + K * (z(t)-tXNew);  
  tP(t) = (1-K) * tPNew;
  
end 


% Plot 
time = 1:Nsteps;

figure
plot(time,x.*ones(1,Nsteps)), hold on
plot(time,z,'or'), hold on
plot(time,tX,'-x'), hold on
legend('True x value','z - Noisy version of x','Kalman estimate')


figure
plot(time,tP,'-x')
title('Estimation error variance as a function of time')






















