Fixed Robot Navigation

By increasing the angular resolution of the cache file and fixing
some errors with handling angles (i.e. correcting 'modulo 2*pi' bugs)
I've made MCL approximately work for the robot with orientation.

Also cleaned up ogmap.py