Radars on unmanned vehicles can be used in indoor rescue scenarios to detect missing people based on their motion. In this case, the phase of the radar signals, which provides information about sub-millimeter changes in distance, have to be evaluated. However, in order to detect or to evaluate the motion of a missing person, the motion of the unmanned vehicle itself must be known and compensated for. To estimate the ego-motion of the unmanned vehicle, the radar system measures the phase information for multiple static objects in the radar's environment. A model describing how the motion of the radar affects the phase signals is used to estimate the ego-motion by solving a system of equations. The accuracy of the estimated position, depends on how accurately the phase measurements represent the model. By analytically decomposing the errors in the measurement model, it is shown that the radar's limited angular resolution induces a significant error. To mitigate this source of error, a weighted least squares approach is derived that minimizes the influence of the angular resolution on the position error. By calculating the position error distribution with and without the weighting approach, the benefits of the proposed algorithms are stated. Furthermore, the results are validated using simulations and real world measurements, showing that the proposed algorithm achieves sub-millimeter position accuracy.