Solved a little bug in the SBA* bundle class, ran more tests to compare different planners.