[WIP] Fix auxiliary particle filter.

matlab/nonlinear-filters/auxiliary_particle_filter.m

@@ -76,13 +76,14 @@ H = ReducedForm.H;
 StateVectorMean = ReducedForm.StateVectorMean;
 StateVectorVarianceSquareRoot = chol(ReducedForm.StateVectorVariance)';
 state_variance_rank = size(StateVectorVarianceSquareRoot,2);
-Q_lower_triangular_cholesky = chol(Q)';
+Q_lower_triangular_cholesky = chol(Q, 'lower');
+H_lower_triangular_cholesky = chol(H, 'lower');
 
 % Set seed for randn().
 options_=set_dynare_seed_local_options(options_,'default');
 
 % Initialization of the likelihood.
-const_lik = log(2*pi)*number_of_observed_variables+log(det(H));
+const_lik = log(2*pi)*number_of_observed_variables + sum(log(diag(H_lower_triangular_cholesky)));
 lik  = NaN(sample_size,1);
 LIK  = NaN;
 
@@ -140,13 +141,15 @@ for t=1:sample_size
             end
         end
     end
-    PredictionError = bsxfun(@minus,Y(:,t),tmp(mf1,:));
-    z = sum(PredictionError.*(H\PredictionError),1) ;
+    StandardPredictionError = H_lower_triangular_cholesky\bsxfun(@minus,Y(:,t), tmp(mf1,:));
+    lambda = weights.*exp(-.5*(const_lik + sum(StandardPredictionError.*StandardPredictionError, 1)));
+    LAMBDA = lambda/sum(lambda);
+    indx = resample(0, LAMBDA, ParticleOptions);
     %    tau_tilde = weights.*(tpdf(z,3*ones(size(z)))+1e-99) ;
     ddl = 3 ;
     tau_tilde = weights.*(exp(gammaln((ddl + 1) / 2) - gammaln(ddl/2))./(sqrt(ddl*pi).*(1 + (z.^2)./ddl).^((ddl + 1)/2))+1e-99) ;
     tau_tilde = tau_tilde/sum(tau_tilde) ;
-    indx = resample(0,tau_tilde',ParticleOptions);
+    
     if pruning
         yhat_ = yhat_(:,indx) ;
     end