fixing trajectory filtering (tested on robot)