Dynamic Filters for Online Planning Optimal Trajectories