diff --git a/nonlinear-filters/src/gaussian_densities.m b/nonlinear-filters/src/gaussian_densities.m index 889a5bbe2..f1c595100 100644 --- a/nonlinear-filters/src/gaussian_densities.m +++ b/nonlinear-filters/src/gaussian_densities.m @@ -42,11 +42,12 @@ proposal = probability2(mut_t,sqr_Pss_t_t,particles) ; prior = probability2(st_t_1,sqr_Pss_t_t_1,particles) ; % likelihood yt_t_1_i = measurement_equations(particles,ReducedForm,ThreadsOptions) ; -eta_t_i = bsxfun(@minus,obs,yt_t_1_i)' ; -yt_t_1 = sum(yt_t_1_i*weigths1,2) ; -tmp = bsxfun(@minus,yt_t_1_i,yt_t_1) ; -Pyy = bsxfun(@times,weigths2',tmp)*tmp' + H ; -sqr_det = sqrt(det(Pyy)) ; -foo = (eta_t_i/Pyy).*eta_t_i ; -likelihood = exp(-0.5*sum(foo,2))/(normconst*sqr_det) + 1e-99 ; +%eta_t_i = bsxfun(@minus,obs,yt_t_1_i)' ; +%yt_t_1 = sum(yt_t_1_i*weigths1,2) ; +%tmp = bsxfun(@minus,yt_t_1_i,yt_t_1) ; +%Pyy = bsxfun(@times,weigths2',tmp)*tmp' + H ; +%sqr_det = sqrt(det(Pyy)) ; +%foo = (eta_t_i/Pyy).*eta_t_i ; +%likelihood = exp(-0.5*sum(foo,2))/(normconst*sqr_det) + 1e-99 ; +likelihood = probability2(obs,sqrt(H),yt_t_1_i) ; IncrementalWeights = likelihood.*prior./proposal ;