Include information on the environment in map-aware particle filter

Question

I have a robot equipped with some sensors for estimating the movement in a 2D environment (IMU, odometer). The robot is free to move within an area delimited by some walls. The map of the environment (i.e. the polygon given by the walls position) is known. The environment is represented by a 2D grid and a reference mask $M_{ref}$ is computed such that the mask is equal to 1 inside the polygon and 0 outside. The starting position of the robot is not known.

I am implementing a (kind of) particle filter to estimate the robot position given the reference mask. First, a mask $M$ is initialized equal to $M_{ref}$. This mask represents the probability for the mower to be in a certain cell. When the robot moves, the sensors estimate the translation, $dx$ and $dy$, and the intersection between $M_{ref}$ and the translated version of $M$ is computed. The intersection is then assigned to $M$ and the rest of the mask is set to 0. After some movements, the intersection shrinks around the real position.

Now, suppose that other elements of the environment are known, such as slopes, landmarks and so on, and the robot is able to detect them. This could greatly help but I don't know how to integrate this information in my method. Are there similar solution in the literature?

Answer 1

Answer

Create a new "probability grid" for each sensing modality and combine them after you have estimated the position of the robot in each of the many different grids.

In short, you have a mask for "Where I am now that I've moved" already, and now need to create a separate one for "where I am given I measured a slope of X", and one for "where I am given I measured I am 10 feet from landmark L" and so on. Yes, one mask for each landmark is fine.

Each grid represents a discretized probability map of p(robot_position| that_sensor). To combine and get p(robot_position| that_sensor & some_other_sensor & etc), you can multiply all masks (cells) together, and normalize back so that the sum of all cells is equal to one. Be careful of very small probability values that will be essentially zero.

This is not a particle filter, mind you. It is a simple probability grid, but is a perfectly reasonable thing to do for bounded environments.

---

Bonus comment

Having said that, examining your setup, you say that the intersection of the robot positions shrinks down to be centered at the real position. I am suspicious if you are only using IMU / motion data. As time goes on and the robot moves, it should in fact eventually become a "uniform" probability, that is you'll see the same probability in each cell of the discretzied polygon. Simply translating the "most likely cell" to the new mask is incorrect, you need to "spread out" the probability by sampling many different possible motions given what the IMU says you did. How to do this is well covered by books like Probabilistic Robotics, or any particle filter tutorial.

To avoid this, you need some global information. Slope, landmarks, etc can help.