The Best Kalman Filter Paper

This is the best paper about KF filter... at less for me.

Using the Kalman Filter to track Human Interactive Motion-Modelling and Initialization of the Kalman Filter for Translation Motion

and This is Octave (Matlab) code from the paper. Input data is position of the object in (x, y)

function [raw, filter] = kf2DHumanMotion(data)
%printf('Loading file: %s\n', data);
input = load(data);

%measurement data
Zx = input(:, 1)';
Zy = input(:, 2)';

raw(1,:) = Zx;
raw(2,:) = Zy;

%update time
dt = 0.1;

%max acceleration
aMax = 5000.0 * dt;

%max speed
vMax = 5000.0 * dt;

%identity matrices
I = eye(2);

%xminus_k = Ax_k + w_k
A = [1.0 0.0 dt 0.0;
0.0 1.0 0.0 dt;
0.0 0.0 1.0 0.0;
0.0 0.0 0.0 1.0];

%z_k = Hx_k + v_k
H = [1.0 0.0 0.0 0.0;
0.0 1.0 0.0 0.0;
0.0 0.0 0.0 0.0;
0.0 0.0 0.0 0.0];

%measurement covariance
covx = 500.0;
covy = 500.0;

%covariance for velocity are added to prevent invert matrice problem
R = [covx 0.0 0.0 0.0;
0.0 covy 0.0 0.0;
0.0 0.0 1.0 0.0;
0.0 0.0 0.0 1.0];

%process covariance
Q = (((aMax * aMax) * dt)/6.0) * [2*I*(dt*dt) 3*I*dt; 3*I*dt 6*I];

dsMax = vMax * dt;
s2 = dsMax*dsMax;
v2 = vMax*vMax;

P_0 = (1/16) * [s2 0.0 0.0 0.0;
0.0 s2 0.0 0.0;
0.0 0.0 v2 0.0;
0.0 0.0 0.0 v2];

%intial value
x_0 = [Zx(1); Zy(1); 0.0; 0.0];
x_k = zeros(4,1);
P_k = zeros(4);
Pminus_k = zeros(4);
K_k = zeros(4);
z_k = [0.0; 0.0; 0.0; 0.0];

filter(1,1) = x_0(1);
filter(2,1) = x_0(2);



count = 0;

for i = 2:size(Zx, 2)
%printf("step %d\n", i - 1);

%if not detect
if (Zx(i) == 0.0) && (Zy(i) == 0.0)
%printf("zero input\n");
%predict only or use old data
if(count < 3)
x_k = A*x_0;
Pminus_k = A*P_0*A' + Q;
x_0 = x_k;
P_0 = Pminus_k;
end
count = count+1;

else
count = 0;
%printf("input (%6.3f, %6.3f)\n", Zx(i), Zy(i));

%Time update
x_k = A*x_0;
Pminus_k = A*P_0*A' + Q;

%measurement update
K_k = (Pminus_k * H') * inv((H*Pminus_k*H') + R);
z_k = [Zx(i); Zy(i); 0; 0];
x_0 = x_k + (K_k * (z_k - (H*x_k)));
P_0 = (eye(4) - K_k*H)*Pminus_k;

end
filter(1,i) = x_0(1);
filter(2,i) = x_0(2);
%printf("------------------------------------------------\n");
end
end