diff --git a/nonlinear-filters/src/auxiliary_particle_filter.m b/nonlinear-filters/src/auxiliary_particle_filter.m
index f9cbf948c..cba5410aa 100644
--- a/nonlinear-filters/src/auxiliary_particle_filter.m
+++ b/nonlinear-filters/src/auxiliary_particle_filter.m
@@ -86,6 +86,18 @@ StateVectors = bsxfun(@plus,StateVectorVarianceSquareRoot*randn(state_variance_r
 if pruning
     StateVectors_ = StateVectors;
 end
+
+epsilon = Q_lower_triangular_cholesky*randn(number_of_structural_innovations,number_of_particles);
+yhat = zeros(2,number_of_particles) ; 
+if pruning
+    yhat_ = zeros(2,number_of_particles) ; 
+    [tmp, tmp_] = local_state_space_iteration_2(yhat,epsilon,ghx,ghu,constant,ghxx,ghuu,ghxu,yhat_,steadystate,ThreadsOptions.local_state_space_iteration_2);
+    StateVectors_ = tmp_(mf0,:);
+else
+    tmp = local_state_space_iteration_2(yhat,epsilon,ghx,ghu,constant,ghxx,ghuu,ghxu,ThreadsOptions.local_state_space_iteration_2);
+end
+StateVectors = tmp(mf0,:) ;
+
 for t=1:sample_size
     yhat = bsxfun(@minus,StateVectors,state_variables_steady_state);
     if pruning
@@ -101,27 +113,14 @@ for t=1:sample_size
     wtilde = exp(-.5*(const_lik+log(det(PredictedObservedVariance))+sum(PredictionError.*(PredictedObservedVariance\PredictionError),1))) ;
     tau_tilde = weights.*wtilde ;
     sum_tau_tilde = sum(tau_tilde) ;
-    %var_wtilde = wtilde-sum_tau_tilde ;
-    %var_wtilde = var_wtilde'*var_wtilde/(number_of_particles-1) ;
-    lik(t) = log(sum_tau_tilde) ; %+ .5*var_wtilde/(number_of_particles*(sum_tau_tilde*sum_tau_tilde)) ;
+    lik(t) = log(sum_tau_tilde) ; 
     tau_tilde = tau_tilde/sum_tau_tilde;
+    indx = resample(0,tau_tilde',ParticleOptions);
     if pruning
-        temp = resample([yhat' yhat_'],tau_tilde',ParticleOptions);
-        yhat = temp(:,1:number_of_state_variables)' ;
-        yhat_ = temp(:,number_of_state_variables+1:2*number_of_state_variables)' ;
-    else
-        yhat = resample(yhat',tau_tilde',ParticleOptions)' ;
+        yhat_ = yhat_(:,indx) ;
     end
-    if pruning
-        [tmp, tmp_] = local_state_space_iteration_2(yhat,zeros(number_of_structural_innovations,number_of_particles),ghx,ghu,constant,ghxx,ghuu,ghxu,yhat_,steadystate,ThreadsOptions.local_state_space_iteration_2);
-    else
-        tmp = local_state_space_iteration_2(yhat,zeros(number_of_structural_innovations,number_of_particles),ghx,ghu,constant,ghxx,ghuu,ghxu,ThreadsOptions.local_state_space_iteration_2);
-    end
-    PredictedObservedMean = weights*(tmp(mf1,:)');
-    PredictionError = bsxfun(@minus,Y(:,t),tmp(mf1,:));
-    dPredictedObservedMean = bsxfun(@minus,tmp(mf1,:),PredictedObservedMean');
-    PredictedObservedVariance = bsxfun(@times,weights,dPredictedObservedMean)*dPredictedObservedMean' +H;
-    wtilde = exp(-.5*(const_lik+log(det(PredictedObservedVariance))+sum(PredictionError.*(PredictedObservedVariance\PredictionError),1))) ;
+    yhat = yhat(:,indx) ;
+    wtilde = wtilde(indx) ;
     epsilon = Q_lower_triangular_cholesky*randn(number_of_structural_innovations,number_of_particles);
     if pruning
         [tmp, tmp_] = local_state_space_iteration_2(yhat,epsilon,ghx,ghu,constant,ghxx,ghuu,ghxu,yhat_,steadystate,ThreadsOptions.local_state_space_iteration_2);
diff --git a/nonlinear-filters/src/resample.m b/nonlinear-filters/src/resample.m
index d49abf12e..3fee0c355 100644
--- a/nonlinear-filters/src/resample.m
+++ b/nonlinear-filters/src/resample.m
@@ -1,5 +1,7 @@
-function resampled_particles = resample(particles,weights,ParticleOptions)
+function resampled_output = resample(particles,weights,ParticleOptions)
 % Resamples particles.
+% if particles = 0, returns the resampling index (except for smooth resampling)
+% Otherwise, returns the resampled particles set.
 
 %@info:
 %! @deftypefn {Function File} {@var{indx} =} resample (@var{weights}, @var{method})
@@ -55,19 +57,22 @@ defaultmethod = 1; % For residual based method set this variable equal to 0.
 
 if defaultmethod
     if ParticleOptions.resampling.method.kitagawa
-        resampled_particles = traditional_resampling(particles,weights,rand);
+        resampled_output = traditional_resampling(particles,weights,rand);
     elseif ParticleOptions.resampling.method.stratified
-        resampled_particles = traditional_resampling(particles,weights,rand(size(weights)));
+        resampled_output = traditional_resampling(particles,weights,rand(size(weights)));
     elseif ParticleOptions.resampling.method.smooth
-        resampled_particles = multivariate_smooth_resampling(particles,weights);
+        if particles==0
+            error('Particle = 0 is incompatible with this resampling method!')
+        end
+        resampled_output = multivariate_smooth_resampling(particles,weights);
     else
-        error('Unknow sampling method!')
+        error('Unknown sampling method!')
     end
 else
     if ParticleOptions.resampling.method.kitagawa
-        resampled_particles = residual_resampling(particles,weights,rand);
+        resampled_output = residual_resampling(particles,weights,rand);
     elseif ParticleOptions.resampling.method.stratified
-        resampled_particles = residual_resampling(particles,weights,rand(size(weights)));
+        resampled_output = residual_resampling(particles,weights,rand(size(weights)));
     else
         error('Unknown sampling method!')
     end