Skip to content
Snippets Groups Projects
Commit a6a3e1b1 authored by Frédéric Karamé's avatar Frédéric Karamé
Browse files

Make the distinction between original Amisano and Tristani's way to calculate...

Make the distinction between original Amisano and Tristani's way to calculate particles weights and the likelihood (CPF1) and Murray's way (CPF2).
parent c8c14f42
No related branches found
No related tags found
No related merge requests found
...@@ -103,7 +103,11 @@ if ParticleOptions.proposal_approximation.cubature || ParticleOptions.proposal_a ...@@ -103,7 +103,11 @@ if ParticleOptions.proposal_approximation.cubature || ParticleOptions.proposal_a
PredictedObservedVarianceSquareRoot = mat(1:number_of_observed_variables,1:number_of_observed_variables); 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); 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)); 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 else
dState = bsxfun(@minus,tmp(mf0,:),PredictedStateMean); dState = bsxfun(@minus,tmp(mf0,:),PredictedStateMean);
dObserved = bsxfun(@minus,tmp(mf1,:),PredictedObservedMean); dObserved = bsxfun(@minus,tmp(mf1,:),PredictedObservedMean);
...@@ -111,15 +115,20 @@ else ...@@ -111,15 +115,20 @@ else
PredictedObservedVariance = dObserved*diag(weights_c)*dObserved' + H; PredictedObservedVariance = dObserved*diag(weights_c)*dObserved' + H;
PredictedStateAndObservedCovariance = dState*diag(weights_c)*dObserved'; PredictedStateAndObservedCovariance = dState*diag(weights_c)*dObserved';
KalmanFilterGain = PredictedStateAndObservedCovariance/PredictedObservedVariance ; KalmanFilterGain = PredictedStateAndObservedCovariance/PredictedObservedVariance ;
StateVectorMean = PredictedStateMean + KalmanFilterGain*(obs - PredictedObservedMean); Error = obs - PredictedObservedMean ;
StateVectorMean = PredictedStateMean + KalmanFilterGain*Error ;
StateVectorVariance = PredictedStateVariance - KalmanFilterGain*PredictedObservedVariance*KalmanFilterGain'; StateVectorVariance = PredictedStateVariance - KalmanFilterGain*PredictedObservedVariance*KalmanFilterGain';
%StateVectorVariance = .5*(StateVectorVariance+StateVectorVariance');
StateVectorVarianceSquareRoot = chol(StateVectorVariance + 1e-6)' ; 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 end
PredictedStateVarianceSquareRoot = chol(PredictedStateVariance + 1e-6)' ; PredictedStateVarianceSquareRoot = chol(PredictedStateVariance + 1e-6)' ;
ProposalStateVector = StateVectorVarianceSquareRoot*randn(size(StateVectorVarianceSquareRoot,2),1)+StateVectorMean ; ProposalStateVector = StateVectorVarianceSquareRoot*randn(size(StateVectorVarianceSquareRoot,2),1)+StateVectorMean ;
if strcmpi(options_.particle.filter_algorithm, 'cpf2')
Prior = probability2(PredictedStateMean,PredictedStateVarianceSquareRoot,ProposalStateVector) ; Prior = probability2(PredictedStateMean,PredictedStateVarianceSquareRoot,ProposalStateVector) ;
Posterior = probability2(StateVectorMean,StateVectorVarianceSquareRoot,ProposalStateVector) ; Posterior = probability2(StateVectorMean,StateVectorVarianceSquareRoot,ProposalStateVector) ;
Likelihood = probability2(obs,H_lower_triangular_cholesky,measurement_equations(ProposalStateVector,ReducedForm,ThreadsOptions)) ; Likelihood = probability2(obs,H_lower_triangular_cholesky,measurement_equations(ProposalStateVector,ReducedForm,ThreadsOptions)) ;
Weights = SampleWeights.*Likelihood.*(Prior./Posterior) ; Weights = SampleWeights.*Likelihood.*(Prior./Posterior) ;
end
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment