# A Particle PHD Filter for Dynamic Grid Map Building towards Indoor Environment
## 3 The Proposed Framework for Building Dynamic Grid Map
### Particle State Updating with Robot Motion  
One of the factors that affects the computing speed of the particle-based PHD filter is the number of grid cells \(N_{grid\_cells}\). Suppose that the length and width of the dynamic grid map are \(map_{size\_x}\), \(map_{size\_y}\), and the grid cell size is _reso_; then: \(N_{grid\_cells}=\frac{map_{size\_x}\cdot map_{size\_y}}{reso\_reso\_reso}\). The size and the speed range of moving objects (such as people) in indoor environments are smaller than those of outdoor moving objects (airplanes and cars). In order to accurately describe the shape and speed of objects, a smaller resolution is needed. At the same time, in order to ensure the running speed of the system, the size of the dynamic map must be appropriate. Therefore, the local dynamic grid map attached to the robot is used for estimation instead of global dynamic grid map estimation, and then, the global velocity of the dynamic object is calculated using the robot's pose. Supposing that the positions of the robot at time \(k\) and \(k+1\) are \((x_{k}^{w},y_{k}^{w},yaw_{k}^{w})\), \((x_{k+1}^{w},y_{k+1}^{w},yaw_{k+1}^{w})\), the particle states are \((i^{x}_{k},i^{y}_{k},i^{y}_{k},i^{y}_{k})\), \((i^{x}_{k+1},i^{y}_{k+1},i^{y}_{k+1},i^{y}_{k+1})\), and the state of the object in the corresponding world coordinate system is \((i^{x}_{k},i^{y}_{k},i^{w}_{k},i^{w}_{k})\), \((i^{x}_{k+1},i^{y}_{k+1},i^{w}_{k+1},i^{w}_{k+1})\), where \(i\in(1,N)\) is the number of particles, as shown in Figure 4 below. As shown in Figure 4, we transform the particle state at time \(k\) to the state at time \(k+1\) as:  
\[R_{k}*^{i}X_{k}^{l}+t_{k}=R_{k+1}*^{i}X_{k+1|k}^{l}+t_{k+1}={}^{i}X_{k}^{w}. \tag{17}\]  
Meanwhile:  
\[R_{k+1}*^{i}X_{k+1}^{l}+t_{k+1}={}^{i}X_{k+1}^{w}. \tag{18}\]  
From Equations (17) and (18), we can obtain:  
\[{}^{i}X_{k+1}^{w}-{}^{i}X_{k}^{w}=R_{k+1}({}^{i}X_{k+1}^{l}-{}^{i}X_{k+1|k}^{l }) \tag{19}\]  
and  
\[{}^{i}X_{k+1|k}^{l}=R_{k+1}^{-1}(R_{k}*^{i}X_{k}^{l}+t_{k}-t_{k+1}). \tag{20}\]  
Figure 4: The relationship between global and local transformation of particle state. Dividing both sides by the time interval \(dt\) between two frames at the same time, it is considered that a dynamic object moves uniformly in the world coordinate system when \(dt\) is small. Then, Equation (19) can be transformed into a speed expression form:  
\[v_{k}^{v}=R_{k}v_{k}^{l}=v_{k+1|k}^{v}=R_{k+1}v_{k+1|k}^{l} \tag{21}\]  
and  
\[v_{k+1|k}^{l}=R_{k+1}^{-1}R_{k}v_{k}^{l}. \tag{22}\]  
From the above, we can find that by converting the information of particles at the time \(k\) into the time \(k+1\) according to Equations (20) and (22), we can complete the estimation from the local dynamic grid map to the global dynamic grid map. The details of the implementation process are as follows. 1. Obtaining the position of robot The transformation from the base_scan (robot) coordinate system to the odom coordinate system can be obtained through the odometry topic published by Gazebo or using TF_Listener, and the transformation of the odom and map coordinate system can be predefined in a roslaunch file in a simulation environment. In the actual positioning process, the pose of the robot can be obtained by slam positioning or Monte Carlo positioning in navigation. 2. Update of particle position information * The scale coordinates of the _i_th_ particle in a local dynamic grid map system of time \(k\) are \((i^{\prime}x_{k}^{l}*reso,i^{\prime}y_{k}^{l}*reso)\). * The scale translational coordinates of the base_scan coordinate system relative to the local dynamic grid map coordinate system are \((map_{size\_x}/2*reso,map_{size\_y}/2*reso)\). * The scale coordinates of the _i_th_ particle in the base_scan coordinate system of time \(k\) are \((i^{\prime}x_{k}^{l}-map_{size\_x}/2,map_{size\_y}/2-i^{\prime}y_{k}^{l})*reso\). * Coordinates of the _i_th_ particle in the world system: \[\left[ \begin{array}{c}x\\ y\end{array} \right]_{w}=\left[