Synthetic aperture radar (SAR) processing on agile radar platforms requires a very accurate estimate of position due to the unpredictable trajectory of such platforms. This is even more challenging in GPS-denied scenarios where the bias in inertial sensors cannot be compensated. In this study, we propose a 3D radar-Aided positioning method based on SAR autofocusing to achieve subresolution positioning accuracy. The proposed multibeam autofocus algorithm builds on multiple SAR motion compensation techniques to generalize and provide a framework for simultaneous omnidirectional imaging and trajectory estimation and correction while taking advantage of the beamforming capabilities of the antenna array. Our initial simulations and proof of concept show that the performance of the proposed approach is adequate even in low signal-To-noise ratio cases.