Using expected floor normal for more robust floor detection