% PURPOSE : 
% AUTHORS : Nando, ..           
% DATE    : June 2001

clear;
echo off;

% =======================================================================
%              INITIALISATION AND PARAMETERS
% =======================================================================

T = 100;                     % Number of time steps.

n_x = 2;                    % Continuous state dimension.
n_y = 2;                    % Dimension of observations.

par.A = [1.01 .1; -.1 .9];
par.B = 0.01*eye(n_x,n_x);
par.C = [20 5; 2 -4];
par.D = 0.1*eye(n_y,n_y);
par.F = [.01 -0.03]';
par.G = [5 0]';

mu = zeros(n_x,T);
mu_pred = zeros(n_x,T);
Sigma = zeros(n_x,n_x,T);
Sigma_pred = zeros(n_x,n_x,T);
y_pred = zeros(n_y,T);

par.mu0 = [1 0.2]';             % Initial Gaussian mean.
par.S0  = 1*eye(n_x,n_x);         % Initial Gaussian covariance.  


% =======================================================================
%                          GENERATE THE DATA
% =======================================================================

x = zeros(n_x,T);
y = zeros(n_y,T);
u = ones(1,T);           % Control signals.

x(:,1) = par.mu0;
for t=2:T,
  x(:,t) = par.A*x(:,t-1) + par.B*randn(n_x,1) + par.F*u(:,t); 
  y(:,t) = par.C*x(:,t) + par.D*randn(n_y,1) + par.G*u(:,t); 
end;

%load kalmanData;

% =======================================================================
%                              KF ESTIMATION
% =======================================================================

% INITIALISATION:
mu(:,1) = par.mu0;
Sigma(:,:,1) = par.S0;

for t=2:T
  % PREDICTION:
  mu_pred(:,t) = par.A*mu(:,t-1) + par.F*u(:,t); 
  Sigma_pred(:,:,t)=par.A*Sigma(:,:,t-1)*par.A' + par.B*par.B'; 
  S = par.C*Sigma_pred(:,:,t)*par.C' +par.D*par.D';  
  y_pred(:,t) = par.C*mu_pred(:,t) + par.G*u(:,t);

  % UPDATE:
  K = Sigma_pred(:,:,t)*par.C'*pinv(S);
  mu(:,t) = mu_pred(:,t) + K*(y(:,t)-y_pred(:,t));
  Sigma(:,:,t) = Sigma_pred(:,:,t) - K*par.C*Sigma_pred(:,:,t);   
end;

% PLOTS:
% =====

figure(1) 
clf
subplot(211)
plot(1:T,x,'r',1:T,mu,'b--','linewidth',2);
legend('True state','KF estimate');
ylabel('x_t','fontsize',15)
xlabel('t','fontsize',15)
subplot(212)
plot(2:T,y(:,2:T),'r',2:T,y_pred(:,2:T),'b--','linewidth',2);
legend('True observation','KF prediction');
ylabel('y_t','fontsize',15)
xlabel('t','fontsize',15)



zoom on;
