viz: sonar beam visualization, show color per default
added the possibility to hand the complete odometry transfomration to the estimators
and added backward compatibility for giving only the robot heading
math: added a helper method to compute the angle of a 2d vector
added first version of a cluster and filter based structure estimation