one variable kalman filter test

kftest.m

% x : state vector [pos;speed]
% X : estimate vector
% n : noise vector - Q : cov.matrix
% z : measurement vector 
% v : measurement noise - R : cov.matrix
% 
% F_x : transition matrix
% F_n: noise matrix
% H : measurement matrix

x=[3 2]';
X=[0 0]';
dt=0.5;

F_x=[1,1;0,1];
F_n=diag([1,1]);
H = ones(2);

Q=diag([0.1,0.1]);
R=diag([1,1]);
P=diag([0.01,0.01]);

% perturbation levels
q = sqrt(diag(Q)) / 2;
r = sqrt(diag(R)) / 2;

tt=0:dt:10;

for t = tt
    n = q.*randn(2,1);
    v = r.*randn(2,1);
    x = F_x*x + F_n*n + [0;sin(t)]
    z = H*x + v;
    
    X = F_x*X;
    P = F_x*P*F_x' + F_n*Q*F_n';
    
    K = (P*H'*(H*P*H' + R)^-1);
    P = P -K*H*P
    X = X + K*(z - H*X);
    
    
    plot(X(1),3,'r*');
    axis([0 130 2 4])
    hold on;
    plot(x(1),3,'bx');
    hold off;
    pause(0.5)
    X(1)- x(1)
    drawnow
end