% DEMSE2  Demonstrate state estimation on a simple scalar nonlinear (time variant) problem
%
%   See also
%   GSSM_N1

%   Copyright  (c) Rudolph van der Merwe (2002)
%
%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
%   academic use only (see included license file) and can be obtained by contacting
%   rvdmerwe@ece.ogi.edu.  Businesses wishing to obtain a copy of the software should
%   contact ericwan@ece.ogi.edu for commercial licensing information.
%
%   See LICENSE (which should be part of the main toolkit distribution) for more
%   detail.

%=============================================================================================

clc;
clear all;

fprintf('\nDEMSE2 : This demonstration shows how the ReBEL toolkit is used for simple state estimation\n');
fprintf('         on a scalar nonlinear problem.\n\n');


%--- General setup

addrelpath('../gssm');         % add relative search path to example GSSM files to MATLABPATH
addrelpath('../data');         % add relative search path to example data files to MATLABPATH

%--- Ask the user which inference algorithm to use

ftype = input('Type of estimator [ ekf, ukf, cdkf, srukf, srcdkf, pf, gspf, gmsppf or sppf ] ? ','s');

if ~stringmatch(ftype,{'ekf','cdkf','ukf','srukf','srcdkf','pf','gspf','sppf','gmsppf'})
    error('That estimator/filter type is not recognized.');
end

number_of_runs = input('Number of independent runs ? ');


%--- Initialise GSSM model from external system description script.

model = gssm_n1('init');

Arg.type = 'state';                                  % inference type (state estimation)
Arg.tag = 'State estimation for GSSM_N1 system.';    % arbitrary ID tag
Arg.model = model;                                   % GSSM data structure of external system
Arg.algorithm = ftype;                               % set inference algorithm to be used

InfDS = geninfds(Arg);                               % Create inference data structure and
[pNoise, oNoise, InfDS] = gensysnoiseds(InfDS,ftype);       % generate process and observation noise sources


%--- Loop over number of independent runs

