From a73e4d94cb792f439a449b54cd5493a386c7bb9e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Fr=C3=A9d=C3=A9ric=20Karam=C3=A9?=
 <frederic.karame@univ-lemans.fr>
Date: Fri, 4 Dec 2015 16:00:04 +0100
Subject: [PATCH] Add the possibility of Gaussian-Mixture Particle Filter
 without resampling.

---
 src/fit_gaussian_mixture.m    |  4 ++--
 src/gaussian_mixture_filter.m | 11 ++++++----
 src/probability3.m            | 39 +++++++++++++++++++++++++++++++++++
 3 files changed, 48 insertions(+), 6 deletions(-)
 create mode 100644 src/probability3.m

diff --git a/src/fit_gaussian_mixture.m b/src/fit_gaussian_mixture.m
index e4c8e4c5d..43f2302bb 100644
--- a/src/fit_gaussian_mixture.m
+++ b/src/fit_gaussian_mixture.m
@@ -1,4 +1,4 @@
-function [StateMu,StateSqrtP,StateWeights] = fit_gaussian_mixture(X,StateMu,StateSqrtP,StateWeights,crit,niters,check) 
+function [StateMu,StateSqrtP,StateWeights] = fit_gaussian_mixture(X,X_weights,StateMu,StateSqrtP,StateWeights,crit,niters,check) 
 
 % Copyright (C) 2013 Dynare Team
 %
@@ -26,7 +26,7 @@ end
 eold = -Inf;
 for n=1:niters
   % Calculate posteriors based on old parameters
-  [prior,likelihood,marginal,posterior] = probability(StateMu,StateSqrtP,StateWeights,X);
+  [prior,likelihood,marginal,posterior] = probability3(StateMu,StateSqrtP,StateWeights,X,X_weights);
   e = sum(log(marginal));
   if (n > 1 && abs((e - eold)/eold) < crit)
     return;
diff --git a/src/gaussian_mixture_filter.m b/src/gaussian_mixture_filter.m
index 77577d04c..c3116fb9a 100644
--- a/src/gaussian_mixture_filter.m
+++ b/src/gaussian_mixture_filter.m
@@ -294,7 +294,7 @@ for t=1:sample_size
             StateParticles = bsxfun(@plus,StateMuPost(:,i),StateSqrtPPost(:,:,i)*nodes') ;
             IncrementalWeights = gaussian_mixture_densities(Y(:,t),StateMuPrior,StateSqrtPPrior,StateWeightsPrior,...
                                                                    StateMuPost,StateSqrtPPost,StateWeightsPost,...
-                                                                   StateParticles,H,const_lik,weights,weights_c,ReducedForm,ThreadsOptions) ;
+                                                                   StateParticles,H,const_lik,ReducedForm,ThreadsOptions) ;
             SampleWeights(i) = sum(StateWeightsPost(i)*weights.*IncrementalWeights) ;
         end
         SumSampleWeights = sum(SampleWeights) ;
@@ -312,13 +312,16 @@ for t=1:sample_size
         StateParticles = importance_sampling(StateMuPost,StateSqrtPPost,StateWeightsPost',number_of_particles) ;
         IncrementalWeights = gaussian_mixture_densities(Y(:,t),StateMuPrior,StateSqrtPPrior,StateWeightsPrior,...
                                                                StateMuPost,StateSqrtPPost,StateWeightsPost,...
-                                                               StateParticles,H,const_lik,1/number_of_particles,...
-                                                               1/number_of_particles,ReducedForm,ThreadsOptions) ;
+                                                               StateParticles,H,const_lik,ReducedForm,ThreadsOptions) ;
         SampleWeights = IncrementalWeights/number_of_particles ;
         SumSampleWeights = sum(SampleWeights,1) ;
         SampleWeights = SampleWeights./SumSampleWeights ;
         lik(t) = log(SumSampleWeights) ;
-        [StateMu,StateSqrtP,StateWeights] = fit_gaussian_mixture(StateParticles,StateMu,StateSqrtP,StateWeights,0.001,10,1) ;
+        if (ParticleOptions.resampling.status.generic && neff(SampleWeights)<ParticleOptions.resampling.threshold*sample_size) || ParticleOptions.resampling.status.systematic
+            StateParticles = resample(StateParticles',SampleWeights',ParticleOptions)';
+            SampleWeights = ones(number_of_particles,1)/number_of_particles;
+        end
+        [StateMu,StateSqrtP,StateWeights] = fit_gaussian_mixture(StateParticles,SampleWeights',StateMu,StateSqrtP,StateWeights,0.001,10,1) ;
     end
 end
 
diff --git a/src/probability3.m b/src/probability3.m
new file mode 100644
index 000000000..49a14b429
--- /dev/null
+++ b/src/probability3.m
@@ -0,0 +1,39 @@
+function [prior,likelihood,C,posterior] = probability3(mu,sqrtP,prior,X,X_weights)
+
+% Copyright (C) 2013 Dynare Team
+%
+% This file is part of Dynare.
+%
+% Dynare is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% Dynare is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU General Public License for more details.
+%
+% You should have received a copy of the GNU General Public License
+% along with Dynare.  If not, see <http://www.gnu.org/licenses/>.
+
+[dim,nov] = size(X);              
+M = size(mu,2) ;
+if nargout>1
+  likelihood = zeros(M,nov);        
+  normfact = (2*pi)^(dim/2);  
+  for k=1:M
+    XX = bsxfun(@minus,X,mu(:,k));
+    S = sqrtP(:,:,k);
+    foo = S \ XX;
+    likelihood(k,:) = exp(-0.5*sum(foo.*foo, 1))/abs((normfact*prod(diag(S))));
+  end
+end
+wlikelihood = bsxfun(@times,X_weights,likelihood) + 1e-99;
+if nargout>2
+  C = prior*wlikelihood + 1e-99;                   
+end
+if nargout>3
+  posterior = bsxfun(@rdivide,bsxfun(@times,prior',wlikelihood),C) + 1e-99 ;
+  posterior = bsxfun(@rdivide,posterior,sum(posterior,1));
+end