fix crash at startup
big refactor
fix initial theta
bug fix
Merge pull request #23 from tobiasandersson/master

Mapping progress