Implement the VFH+ algorithm

[agarie]

VFH+ is an enhanced version of VFH. It uses some heuristics so there are less parameters to manually adjust.

From my initial reading of the paper, there shouldn't be many difficulties in adapting the code here.

[mjs513]

Hi aarie

Guess you are getting back into this. Can't wait until you get VFH+ implemented as well as the other issues you posted.

Mike