twodim_rand.m

For estimation and prediction of random 2 dimensional motion using Kalman filter in MATLAB

% Generate a random path
npts = 200;
vx0=1;
vy0=1;
px0=0;
py0=0;
dt = 0.1;
% Random acceleration at each step
ax = 20*randn(1,npts);
ay = 20*randn(1,npts);
% Construct the velocity and position values.
vx = zeros(1,npts);
vy = zeros(1,npts);
for k=1:npts
    vx(k)=vx0+ax(k)*dt; vx0=vx(k);
    vy(k)=vy0+ay(k)*dt; vy0=vy(k);
    px(k)=px0+vx(k)*dt; px0=px(k);
    py(k)=py0+vy(k)*dt; py0=py(k);
end

% Open a figure and set up the axes by recording an invisible path.
figure(5);
plot(px,py,'w')

% Set up the system model

% Measurement noise
sigma_meas=1;

% Predictor transformation x(k+1)=A*x(k).
A = [1 0 dt 0;
    0 1 0 dt;
    0 0 1 0;
    0 0 0 1];
% Observation Matrix. Observes only position information
H = [1 0 0 0;0 1 0 0];
% Estimate of error covariance matrix
P = eye(4)*0.01;
% Estimate of input noise covariance matrix. None is assumed.
Q = zeros(4);
% Estimate of measurement noise covariance.
R = eye(2)*0.1;

% Set up the prediction-correction cycle. xm is the estimate of the current
% state produced on the "last" prediction cycle. x and z are records of the
% state and measurement results for later plotting.
xm = [0;0;0;0];
z = zeros(2,npts);
x = zeros(4,npts+1);
hold on;
for k=1:npts;
    % Observation
    z(:,k) = [px(k);py(k)]+randn(2,1)*sigma_meas;
    % Update
    P1 = A*P*A'+Q;
    K=P1*H'*inv(H*P*H'+R);
    P = (eye(4)-K*H)*P1;
    x(:,k+1)=xm+K*(z(:,k)-H*xm);

    % Estimation of velocity by position difference
    x(3,k+1)=(x(1,k+1)-x(1,k))/dt;
    x(4,k+1)=(x(2,k+1)-x(2,k))/dt;

    % New prediction
    xm=A*x(:,k+1);
    % Plot the current points.
    plot(px(k),py(k),'-o',x(1,k),x(2,k),'-r*')
    drawnow
    pause(0.05)
end
hold off
title('2D Position','fonts',18)
xlabel('X position','fonts',18)
ylabel('Y Position','fonts',18)

figure(2)
subplot(2,1,1)
plot((0:npts),x(3,:),'-r',(1:npts),vx,'-')
title('Horizontal Velocity')
legend('Tracked','True')
legend('location','NorthEast')
subplot(2,1,2)
plot((0:npts),x(4,:),'-r',(1:npts),vy,'-')
title('Vertical Velocity')
legend('Tracked','True')
legend('location','NorthEast')
xlabel('Time Steps','fonts',18)