From a6a3e1b109fd44a96889089a03374bdddcf58b01 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Fr=C3=A9d=C3=A9ric=20Karam=C3=A9?=
 <frederic.karame@univ-lemans.fr>
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 e692c66..5eda6b9 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
-- 
GitLab