The study of techniques to estimate the position of mobile devices with a high level of accuracy and robustness is essential to provide advanced location based services in indoor environments. An algorithm to enable mobile devices to estimate their positions in known indoor environments is proposed in this paper under the assumption that fixed anchor nodes are available at known locations. The proposed algorithm is specifically designed to be executed on the mobile device whose position is under investigation, and it allows the device to estimate its position within the environment by actively measuring distance estimates from the anchor nodes. In order to reduce the impact of the errors caused by the arrangement of the anchor nodes in the environment, the proposed algorithm first transforms the localization problem into an optimization problem, and then, it solves the derived optimization problem using techniques inspired by nonlinear programming. Experimental results obtained using ultra-wide band signaling are presented to assess the performance of the algorithm and to compare it with reference alternatives. The presented experimental results confirm that the proposed algorithm provides an increased level of accuracy and robustness with respect to two reference alternatives, regardless of the position of the anchor nodes.