Fix particle filter update