Fix initial x and theta and add some logging