Commit c8c14f42 authored by Frédéric Karamé's avatar Frédéric Karamé
Browse files

Re-activate the option of resampling in Gaussian Particle Filter.

parent c50392de
...@@ -92,14 +92,14 @@ if isempty(H) ...@@ -92,14 +92,14 @@ if isempty(H)
H = 0; H = 0;
H_lower_triangular_cholesky = 0; H_lower_triangular_cholesky = 0;
else else
H_lower_triangular_cholesky = chol(H)' ; %reduced_rank_cholesky(H)'; H_lower_triangular_cholesky = reduced_rank_cholesky(H)';
end end
% Get initial condition for the state vector. % Get initial condition for the state vector.
StateVectorMean = ReducedForm.StateVectorMean; StateVectorMean = ReducedForm.StateVectorMean;
StateVectorVarianceSquareRoot = chol(ReducedForm.StateVectorVariance)';%reduced_rank_cholesky(ReducedForm.StateVectorVariance)'; StateVectorVarianceSquareRoot = reduced_rank_cholesky(ReducedForm.StateVectorVariance)';
state_variance_rank = size(StateVectorVarianceSquareRoot,2); state_variance_rank = size(StateVectorVarianceSquareRoot,2);
Q_lower_triangular_cholesky = chol(Q)'; %reduced_rank_cholesky(Q)'; Q_lower_triangular_cholesky = reduced_rank_cholesky(Q)';
% Initialization of the likelihood. % Initialization of the likelihood.
const_lik = (2*pi)^(number_of_observed_variables/2) ; const_lik = (2*pi)^(number_of_observed_variables/2) ;
...@@ -115,7 +115,7 @@ for t=1:sample_size ...@@ -115,7 +115,7 @@ for t=1:sample_size
gaussian_densities(Y(:,t),StateVectorMean,... gaussian_densities(Y(:,t),StateVectorMean,...
StateVectorVarianceSquareRoot,PredictedStateMean,... StateVectorVarianceSquareRoot,PredictedStateMean,...
PredictedStateVarianceSquareRoot,StateParticles,H,const_lik,... PredictedStateVarianceSquareRoot,StateParticles,H,const_lik,...
ReducedForm,ThreadsOptions) ; weights2,weights_c2,ReducedForm,ThreadsOptions) ;
SampleWeights = weights2.*IncrementalWeights ; SampleWeights = weights2.*IncrementalWeights ;
else else
StateParticles = bsxfun(@plus,StateVectorVarianceSquareRoot*randn(state_variance_rank,number_of_particles),StateVectorMean) ; StateParticles = bsxfun(@plus,StateVectorVarianceSquareRoot*randn(state_variance_rank,number_of_particles),StateVectorMean) ;
...@@ -123,19 +123,22 @@ for t=1:sample_size ...@@ -123,19 +123,22 @@ for t=1:sample_size
gaussian_densities(Y(:,t),StateVectorMean,... gaussian_densities(Y(:,t),StateVectorMean,...
StateVectorVarianceSquareRoot,PredictedStateMean,... StateVectorVarianceSquareRoot,PredictedStateMean,...
PredictedStateVarianceSquareRoot,StateParticles,H,const_lik,... PredictedStateVarianceSquareRoot,StateParticles,H,const_lik,...
ReducedForm,ThreadsOptions) ; 1/number_of_particles,1/number_of_particles,ReducedForm,ThreadsOptions) ;
SampleWeights = IncrementalWeights/number_of_particles ; SampleWeights = IncrementalWeights/number_of_particles ;
end end
SampleWeights = SampleWeights + 1e-6*ones(size(SampleWeights,1),1) ; SampleWeights = SampleWeights + 1e-6*ones(size(SampleWeights,1),1) ;
SumSampleWeights = sum(SampleWeights) ; SumSampleWeights = sum(SampleWeights) ;
lik(t) = log(SumSampleWeights) ; lik(t) = log(SumSampleWeights) ;
SampleWeights = SampleWeights./SumSampleWeights ; SampleWeights = SampleWeights./SumSampleWeights ;
if not(ParticleOptions.distribution_approximation.cubature || ParticleOptions.distribution_approximation.unscented)
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
end
StateVectorMean = StateParticles*SampleWeights ; StateVectorMean = StateParticles*SampleWeights ;
temp = bsxfun(@minus,StateParticles,StateVectorMean) ; temp = bsxfun(@minus,StateParticles,StateVectorMean) ;
%disp(SampleWeights) StateVectorVarianceSquareRoot = reduced_rank_cholesky( bsxfun(@times,SampleWeights',temp)*temp' )';
%disp(StateParticles)
%disp(StateVectorMean)
StateVectorVarianceSquareRoot = chol( bsxfun(@times,SampleWeights',temp)*temp' )';%reduced_rank_cholesky( bsxfun(@times,SampleWeights',temp)*temp' )';
end end
LIK = -sum(lik(start:end)); LIK = -sum(lik(start:end));
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment