# A Particle PHD Filter for Dynamic Grid Map Building towards Indoor Environment
## 2 Particle Realization of PHD Filter under Grid Map
### Measurement-Driven Particle PHD Filter Realization Based on Grid Map  
Let the PHD \(D_{k|k}(x|z_{1:k})\) at time \(k\) be represented by a set of \(v\) particles and its weights \(\left\{x_{k}^{i},w_{k}^{i}\right\}_{i=1}^{v}\):  
\[D_{k|k}(x|z_{1:k})\approx\sum_{i=1}^{v}w_{k|k}^{i}\delta_{x_{k}^{i}}(x). \tag{6}\]  
Here, \(\delta_{a}(x)\) is the Dirac delta function, which centers in \(a\). The predicted particles include persistent particles and newborn particles, which are generated by:  
\[x_{k+1|k}^{i}\sim\left\{\begin{array}{c}q_{k+1}(\bullet|x_{k}^{i})\\ b_{k+1}(\bullet|Z_{k+1})\end{array}\right.. \tag{7}\]  
The \(q_{k+1}(\bullet|x_{k}^{i})\) and \(b_{k+1}(\bullet|Z_{k+1})\) are the proposal density of the surviving particles and the newborn particles, respectively. Usually, suppose that the proposal density \(q_{k+1}(\bullet|x_{k}^{i})\) is equal to the transition density \(f_{+}(x_{k+1}|x_{k})\). The sampling provides the two sets of particles, persistent particle set \(\left\{x_{k+1|k,p+}^{i},w_{k+1|k,p+}^{i}\right\}_{i=1}^{L_{p}}\) and newborn particle set \(\left\{x_{k+1|k,p}^{i},w_{k+1|k,b}^{i}\right\}_{i=1}^{L_{b}}\) where persistence particle weights are multiplied with the persistence probability \(w_{k+1|k,p+}^{i}=p_{S}w_{k|k,p}^{i}\) and newborn particle weights are all equal to a constant system preset value. Due to the randomness of the newborn particles, it is often beneficial to consider their influence on the state estimation only after another recursion. So, the predicted PHD should be represented by the persistent set [9; 30]:  
\[D_{k+1|k}(x|z_{1:k+1})=\sum\limits_{i=1}^{L_{p}}w_{k+1|k,p+}^{i}\delta_{x_{k+1|k, p+}^{i}}(x). \tag{8}\]  
The standard PHD update can be represented as:  
\[D_{k+1|k+1}(x|z_{1:k+1})=\sum\limits_{r=1}^{L}w_{k+1|k+1}^{r} \delta(x-x_{k+1|k+1}^{r})\] \[w_{k+1|k+1}^{r}=\sum\limits_{z\in Z}\frac{g_{k}(z|x_{k+1|k}^{r}) w_{k+1|k}^{r}}{\kappa_{k+1}(z)+\sum\limits_{j=1}^{L}\mathcal{R}_{k+1}(z|x_{k+1 |k}^{j})w_{k+1|k}^{j}} \tag{9}\]  
In this paper, the observation is provided by the measurement grid map (with \(n\times m\) resolution cells) calculated through every laser scan. The observations can be represented at time step \(k\) by \(z_{k}=\left\{z_{k}^{i,j}:i=1,\cdots,n,j=1,\cdots,m\right\}\) and all observations up to \(k\) can be represented by \(Z_{1:k}=\left\{z_{t}:t=1,\cdots,k\right\}\). Then, the particle weight update equation can be transformed into [31]:  
\[w_{k+1|k+1}^{r}=\frac{\prod\limits_{i\in C_{i}(x_{k+1|k}^{r})}\prod\limits_{j \in C_{i}(x_{k+1|k}^{r})}g(z_{k}^{i,j}|x_{k+1|k}^{r})}{K_{k}(z)+C_{k}(z_{k}^{r,s})}w_{k+1|k}^{r} \tag{10}\]  
where \(g(z_{k}^{i,j}|x_{k+1|k}^{r})\) is the importance weight, which can be calculated by using the likelihood ratio in cell \((i,j)\) for a target in state \(x_{k+1|k}^{r}\). \(C_{i}(x_{k+1|k}^{r})\) and \(C_{j}(x_{k+1|k}^{r})\) are the sets of subscripts \(i\) and \(j\), indicating the neighboring regions affected by the targets. \(K_{k}(z)\) is a normalization constant to represent the clutter density. Meanwhile, \(C_{k}(z_{k}^{r,s})\) is given by:  
\[C_{k}(z_{k}^{r,s})=\sum\limits_{p\in P^{s}}\prod\limits_{i\in C_{i}(x_{k+1|k}^ {r})}\prod\limits_{j\in C_{i}(x_{k+1|k}^{r})}g(z_{k}^{i,j}|x_{k+1|k}^{r})w_{k+1 |k}^{r}. \tag{11}\]  
After the particle weights are updated, we can resample the new particle set \(\left\{x_{k+1|k+1,p}^{i},w_{k+1|k+1,p}^{i}\right\}_{i=1}^{v}\) from the persistent particle set \(\left\{x_{k+1|k,p+}^{i},w_{k+1|k+1,p+}^{i}\right\}_{i=1}^{L_{p}}\) and the newborn particle set \(\left\{x_{k+1|k,p}^{i},w_{k+1|k+1,p}^{i}\right\}_{i=1}^{L_{p}}\) according to their weight. Where \(v=M_{p}M\), \(w_{k+1|k+1,p}^{i}=\frac{1}{v}(\sum\limits_{i=1}^{L_{p}}w_{k+1|k+1,p+}^{i}+ \sum\limits_{i=1}^{L_{p}}w_{k+1|k+1,p}^{i})\). PHD RFS of multi-target at time \(k+1\) can be represented by:  
\[D_{k+1|k}(x|z_{1:k+1})=\sum\limits_{i=1}^{v}w_{k+1|k+1,p}^{i}\delta_{x_{k+1|k+ 1,p}^{i}}(x). \tag{12}\]