Optimal measurement selection algorithm and estimator for ultra-wideband symmetric ranging localization

A state estimator is derived for an agent with the ability to measure single ranges to fixed points in its environment, and equipped with an accelerometer and a rate gyroscope. The state estimator makes no agent-specific assumptions, and can be immediately applied to any rigid body with these sensors. Also, the state estimator doesn't use any trilateration-based method to calculate position from range measurements. As the considered system can only make a single range measurement at a time, we present a greedy optimization algorithm for selecting the best measurement. Experiments in an indoor testbed using an externally controlled multicopter demonstrate the efficacy of the algorithm, specifically showing an improvement over a na\"ive strategy of a fixed sequence of measurements. In separate experiments, the algorithm is also used in feedback control, to control the position of the multicopter.