Commit a6a3e1b1 authored by Frédéric Karamé's avatar Frédéric Karamé

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
......@@ -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
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