Particle filtering is a common technique for six degree of freedom (6D) pose estimation due to its ability to tractably represent belief over object pose. However, the particle filter is prone to particle deprivation due to the high-dimensional nature of 6D pose. When particle deprivation occurs, it can cause mode collapse of the underlying belief distri-bution during importance sampling. If the region surrounding the true state suffers from mode collapse, recovering its belief is challenging since the area is no longer represented in the probability mass formed by the particles. Previous methods mitigate this problem by randomizing and resetting particles in the belief distribution, but determining the frequency of reinvigoration has relied on hand-tuning abstract heuristics. In this paper, we estimate the necessary reinvigoration rate at each time step by introducing a Counter-Hypothetical likelihood function, which is used alongside the standard likelihood. Inspired by the notions of plausibility and implausibility from Evidential Reasoning, the addition of our Counter-Hypothetical likelihood function assigns a level of doubt to each particle. The competing cumulative values of confidence and doubt across the particle set are used to estimate the level of failure within the filter, in order to determine the portion of particles to be reinvigorated. We demonstrate the effectiveness of our method on the rigid body object 6D pose tracking task.

Advancements in autonomous agents have led to an increasingly ubiquitous presence of robots in human environments where social and physical interaction is expected. Such environments are often composed of heterogeneous agents with disparate action capabilities, intentions, and motivations. Intra- and inter-agent dissimilarity often prevents enacting effective behavioral skills (e.g., collaboration, communication, coordination) towards dynamic task allocation objectives. We propose a Bayesian probabilistic inference approach, MultiRobot Belief Propagation with Identity constraints (MRPBI), for 1) decentralized task allocation in multi-agent systems and 2) modeling task affinity using personal identities. MRPBI leverages competing costs of individual and group capabilities that result in less error-prone convergence to steady state, scalability without loss of accuracy, and sensitivity to environmental dynamics. An implementation of MRBP-I as a distributed algorithm that weighs factors of both individual and cooperative perception in an energy-minimizing task allocation scheme is presented.

We introduce the concept of trajectory-specific sensory manifolds. They are the unique multidimensional and time-varying combinations of afferent signals that obligatorily emerge during a limb movement. We use the example of muscle spindles (i.e., the muscle's proprioceptors for length and velocity) that arise during movements of an arm (a planar 2-DOF 6-muscle model) during the production of straight, curved and oscillatory hand movements. Through the use of parallel coordinates, we visualize the high-dimensional evolution of the afferent signaling across muscles and tasks. We demonstrate that a given movement gives rise to a distinct sensory manifold embedded in the 12-D space of spindle information that is largely independent of the choice of muscle coordination strategy. Given that muscle lengths and velocities are fully determined by joint kinematics, such manifolds provide a rich set of information to use in its control.