Stein-MAP: A Sequential Variational Inference Framework for Maximum A Posteriori Estimation

State estimation poses substantial challenges in robotics, often involving encounters with multimodality in real-world scenarios. To address these challenges, it is essential to calculate Maximum a posteriori (MAP) sequences from joint probability distributions of latent states and observations over time. However, it generally involves a trade-off between approximation errors and computational complexity. In this article, we propose a new method for MAP sequence estimation called Stein-MAP, which effectively manages multimodality with fewer approximation errors while significantly reducing computational and memory burdens. Our key contribution lies in the introduction of a sequential variational inference framework designed to handle temporal dependencies among transition states within dynamical system models. The framework integrates Stein's identity from probability theory and reproducing kernel Hilbert space (RKHS) theory, enabling computationally efficient MAP sequence estimation. As a MAP sequence estimator, Stein-MAP boasts a computational complexity of O(N), where N is the number of particles, in contrast to the O(N^2) complexity of the Viterbi algorithm. The proposed method is empirically validated through real-world experiments focused on range-only (wireless) localization. The results demonstrate a substantial enhancement in state estimation compared to existing methods. A remarkable feature of Stein-MAP is that it can attain improved state estimation with only 40 to 50 particles, as opposed to the 1000 particles that the particle filter or its variants require.

Bayesian Learning for Dynamic Target Localization with Human-provided Spatial Information

This paper considers a human-autonomy collaborative sensor data fusion for dynamic target localization in a Bayesian framework. To compensate for the shortcomings of an autonomous tracking system, we propose to collect spatial sensing information from human operators who visually monitor the target and can provide target localization information in the form of free sketches encircling the area where the target is located. Our focus in this paper is to construct an adaptive probabilistic model for human-provided inputs where the adaption terms capture the level of reliability of the human inputs. The next contribution of this paper is a novel joint Bayesian learning method to fuse human and autonomous sensor inputs in a manner that the dynamic changes in human detection reliability are also captured and accounted for. A unique aspect of this Bayesian modeling framework is its analytical closed-form update equations, endowing our method with significant computational efficiency. Simulations demonstrate our results.

Distributed Unconstrained Optimization with Time-varying Cost Functions

In this paper, we propose a novel solution for the distributed unconstrained optimization problem where the total cost is the summation of time-varying local cost functions of a group networked agents. The objective is to track the optimal trajectory that minimizes the total cost at each time instant. Our approach consists of a two-stage dynamics, where the first one samples the first and second derivatives of the local costs periodically to construct an estimate of the descent direction towards the optimal trajectory, and the second one uses this estimate and a consensus term to drive local states towards the time-varying solution while reaching consensus. The first part is carried out by the implementation of a weighted average consensus algorithm in the discrete-time framework and the second part is performed with a continuous-time dynamics. Using the Lyapunov stability analysis, an upper bound on the gradient of the total cost is obtained which is asymptotically reached. This bound is characterized by the properties of the local costs. To demonstrate the performance of the proposed method, a numerical example is conducted that studies tuning the algorithm's parameters and their effects on the convergence of local states to the optimal trajectory.

Online Target Localization using Adaptive Belief Propagation in the HMM Framework

This paper proposes a novel adaptive sample space-based Viterbi algorithm for ultra-wideband (UWB) based target localization in an online manner. As the discretized area of interest is defined as a finite number of hidden states, the most probable trajectory of the unspecified agent is computed efficiently via dynamic programming in a Hidden Markov Model (HMM) framework. Furthermore, the approach has no requirements about Gaussian assumption and linearization for Bayesian calculation. However, the issue of computational complexity becomes very critical as the number of hidden states increases for estimation accuracy and large space. Previous localization works, based on discrete-state HMM, handle a small number of hidden variables, which represent specific paths or places. Inspired by the k-d Tree algorithm (e.g., quadtree) that is commonly used in the computer vision field, we propose a belief propagation in the most probable belief space with a low to high-resolution sequentially, thus reducing the required resources significantly. Our method has three advantages for localization: (a) no Gaussian assumptions and linearization, (b) handling the whole area of interest, not specific or small map representations, (c) reducing computation time and required memory size. Experimental tests demonstrate our results.

UWB-based Target Localization using Adaptive Belief Propagation in the HMM Framework

This paper proposes a novel adaptive sample space-based Viterbi algorithm for ultra-wideband (UWB) based target localization in an online manner. As the discretized area of interest is defined as a finite number of hidden states, the most probable trajectory of the unspecified agent is computed efficiently via dynamic programming in a Hidden Markov Model (HMM) framework. Furthermore, the approach has no requirements about Gaussian assumption and linearization for Bayesian calculation. However, the issue of computational complexity becomes very critical as the number of hidden states increases for estimation accuracy and large space. Previous localization works, based on discrete-state HMM, handle a small number of hidden variables, which represent specific paths or places. Inspired by the k-d Tree algorithm (e.g., quadtree) that is commonly used in the computer vision field, we propose a belief propagation in the most probable belief space with a low to high-resolution sequentially, thus reducing the required resources significantly. Our method has three advantages for localization: (a) no Gaussian assumptions and linearization, (b) handling the whole area of interest, not specific or small map representations, (c) reducing computation time and required memory size. Experimental tests demonstrate our results.

Learning Contraction Policies from Offline Data

