This is the best paper about KF filter… at less for me.
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
