Fixes comments and function

Fix error for generate theta in odometry
Fix comments for odometry, map and mapinv
Fix equations