functioning octomap filter, needs better tf sync though