Added some structure for the coordinate systems and changed visualization a bit
Merged the previous two commits