# A Particle PHD Filter for Dynamic Grid Map Building towards Indoor Environment
## 3 The Proposed Framework for Building Dynamic Grid Map
### Preprocessing of LIDAR Measurements  
In order to improve the obstacle avoidance ability of the robot, the real-time performance of a dynamic grid map must be guaranteed. However, in the process of building a dynamic grid map with a particle-based PHD filter, it often needs a large number of particles to achieve better estimation of objects, and more particles will reduce the real-time performance of the system, which also limits the number of particles. However, there are many static objects (such as walls, tables) in the indoor navigation environment. Similar environments are established in Gazebo, as shown in Figure 2a. The white object represents the robot, the yellow object represents a moving dynamic object, and the rest are static objects. The prior map and laser measurements are shown in Figure 2b, and red indicates the laser measurements. If all the scanning points measured by the laser are input into the particle filter for estimation, the particles distribution of the filter is shown in Figure 2c. It can be found that a large number of particles are occupied on the static walls and tables for the estimation of zero velocity. In the same number of particles, this will reduce the state estimation effect of yellow dynamic objects. However, the static object information can be directly transmitted to the global planner through the static cost map during navigation, and it does not need to estimate the speed information. Therefore, in order to use as few particles as possible to estimate the dynamic object better, the input filter measurements must be preprocessed first. In order to solve this problem, firstly, a prior grid map is used to preprocess to get an inflated occupancy probability grid map. Assuming that the grid coordinate is  
Figure 1: The framework of system. Figure 2: (**a**) Test environment; (**b**) Laser measurements including static and dynamic objects; (**c**) Particles distribution. \((index^{i}_{x},indeg^{i}_{x})\), the nearest distance from the obstacle is _dis_, and then the occupancy probability function of the grid cell is:  
\[M((index^{i}_{x},indeg^{i}_{x}))=\left\{ \begin{array}{ll}e^{-\frac{\textit{ dis}^{2}}{2\sigma^{2}}}&\textit{dis}<\textit{dis}\_th\\ 0&\textit{else}\end{array} \right. \tag{13}\]  
where _dis_ is the nearest distance from the grid cell to the obstacle in the prior map, _dis_th_ is the distance threshold for calculating occupancy probability, and \(\sigma\) is the set constant parameter. Using the depth-first method, we can calculate the occupancy probability of the entire grid map from the grid cell occupied by obstacles. Since the inflated occupancy probability grid map only needs to be calculated once, the real-time performance of the system will not be reduced. According to the robot position \((x^{r}_{map},y^{r}_{map},theta^{r}_{map})\), the laser point coordinates in the Lidar coordinate system can be projected into the map coordinate system to obtain its occupancy probability and judge whether it is a dynamic object or static object. For the _i_th_ laser point, we can directly obtain the polar measurements \(ranges[i]\) and \(angles[i]\) in the Lidar coordinate system, so the Cartesian coordinates in the Lidar coordinate system can be obtained:  
\[\left\{ \begin{array}{ll}x^{i}_{laser}=ranges[i]*\cos(theatas[i])\\ y^{i}_{laser}=ranges[i]*\sin(theatas[i])\end{array} \right.. \tag{14}\]  
Then, the coordinates \((x^{i}_{map},y^{i}_{map})\) of the _i_th_ laser point in the coordinates of the world coordinate system (map) can be calculated (assuming that the robot coordinate system coincides with the Lidar coordinate system):  
\[\left\{ \begin{array}{ll}x^{i}_{map}=x^{r}_{map}+x^{i}_{laser}*\cos(theta^ {r}_{map})-y^{i}_{laser}*\sin(theta^{r}_{map})\\ y^{i}_{map}=y^{r}_{map}+x^{i}_{laser}*\sin(theta^{r}_{map})+y^{i}_{laser}* \cos(theta^{r}_{map})\end{array}