tSim = 10;
h = 0.01;
t = 0:h:tSim;
tt = numel(t);

% przyspieszenie teoretyczne
a = square(t, 1/tSim,20) * 5 - 2;

% predkosc teoretyczna
v = zeros(1,tt);
for i = 1:(tt-1)
   v(i+1) = v(i) + h * a(i);
end

% zaszumienie wartosci teoretycznych
sigma_a = 0.5;
as = a + sigma_a * randn(1,tt);
sigma_v = 1;
vs = v + sigma_v * randn(1,tt);

% droga wynikajaca z modelu
s = zeros(1,tt);
for i = 1:(tt-1)
    s(i+1) = s(i) + h * v(i) + h^2/2 * a(i);
end

% macierz pomiarow
y = [vs;as];

% model FK
A = [1 h h^2/2;
    0 1 h;
    0 0 1];
B = 0;
H = [0 1 0;
    0 0 1];
R = [sigma_v^2 0;
    0 sigma_a^2];
q = [h h^2/2;
    1 h;
    0 1];
W = eye(2) * 0.002;

% FK
xHat = zeros(3,tt);
xPost = zeros(3,1);
Ppost = zeros(3,3);
I = eye(3);
for i = 1:tt
    [xPost,Ppost] = KalmanFilter(A,B,H,q,W,R,0,y(:,i),xPost,Ppost,h);
    xHat(:,i) = xPost;
end

% wyniki
figure;
subplot(3,1,1);
plot(t,a,'g');hold on;grid on;
plot(t,as,'r');
plot(t,xHat(3,:),'b');
legend('wzor','pomiar','estymata');
title('przyspieszenie');
subplot(3,1,2);
plot(t,v,'g');hold on;grid on;
plot(t,vs,'r');
plot(t,xHat(2,:),'b');
legend('wzor','pomiar','estymata');
title('predkosc');
subplot(3,1,3);
plot(t,s,'g');hold on;grid on;
plot(t,xHat(1,:),'b');
legend('wzor','estymata');
title('przemieszczenie');