% Kalman filter demo
% written by Emtiyaz, CS, UBC
% last updated on Feb 25, 2008
% This program computes state estimate for 2-D vehicle with uniform speed, driven by
% white noise.
clf
%%%%%%%%%%%
% Vehicle Model
%%%%%%%%%%%
%sampling frequency
Ts = .1;
% system matrices
A = [1 Ts 0 0; 0 1 0 0; 0 0 1 Ts; 0 0 0 1];
B = [ 0 0; 0 1; 0 0; 0 1];
C = [ 1 0 0 0; 0 0 1 0];
D = eye(2,2);
% noise matrices
Q = .1*eye(2,2);
R = .5*eye(2,2);
% initial state
mu0 = [0 0 5 3]'; S0 = eye(4,4); %this defines a vehicle with average speed [5,3]
% simulation time
T = 128; 

%%%%%%%%%%%
% generate data
%%%%%%%%%%%
x0 = mu0 + sqrtm(S0)*randn(4,1);
x(:,1) = A*x0 + B*sqrtm(Q)*randn(2,1);
y(:,1) = C*x(:,1) + D*sqrtm(R)*randn(2,1);
for t = 2:T
  x(:,t) = A*x(:,t-1) + B*sqrtm(Q)*randn(2,1);
  y(:,t) = C*x(:,t) + D*sqrtm(R)*randn(2,1);
end

%%%%%%%%%%%
%plot the true state
%%%%%%%%%%%
subplot(221)
title('Position')
plot(x(1,:),x(3,:),'k','linewidth',2);
hold on
subplot(223)
title('Error Covariance x position')
axis([1 64 0 0.1])
subplot(224)
title('P_t^{11}')
axis([1 64 0 1.2])
title('Error Covariance y position')

%%%%%%%%%%%
% Kalman Filter
%%%%%%%%%%%
[xt  Pt] =KF(y,A,B,C,D,Q,R,mu0,S0);