Sorted beachmap vector by angle of poles
Found good parameter for k_s. Odometry should be integrated fine.