Simultaneous localization and mapping (SLAM) is the task of building a map representation of an unknown environment while at the same time using it for positioning. A probabilistic interpretation of the SLAM task allows for incorporating prior knowledge and for operation under uncertainty. Contrary to the common practice of computing point estimates of the system states, we capture the full posterior density through approximate Bayesian inference. This dynamic learning task falls under state estimation, where the state-of-the-art is in sequential Monte Carlo methods that tackle the forward filtering problem. In this paper, we introduce a framework for probabilistic SLAM using particle smoothing that does not only incorporate observed data in current state estimates, but it also backtracks the updated knowledge to correct for past drift and ambiguities in both the map and in the states. Our solution can efficiently handle both dense and sparse map representations by Rao-Blackwellization of conditionally linear and conditionally linearized models. We show through simulations and real-world experiments how the principles apply to radio (Bluetooth low-energy/Wi-Fi), magnetic field, and visual SLAM. The proposed solution is general, efficient, and works well under confounding noise.

We present a novel algorithm for online, real-time orientation estimation. Our algorithm integrates gyroscope data and corrects the resulting orientation estimate for integration drift using accelerometer and magnetometer data. This correction is computed, at each time instance, using a single gradient descent step with fixed step length. This fixed step length results in robustness against model errors, e.g., caused by large accelerations or by short-term magnetic field disturbances, which we numerically illustrate using Monte Carlo simulations. Our algorithm estimates a three-dimensional update to the orientation rather than the entire orientation itself. This reduces the computational complexity by approximately 1/3 with respect to the state of the art. It also improves the quality of the resulting estimates, specifically when the orientation corrections are large. We illustrate the efficacy of the algorithm using experimental data.

The linear–quadratic-Gaussian (LQG) control paradigm is well-known in literature. The strategy of minimizing the cost function is available, both for the case where the state is known and where it is estimated through an observer. The situation is different when the cost function has an exponential discount factor, also known as a prescribed degree of stability. In this case, the optimal control strategy is only available when the state is known. This paper builds onward from that result, deriving an optimal control strategy when working with an estimated state. Expressions for the resulting optimal expected cost are also given.