clear all
npts = 2000;
del = 1/2000;
t = 0:del:1;
inp = (1+.2 .* t + .1 .*t.^2) + cos(2. * pi * 2.5 .* t);
X0 = [1,.1,.01]';
% it is assumed that the measurement vector H=[1,0,0]
% this is the update interval in seconds
T = 1.;
% enter the mesurement noise variance
R = .035;
% this is the state noise variance
nvar = .5;
[residual, estimate] = kalman_filter(npts, T, X0, inp, R, nvar);
figure(1)
plot(residual,'k')
xlabel ('Sample number')
ylabel ('Residual')
figure(2)
subplot(2,1,1)
plot(inp,'k')
axis tight
ylabel ('position - truth')
subplot(2,1,2)
plot(estimate,'k')
axis tight
xlabel ('Sample number')
ylabel ('Predicted position')