Fixed the update function for the heuristic map. Now it actually works. Thanks Jannick.
Merge branch 'Release/MileStone3/debug' of https://github.com/srnjcbsn/Beduin into Release/MileStone3/debug