1. Fixed noises for motion and observation model

2. Fixed the bug where robot would not stop if it reached a node other than the target node index
on the target orbit.