%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% S-114.4202 Research Seminar on Computational Science
% Bayesian Estimation of Time-Varying Processes (5 p) L V
%
% $Id: kf_ex.m,v 1.1 2009/03/30 17:00:18 ssarkka Exp $
%
% This software is distributed under the GNU General Public 
% Licence (version 2 or later); please refer to the file 
% Licence.txt, included with the software, for details.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %
  % Generate data
  %
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  randn('state',123);
  steps = 100;

  w = 0.5;
  q = 0.01;
  r = 0.1;

  %% This is the transition matrix
  A = [cos(w)    sin(w)/w; 
       -w*sin(w) cos(w)];

  %% This is the process noise covariance
  Q = [0.5*q*(w-cos(w)*sin(w))/w^3 0.5*q*sin(w)^2/w^2;
       0.5*q*sin(w)^2/w^2          0.5*q*(w+cos(w)*sin(w))/w];

  % This is the true initial value
  x0 = [0;0.1]; 

  X = zeros(2,steps);  % The true signal
  Y = zeros(1,steps);  % Measurements
  T = 1:steps;         % Time
  x = x0;
  for k=1:steps
    x = gauss_rnd(A*x,Q);
    y = gauss_rnd(x(1),r);
    X(:,k) = x;
    Y(:,k) = y;
  end

  clf;
  subplot(1,1,1);
  plot(T,X(1,:),'--',T,Y,'o');
  legend('True signal','Measurements');
  fprintf('This is the simulated data. Press enter.\n');
  pause;
  
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %
  % Base line solution. The estimates
  % of x_k are stored as columns of
  % the matrix EST1.
  %
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
  m1 = x0;  % Initialize to true value
  EST1 = zeros(2,steps);
  for k=1:steps
    m1(2) = Y(k)-m1(1);
    m1(1) = Y(k);

    EST1(:,k) = m1;
  end

  %% Plot the signal and its estimate
  clf;
  subplot(2,1,1);
  plot(T,X(1,:),'--',T,EST1(1,:),'-',T,Y,'o');
  legend('True signal','Estimated signal','Measurements');
  
  %% Plot the derivative and its estimate
  subplot(2,1,2);
  plot(T,X(2,:),'--',T,EST1(2,:),'-');
  legend('True derivative','Estimated derivative');

  %% Compute error
  err1 = rmse(X,EST1)
  
  fprintf('This is the base line estimate. Press enter.\n');
  pause;
  
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %
  % Kalman filter solution. The estimates
  % of x_k are stored as columns of
  % the matrix EST2.
  %
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  m2 = x0;     % Initialize to true value
  P2 = eye(2); % Some uncertanty in covariance  
  EST2 = zeros(2,steps);

  for k=1:steps
    %% Replace these with the Kalman filter equations
    m2 = m2;
    P2 = P2;
    
    %% Store the results
    EST2(:,k) = m2;
  end

  %% Plot the signal and its estimate
  clf;
  subplot(2,1,1);
  plot(T,X(1,:),'--',T,EST2(1,:),'-',T,Y,'o');
  legend('True signal','Estimated signal','Measurements');
  
  %% Plot the derivative and its estimate
  subplot(2,1,2);
  plot(T,X(2,:),'--',T,EST2(2,:),'-');
  legend('True derivative','Estimated derivative');

  %% Compute error
  err2 = rmse(X,EST2)

  fprintf('This will be the KF estimate. Press enter.\n');
  pause;


  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %
  % Stationary Kalman filter solution.
  % The estimates of x_k are stored as columns of
  % the matrix EST3.
  %
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  m3 = x0;     % Initialize to true value
  K  = [0;0];  % Store the stationary gain here
  
  EST3 = zeros(2,steps);

  for k=1:steps
    %% Replace these with the stationary Kalman filter equations
    m3 = m3;
    
    %% Store the results
    EST3(:,k) = m3;
  end

  %% Plot the signal and its estimate
  clf;
  subplot(2,1,1);
  plot(T,X(1,:),'--',T,EST3(1,:),'-',T,Y,'o');
  legend('True signal','Estimated signal','Measurements');
  
  %% Plot the derivative and its estimate
  subplot(2,1,2);
  plot(T,X(2,:),'--',T,EST3(2,:),'-');
  legend('True derivative','Estimated derivative');

  %% Compute error
  err3 = rmse(X,EST3)

  fprintf('This will be the SKF estimate. Press enter.\n');
  pause;


