Understanding and implementing belief space planning

Question

I am currently working on state estimation/navigation for a system with multiple robots. As of now, what I have is each robot localizing itself with a Kalman filter, given vision based measurements. As next steps, I am aiming to do two things:

1. Extend this filtering framework to span over all robots so that they can cooperate and improve each other's localization
2. Along with the above, construct a path planning framework such that they can navigate in a way that their localization accuracy is always maximized, thereby eliminating the problem of losing position etc.

To this end, I've been reading about multi-robot state estimation and planning strategies, and have come across belief space planning: or planning under uncertainty. While the math intuitively makes sense, I am having issues with how to implement these techniques in my real world scenario, especially for multiple robots. I have experience using algorithms such as EKF, UKF etc., and sampling based planning strategies like PRM/RRT, but I am having trouble with the probabilistic link between these two.

So far, I've been looking into research papers, but as someone who's mainly a programmer, I'm trying find something more approachable that will help me link the (somewhat abstract) math to my specific problem: for instance, helping me define terms such as 'joint belief of the entire group', using the data I have in hand. What are my best options, and are there any better resources I can consult?

Answer 1

The kalman filter that you've already been using on single robots can be broadened to apply to the swarm of robots. If you previously represented the state of a single robot with 5 variables, and you have 3 robots, then combine all 3 robot states into one state with 15 variables. That larger state representing the entire group could reasonably be called the "joint belief of the entire group" and would be updated in response to any sensor measurement by any member of the group.