Fixed bug with trajectory recording
Experimenting with adaptive threshold