Continue improving trajectory-based pf