% 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')