3D Self-Localisation From Angle of Arrival Measurements

We propose a 3D self-localisation method that uses 3D angle of arrival (AOA) information (ie., azimuth and elevation measurements) from landmarks. The formulation is based on minimising the collinearity error between the estimated line of sight (LOS) to the landmark and the measured AOA. This method runs in two parts - initial estimation of the vehicle azimuth and position assuming the vehicle has no tilt, and iterative 3D pose estimation based on a small angle approximation approach. Simulation study indicates that this method is efficient, requiring a small number of iterations, globally convergent and robust.