Added a ground just to make things a bit easier :-)
Added a first implementation of multi-grid map

Extended math lib functionalities