Layered costmap implementation for dynamic furniture tracking