This paper proposes a data-driven method for learning convergent control policies from offline data using Contraction theory. Contraction theory enables constructing a policy that makes the closed-loop system trajectories inherently convergent towards a unique trajectory. At the technical level, identifying the contraction metric, which is the distance metric with respect to which a robot's trajectories exhibit contraction is often non-trivial. We propose to jointly learn the control policy and its corresponding contraction metric while enforcing contraction. To achieve this, we learn an implicit dynamics model of the robotic system from an offline data set consisting of the robot's state and input trajectories. Using this learned dynamics model, we propose a data augmentation algorithm for learning contraction policies. We randomly generate samples in the state-space and propagate them forward in time through the learned dynamics model to generate auxiliary sample trajectories. We then learn both the control policy and the contraction metric such that the distance between the trajectories from the offline data set and our generated auxiliary sample trajectories decreases over time. We evaluate the performance of our proposed framework on simulated robotic goal-reaching tasks and demonstrate that enforcing contraction results in faster convergence and greater robustness of the learned policy.

Learning-based Measurement Scheduling for Loosely-Coupled Cooperative Localization

In cooperative localization, communicating mobile agents use inter-agent relative measurements to improve their dead-reckoning-based global localization. Measurement scheduling enables an agent to decide which subset of available inter-agent relative measurements it should process when its computational resources are limited. Optimal measurement scheduling is an NP-hard combinatorial optimization problem. The so-called sequential greedy (SG) algorithm is a popular suboptimal polynomial-time solution for this problem. However, the merit function evaluation for the SG algorithms requires access to the state estimate vector and error covariance matrix of all the landmark agents (teammates that an agent can take measurements from). This paper proposes a measurement scheduling for CL that follows the SG approach but reduces the communication and computation cost by using a neural network-based surrogate model as a proxy for the SG algorithm's merit function. The significance of this model is that it is driven by local information and only a scalar metadata from the landmark agents. This solution addresses the time and memory complexity issues of running the SG algorithm in three ways: (a) reducing the inter-agent communication message size, (b) decreasing the complexity of function evaluations by using a simpler surrogate (proxy) function, (c) reducing the required memory size.Simulations demonstrate our results.

An IMM-based Decentralized Cooperative Localization with LoS and NLoS UWB Inter-agent Ranging

This paper studies the global localization of a group of communicating mobile agents via an ultra-wideband (UWB) inter-agent ranging aided dead-reckoning system. We propose a loosely coupled cooperative localization algorithm that acts as an augmentation atop the local dead-reckoning system of each mobile agent. This augmentation becomes active only when an agent wants to process a relative measurement it has taken. The main contribution of this paper is to address the challenges in the proper processing of the UWB range measurements in the framework of the proposed cooperative localization. Even though UWB offers a decimeter level accuracy in line-of-sight (LoS) ranging, its accuracy degrades significantly in non-line-of-sight (NLoS) due to the significant unknown positive bias in the measurements. Thus, the measurement models for the UWB LoS and NLoS ranging conditions are different, and proper processing of NLoS measurements requires a bias compensation measure. On the other hand, in practice, the measurement modal discriminators determine the type of UWB range measurements with only some level of certainty. To take into account the probabilistic nature of the NLoS identifiers, our proposed cooperative localization employs an interacting multiple model (IMM) estimator. The effectiveness of our proposed method is demonstrated via an experiment for a group of pedestrians who use UWB relative range measurements among themselves to improve their geolocation using a shoe-mounted INS system.

A sub-modular receding horizon solution for mobile multi-agent persistent monitoring

We study the problem of persistent monitoring of finite number of inter-connected geographical nodes for event detection via a group of heterogeneous mobile agents. We use Poisson process model to capture the probability of the events occurring at the geographical nodes. We then tie a utility function to the probability of detecting an event in each point of interest and use it in our policy design to incentivize the agents to visit the geographical nodes with higher probability of event occurrence. We show that the design of an optimal monitoring policy to maximize the utility of event detection over a mission horizon is an NP-hard problem. By showing that the reward function is a monotone increasing and submodular function, we then proceed to propose a suboptimal dispatch policy design with a known optimality gap. To reduce the time complexity of constructing the feasible search set and also to induce robustness to changes in event occurrence and other operational factors, we preform our suboptimal policy design in a receding horizon setting. Our next contribution is to add a new term to our optimization problem to compensate for the shortsightedness of the receding horizon approach. This added term provides a measure of importance for nodes beyond the receding horizon's sight, and is meant to give the policy design an intuition to steer the agents towards areas with higher importance on the global map. Finally, we discuss how our proposed algorithm can be implemented in a decentralized manner. We demonstrate our results through a simulation study.

Cooperative Localization under Limited Connectivity

We report two novel decentralized multi-agent cooperative localization algorithms in which, to reduce the communication cost, inter-agent state estimate correlations are not maintained but accounted for implicitly. In our first algorithm, to guarantee filter consistency, we account for unknown inter-agent correlations via an upper bound on the joint covariance matrix of the agents. In the second method, we use an optimization framework to estimate the unknown inter-agent cross-covariance matrix. In our algorithms, each agent localizes itself in a global coordinate frame using a local filter driven by local dead reckoning and occasional absolute measurement updates and opportunistically corrects its pose estimate whenever a relative measurement takes place between this agent and another mobile agent. To process that relative measurement, only those two agents need to communicate with each other. Consequently, our algorithms are decentralized algorithms that do not impose restrictive network-wide connectivity condition. Moreover, we make no assumptions about the type of agents or relative measurements. We demonstrate our algorithms in simulation and a robotic experiment.