fix planner position not being updated correctly
fix z step value drift