Fixed some bugs, discovered during outdoor testing
- fixed poles being mapped due to random high intensity reflections
- tuned covariance paramater to get smoother results