From a6a3e1b109fd44a96889089a03374bdddcf58b01 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Fr=C3=A9d=C3=A9ric=20Karam=C3=A9?= Date: Fri, 4 Dec 2015 15:29:25 +0100 Subject: [PATCH] Make the distinction between original Amisano and Tristani's way to calculate particles weights and the likelihood (CPF1) and Murray's way (CPF2). --- src/conditional_filter_proposal.m | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/src/conditional_filter_proposal.m b/src/conditional_filter_proposal.m index e692c66b5..5eda6b9b1 100644 --- a/src/conditional_filter_proposal.m +++ b/src/conditional_filter_proposal.m @@ -103,7 +103,11 @@ if ParticleOptions.proposal_approximation.cubature || ParticleOptions.proposal_a PredictedObservedVarianceSquareRoot = mat(1:number_of_observed_variables,1:number_of_observed_variables); CovarianceObservedStateSquareRoot = mat(number_of_observed_variables+(1:number_of_state_variables),1:number_of_observed_variables); StateVectorVarianceSquareRoot = mat(number_of_observed_variables+(1:number_of_state_variables),number_of_observed_variables+(1:number_of_state_variables)); - StateVectorMean = PredictedStateMean + (CovarianceObservedStateSquareRoot/PredictedObservedVarianceSquareRoot)*(obs - PredictedObservedMean); + Error = obs - PredictedObservedMean ; + StateVectorMean = PredictedStateMean + (CovarianceObservedStateSquareRoot/PredictedObservedVarianceSquareRoot)*Error ; + if strcmpi(options_.particle.filter_algorithm, 'cpf1') + Weights = SampleWeights.*probability2(zeros(number_of_observed_variables,1),PredictedObservedVarianceSquareRoot,Error) ; + end else dState = bsxfun(@minus,tmp(mf0,:),PredictedStateMean); dObserved = bsxfun(@minus,tmp(mf1,:),PredictedObservedMean); @@ -111,15 +115,20 @@ else PredictedObservedVariance = dObserved*diag(weights_c)*dObserved' + H; PredictedStateAndObservedCovariance = dState*diag(weights_c)*dObserved'; KalmanFilterGain = PredictedStateAndObservedCovariance/PredictedObservedVariance ; - StateVectorMean = PredictedStateMean + KalmanFilterGain*(obs - PredictedObservedMean); + Error = obs - PredictedObservedMean ; + StateVectorMean = PredictedStateMean + KalmanFilterGain*Error ; StateVectorVariance = PredictedStateVariance - KalmanFilterGain*PredictedObservedVariance*KalmanFilterGain'; - %StateVectorVariance = .5*(StateVectorVariance+StateVectorVariance'); StateVectorVarianceSquareRoot = chol(StateVectorVariance + 1e-6)' ; + if strcmpi(options_.particle.filter_algorithm, 'cpf1') + Weights = SampleWeights.*probability2(zeros(number_of_observed_variables,1),chol(PredictedObservedVariance)',Error) ; + end end PredictedStateVarianceSquareRoot = chol(PredictedStateVariance + 1e-6)' ; ProposalStateVector = StateVectorVarianceSquareRoot*randn(size(StateVectorVarianceSquareRoot,2),1)+StateVectorMean ; -Prior = probability2(PredictedStateMean,PredictedStateVarianceSquareRoot,ProposalStateVector) ; -Posterior = probability2(StateVectorMean,StateVectorVarianceSquareRoot,ProposalStateVector) ; -Likelihood = probability2(obs,H_lower_triangular_cholesky,measurement_equations(ProposalStateVector,ReducedForm,ThreadsOptions)) ; -Weights = SampleWeights.*Likelihood.*(Prior./Posterior) ; +if strcmpi(options_.particle.filter_algorithm, 'cpf2') + Prior = probability2(PredictedStateMean,PredictedStateVarianceSquareRoot,ProposalStateVector) ; + Posterior = probability2(StateVectorMean,StateVectorVarianceSquareRoot,ProposalStateVector) ; + Likelihood = probability2(obs,H_lower_triangular_cholesky,measurement_equations(ProposalStateVector,ReducedForm,ThreadsOptions)) ; + Weights = SampleWeights.*Likelihood.*(Prior./Posterior) ; +end \ No newline at end of file