Improved leveling algorithm