An algorithm for accurate and robust indoor localization based on nonlinear programming