Merge pull request #2 from RoboGroup3/master

Merge
Now use a plane normal with a maximum deviation angle for more robust floor detection
Merge pull request #5 from hasofanten/master

Using expected floor normal for more robust floor detection