diff --git a/src/gaussian_filter.m b/src/gaussian_filter.m
index 5500ea8c1d216bdb96f1de8d35ada06aa36e9696..9ec83a0d47387bdfd3d2dd0dcb9bb9546c135770 100644
--- a/src/gaussian_filter.m
+++ b/src/gaussian_filter.m
@@ -92,14 +92,14 @@ if isempty(H)
     H = 0;
     H_lower_triangular_cholesky = 0;
 else
-    H_lower_triangular_cholesky = reduced_rank_cholesky(H)';
+    H_lower_triangular_cholesky = chol(H)' ; %reduced_rank_cholesky(H)';
 end
 
 % Get initial condition for the state vector.
 StateVectorMean = ReducedForm.StateVectorMean;
-StateVectorVarianceSquareRoot = reduced_rank_cholesky(ReducedForm.StateVectorVariance)';
+StateVectorVarianceSquareRoot = chol(ReducedForm.StateVectorVariance)';%reduced_rank_cholesky(ReducedForm.StateVectorVariance)';
 state_variance_rank = size(StateVectorVarianceSquareRoot,2);
-Q_lower_triangular_cholesky = reduced_rank_cholesky(Q)';
+Q_lower_triangular_cholesky = chol(Q)'; %reduced_rank_cholesky(Q)';
 
 % Initialization of the likelihood.
 const_lik = (2*pi)^(number_of_observed_variables/2) ;
@@ -126,12 +126,16 @@ for t=1:sample_size
                                         1/number_of_particles,1/number_of_particles,ReducedForm,ThreadsOptions) ;
         SampleWeights = IncrementalWeights/number_of_particles ;
     end
+    SampleWeights = SampleWeights + 1e-6*ones(size(SampleWeights,1),1) ;
     SumSampleWeights = sum(SampleWeights) ;
     lik(t) = log(SumSampleWeights) ;
     SampleWeights = SampleWeights./SumSampleWeights ;
     StateVectorMean = StateParticles*SampleWeights ;
     temp = bsxfun(@minus,StateParticles,StateVectorMean) ;
-    StateVectorVarianceSquareRoot = reduced_rank_cholesky( bsxfun(@times,SampleWeights',temp)*temp' )';
+    %disp(SampleWeights)
+    %disp(StateParticles) 
+    %disp(StateVectorMean)
+    StateVectorVarianceSquareRoot = chol( bsxfun(@times,SampleWeights',temp)*temp' )';%reduced_rank_cholesky( bsxfun(@times,SampleWeights',temp)*temp' )';
 end
 
 LIK = -sum(lik(start:end));