Improved penalty function