for k=1:number_of_runs

  randn('state',sum(100*clock));          % stir the pot... shuffle the deck :-)
  rand('state',sum(100*clock));

  %--- Generate some data

  N  = 60;                                                % number of datapoints
  X  = zeros(model.statedim,N);                           % state data buffer
  y  = zeros(model.obsdim,N);                             % observation data buffer

  pnoise = feval(model.pNoise.sample, model.pNoise, N);   % generate process noise
  onoise = feval(model.oNoise.sample, model.oNoise, N);   % generate observation noise

  X(1) = 1;                                               % initial state
  y(1) = feval(model.hfun, model, X(1), onoise(1), 1);    % observation of initial state
  for j=2:N,
    X(j) = feval(model.ffun, model, X(:,j-1), pnoise(j-1), j-1);
    y(j) = feval(model.hfun, model, X(:,j), onoise(j), j);
  end

  U1 = [0:N-1];
  U2 = [1:N];

  %--- Setup runtime buffers

  Xh = zeros(InfDS.statedim,N);          % state estimation buffer
  Xh(:,1) = 1;                           % initial estimate of state E[X(0)]
  Px = 3/4*eye(InfDS.statedim);          % initial state covariance

  %--- Call inference algorithm / estimator

  switch ftype


    %------------------- Extended Kalman Filter ------------------------------------
    case 'ekf'

        [Xh, Px] = ekf(Xh(:,1), Px, pNoise, oNoise, y, U1, U2, InfDS);


    %------------------- Unscented Kalman Filter -----------------------------------
    case 'ukf'

        alpha = 1;         % scale factor (UKF parameter)
        beta  = 2;         % optimal setting for Gaussian priors (UKF parameter)
        kappa = 0;         % optimal for state dimension=2 (UKF parameter)

        InfDS.spkfParams = [alpha beta kappa];

        [Xh, Px] = ukf(Xh(:,1), Px, pNoise, oNoise, y, U1, U2, InfDS);


    %------------------- Central Difference Kalman Filter ---------------------------
    case 'cdkf'

        InfDS.spkfParams = sqrt(3);    % scale factor (CDKF parameter h)

        [Xh, Px] = cdkf(Xh(:,1), Px, pNoise, oNoise, y, U1, U2, InfDS);


    %------------------- Square Root Unscented Kalman Filter ------------------------
    case 'srukf'

        alpha = 1;         % scale factor (UKF parameter)
        beta  = 2;         % optimal setting for Gaussian priors (UKF parameter)
        kappa = 0;         % optimal for state dimension=2 (UKF parameter)

        Sx = chol(Px)';

        InfDS.spkfParams = [alpha beta kappa];

        [Xh, Sx] = srukf(Xh(:,1), Sx, pNoise, oNoise, y, U1, U2, InfDS);


    %------------------- Square Root Central Difference Kalman Filter ---------------
    case 'srcdkf'

        InfDS.spkfParams  = sqrt(3);    % scale factor (CDKF parameter h)

        Sx = chol(Px)';

        [Xh, Sx] = srcdkf(Xh(:,1), Sx, pNoise, oNoise, y, U1, U2, InfDS);


    %------------------- Generic Particle Filter (a.k.a Bootstrap-filter of CONDENSATION -----------
    case 'pf'

        M = 200;                             % number of particles
        ParticleFiltDS.N = M;
        ParticleFiltDS.particles = randn(InfDS.statedim,M)+cvecrep(Xh(:,1),M);  % initialize particles
        ParticleFiltDS.weights = cvecrep(1/M,M); % initialize weights

        InfDS.resampleThreshold = 0.5;    % set resample threshold
        InfDS.estimateType = 'mean';      % estimate type for Xh

        [Xh, ParticleFiltDS] = pf(ParticleFiltDS, pNoise, oNoise, y, U1, U2, InfDS);

    %------------------- Gaussian-Sum Particle Filter ---------------------------------------------
    case 'gspf'

        M = 200;                             % number of particles
        ParticleFiltDS.N = M;

        initialParticles = randn(InfDS.statedim,M)+cvecrep(Xh(:,1),M);  % initialize particles

        ParticleFiltDS.stateGMM = gmmfit(initialParticles, 2, [0.001 10], 'sqrt');  % fit a 3 component GMM to initial state distribution

        InfDS.estimateType = 'mean';      % estimate type for Xh

        [Xh, ParticleFiltDS] = gspf(ParticleFiltDS, pNoise, oNoise, y, U1, U2, InfDS);

    %------------------- Sigma-Point Bayes Filter ---------------------------------------------
    case 'gmsppf'

      M = 200;
      ParticleFiltDS.N = M;            % number of particles

      initialParticles = randn(InfDS.statedim,M)+cvecrep(Xh(:,1),M);  % initialize particles

      tempCov = zeros(1,1,2); tempCov(:,:,1) = sqrt(2); tempCov(:,:,2)=1;

      ParticleFiltDS.stateGMM = gmmfit(initialParticles, 2, [0.001 10], 'sqrt');  % fit a 3 component GMM to initial state distribution

      InfDS.estimateType = 'mean';    % estimate type for Xh

      InfDS.spkfType = 'srcdkf';      % Type of SPKF to use inside SPPF (note that ParticleFiltDS.particlesCov should comply)
      InfDS.spkfParams  = sqrt(3);    % scale factor (CDKF parameter h)

      Arg.type='gmm';
      Arg.cov_type='sqrt';
      Arg.dim=model.Vdim;
      Arg.M = 2;
      Arg.mu = cvecrep(model.pNoise.mu,Arg.M);
      Arg.cov = zeros(Arg.dim,Arg.dim,Arg.M);
      Arg.cov(:,:,1) = 2*model.pNoise.cov(:,:,1);
      Arg.cov(:,:,2) = 0.5*model.pNoise.cov(:,:,1);
      Arg.weights = [0.5 0.5];
      pNoise = gennoiseds(Arg);

      [Xh, ParticleFiltDS] = gmsppf(ParticleFiltDS, pNoise, oNoise, y, U1, U2, InfDS);



    %------------------- Sigma-Point Particle Filter -----------------------------------------------
    case 'sppf'

        M = 200;                             % number of particles
        ParticleFiltDS.N = M;
        ParticleFiltDS.particles  = cvecrep(Xh(:,1),M);  % initialize particle means
        ParticleFiltDS.particlesCov = repmat(eye(InfDS.statedim),[1 1 M]);       % particle covariances

        pNoiseGAUS.cov = sqrt(2*3/4);
        oNoiseGAUS.cov = sqrt(1e-1);

        [pNoiseGAUS, oNoiseGAUS, foo] = gensysnoiseds(InfDS,'srcdkf');

        ParticleFiltDS.pNoise = pNoiseGAUS;
        ParticleFiltDS.oNoise = oNoiseGAUS;
        ParticleFiltDS.weights = cvecrep(1/M,M); % initialize weights

        InfDS.spkfType = 'srcdkf';         % Type of SPKF to use (note that ParticleFiltDS.particlesP should comply)
        InfDS.spkfParams = [sqrt(3)];
        InfDS.resampleThreshold = 0.5;    % set resample threshold
        InfDS.estimateType = 'mean';      % estimate type for Xh

        [Xh, ParticleFiltDS] = sppf(ParticleFiltDS, pNoise, oNoise, y, U1, U2, InfDS);

  end

  %--- Plot results

  figure(1); clf;
  p1 = plot(X(1,:)); hold on
  p2 = plot(y,'g+');
  p3 = plot(Xh(1,:),'r'); hold off;
  legend([p1 p2 p3],'clean','noisy',[ftype ' estimate']);
  xlabel('time');
  title('DEMSE2 : Nonlinear Time Variant State Estimation (non Gaussian noise)');

  drawnow

  %--- Calculate mean square estimation error

  rmse(k) = sqrt(mean((Xh(1,2:end)-X(1,2:end)).^2));
  fprintf('\n %d:%d  Root-mean-square-error (RMSE) of estimate : %4.3f\n', k, number_of_runs, rmse(k));

end

mean_RMSE = mean(rmse);

fprintf('\n Mean RMSE : %4.3f\n\n',mean_RMSE);



%--- House keeping

remrelpath('../gssm');       % remove relative search path to example GSSM files from MATLABPATH
remrelpath('../data');       % remove relative search path to example data files from MATLABPATH