Small fixes, added initial correct pose for a stable start