Fixed bug with trajectory saving