Odometry not working for θ

[GuiRitter]

θ is not calculated correctly by using the same equations that calculate x and y. The Gazebo model works by using θ read from the simulation, but this can't be ported to a physical prototype.