# A Particle PHD Filter for Dynamic Grid Map Building towards Indoor Environment
## 3 The Proposed Framework for Building Dynamic Grid Map
### Motion Model Design  
Considering the space limit of the indoor environment, the speed of the system to estimate the dynamic object must be improved. Dynamic objects not only exist in a state of constant velocity but also in a state of acceleration, deceleration, and turning. The CS (Current Statistical) model has a better performance when tracking maneuvering objects, but when it estimates uniform motion, the error has a large fluctuation. Here, we combine the two to improve its estimation effect in maneuvering and uniform motion. The standard CS model for two-dimensional motion is:  
\[X_{k+1|k}=\phi_{k+1|k}X_{k}+U_{k}\overline{a}_{k}+w(k) \tag{26}\]  
where \(x_{k}=[x_{k},y_{k},\dot{x}_{k},\dot{y}_{k},\ddot{x}_{k},\ddot{y}_{k}]\) is a vector that includes position, velocity, and acceleration information and \(\overline{a}_{k}=[\overline{a}_{x},\overline{a}_{y}]\) is the mean acceleration of the dynamic object. The specific matrices expression of \(\phi_{k+1|k}\) and \(U_{k+1|k}\) can be shown as follows:  
\[\phi_{k+1|k}=\left[ \begin{array}{cccccc}1&0&\Delta t&0&\varphi_{1}&0\\ 0&1&0&\Delta t&0&\varphi_{1}\\ 0&0&1&0&\varphi_{2}&0\\ 0&0&0&1&0&\varphi_{2}\\ 0&0&0&0&\varphi_{3}&0\\ 0&0&0&0&0&\varphi_{3}\end{array} \right] \tag{27}\]  
where:  
\[ \begin{array}{l}\varphi_{1}=(\tau\Delta t-1+e^{-\tau\Delta t})/\tau^{2}\\ \varphi_{2}=(1-e^{-\tau\Delta t})/\tau\\ \varphi_{2}=e^{-\tau\Delta t}\\ u_{1}=(-\tau\Delta t+(\tau\Delta t)^{2}/2+1-e^{-\tau\Delta t})/\tau^{2}\\ u_{2}=(\tau\Delta t-1+e^{-\tau\Delta t})/\tau\\ u_{2}=1-e^{-\tau\Delta t}\end{array}  \tag{28}\]  
where \(\tau\) is the reciprocal of the acceleration time constant and \(\Delta t\) is the time interval between the two samples. The \(w(k)\) is Gaussian white noise, and its variance is:  
\[Q_{k}=2\tau\sigma_{a}^{2}q=2\tau\sigma_{a}^{2}\left[ \begin{array}{cccccc}q_ {11}&0&q_{12}&0&q_{13}&0\\ 0&q_{11}&0&q_{12}&0&q_{13}\\ q_{12}&0&q_{22}&0&q_{23}&0\\ 0&q_{12}&0&q_{22}&0&q_{23}\\ q_{13}&0&q_{23}&0&q_{33}&0\\ 0&q_{13}&0&q_{23}&0&q_{33}\end{array} \right]. \tag{29}\]  
The constant matrix \(q\) is related to \(\Delta t\) and \(\tau\), which can be represented as follows:  
\[ \begin{array}{l}q_{11}=(1-e^{-2\tau\Delta t}+2\tau\Delta t+2(\tau\Delta t)^ {3}/3-2(\tau\Delta t)^{2}-4\tau\Delta te^{-\tau\Delta t})/(2\tau^{5})\\ q_{12}=(1+e^{-2\tau\Delta t}-2e^{-\tau\Delta t}+2\tau\Delta te^{-\tau \Delta t}-2\tau\Delta t+(\tau\Delta t)^{2})/(2\tau^{4})\\ q_{13}=(1-e^{-2\tau\Delta t}-2\tau\Delta te^{-\tau\Delta t})/(2\tau^{3})\\ q_{22}=(4e^{-\tau\Delta t}-3-e^{-2\tau\Delta t}+2\tau\Delta t)/(2\tau^{3})\\ q_{23}=(e^{-2\tau\Delta t}+1-2e^{-\tau\Delta t})/(2\tau^{2})\\ q_{23}=(1-e^{-2\tau\Delta t})/(2\tau)\end{array} . \tag{30}\]  
Here, when \(\overline{a}_{k}\) is zero, the CS model becomes a CV (Constant Velocity) model. In order to reduce the estimation error of the object in a uniform motion, a part of the particles (for example, half of the total number of particles) is set as the CV model in the particle prediction stage. Then, it can achieve a better estimation effect in various motion conditions such as acceleration, deceleration, and uniform motion. At the same time, when using theCV model, the acceleration in its state vector is multiplied by a positive number (less than 1), which can restrain the acceleration and make it more stable in the uniform motion stage. After all particles are predicted by the two motion models, the measurement grid map is used to update the particle weights, and then re-sampling can avoid particle attenuation which, more importantly, can realize the interaction between the two models. Through the state statistics, the current acceleration average value of the dynamic object can be obtained, which is used to update the variance of the current acceleration of the particles:  
\[{(\sigma_{a}^{i})}^{2}=\left\{ \begin{array}{l}{(4-\pi){(a_{\max}-\overline{a }_{k}^{i})}^{2}/\pi,\overline{a}_{k}^{i}>0}\\ {(4-\pi){(-a_{-\max}+\overline{a}_{k}^{i})}^{2}/\pi,\overline{a}_{k}^{i}<0} \end{array} \right.