HMM-Based Dynamic Mapping with Gaussian Random Fields ^†

Abstract

This paper focuses on the mapping problem for mobile robots in dynamic environments where the state of every point in space may change, over time, between free or occupied. The dynamical behaviour of a single point is modelled by a Markov chain, which has to be learned from the data collected by the robot. Spatial correlation is based on Gaussian random fields (GRFs), which correlate the Markov chain parameters according to their physical distance. Using this strategy, one point can be learned from its surroundings, and unobserved space can also be learned from nearby observed space. The map is a field of Markov matrices that describe not only the occupancy probabilities (the stationary distribution) as well as the dynamics in every point. The estimation of transition probabilities of the whole space is factorised into two steps: The parameter estimation for training points and the parameter prediction for test points. The parameter estimation in the first step is solved by the expectation maximisation (EM) algorithm. Based on the estimated parameters of training points, the parameters of test points are obtained by the predictive equation in Gaussian processes with noise-free observations. Finally, this method is validated in experimental environments.

1. Introduction

1.1. Literature Review

Dynamic environments are particularly important and complex. These environments include static objects and different kinds of dynamic objects. High dynamic objects, such as moving people, change their position quickly. Low dynamic objects, such as doors and pieces of furniture, can appear and disappear from particular locations however those events are comparatively rare. Autonomous robots should be able to know if objects are static or dynamic to help in path planning.

In earlier research, the environments were assumed to be static. The classical method for static environments is occupancy grid mapping [1,2,3] where maps are divided into a grid and the states of different grid cells are assumed to be independent. In dynamic environments, one popular strategy is to estimate the number of potential targets, their positions, and velocities from sensor data [4,5,6]. The dynamic object detection needs to identify the objects and their correspondence in different time instants. The other one is to apply Markov chains. The dynamic occupancy grids proposed in [7,8,9,10,11,12,13] does not rely on high-level object models. Every grid cell is associated with a Markov chain, where its future occupancy state only depends on the current state. Since the occupancy observations are noisy, the states are not directly observable and the process is modelled instead by hidden Markov models (HMMs) [14] at each point in space. Estimating good parameters in an HMM requires considerable data. If the dependence between different grid cells is taken into account, as is done in [8], maps are built with inconsistencies.

1.2. Research Gap and Motivation

Dynamic object detection requires more powerful sensors [15], such as cameras, while HMM-based methods can be applied with simple distance sensors. Normally, the correlation between parameters in space is not considered in HMM-based methods and inconsistent maps will be produced. In our previous work [16], the inconsistency in discrete space was dealt with by using Markov random fields to regularise the grid. The static mapping methods [17,18,19,20,21,22,23], based on Gaussian Random Fields (GRFs), build smooth occupancy grid maps and predict the occupancy of unobserved space in continuous space. This paper is motivated by them and proposes a new dynamic mapping method based on GRF that is able to deal with continuous space instead of a discrete grid. The state change at every point in continuous space is modelled by a Markov chain with two parameters, and the HMM proposed in [8] is extended with normalised emission probabilities. In one time instant, the map is assumed to be static, and an occupancy grid map is built to obtain the normalised emission probabilities. GRFs are applied to consider the correlation between a point and its neighbouring points. Given the occupancy grid maps, the parameters of every point in the whole space can be estimated. In order to reduce the computation complexity, the parameter estimation is divided into two steps. The first step is to estimate the parameters for training points using the EM algorithm [24,25]. The second step is to predict the parameters for test points using the predictive equation of Gaussian processes with noise-free observations.

1.3. Contribution and Paper Organisation

The main contributions of this paper are highlighted below.

(1)

The extension of the HMM with normalised emission probabilities is developed. Instead of observation models, posterior probability can be used directly to estimate HMM parameters, which is convenient in computation in this paper.

(2)

The HMM parameters of observed space can be smoothed. GRFs are applied to consider the dependence between HMM parameters of different points. The noise in measurements can be filtered and consistent dynamic maps are produced.

(3)

The dynamic behaviour of unobserved space can be predicted. Given the spatial correlation between different points, unobserved space can learn HMM parameters from surrounding observed space and their parameters will be similar.

This paper is organised as follows. The related work is summarised in Section 2. The HMMs with normalised emission probabilities are described in Section 3. The GRF-based methods with known poses and pose uncertainty are proposed in Section 4 and Section 5, respectively. The proposed method is validated in experiments in Section 6.

2. Related Work

In dynamic environments, one point in space may have different states in different time instants, and Markov chains can be applied to model the dynamic behaviour. In [8], the map is divided into grid cells, and every grid cell has two possible states: occupied and free. A Markov chain with two parameters is applied to individually modelled every grid cell. One parameter represents the transition probabilities of state from free to occupied. The other one represents the transition probabilities of state from occupied to free. The states can not be observed certainly and an HMM can be applied. In order to deal with incorrect observations, the underlying possible states are extended and consist of seven components: “true”, “false”, “unknown”, “dynamic”, “falsely false”, “falsely true”, and “falsely true/false” in [13]. Based on the parameters of HMMs, dynamic objects can be classified [9]. The dynamic maps based on HMMs are used to do lifelong localisation tasks [10] and simultaneous localisation and mapping [26,27]. In [11], the dynamic behaviour is modelled as an Input-output Hidden Markov Model (IOHMM) [7,28], where the observations of the neighbouring cells in the previous time step are considered, to take the spatial correlation into account. The input of an IOHMM is the observations of neighbouring cells in the previous time step. In [12], the Explicit-state-Duration Hidden Markov Model (EDHMM) is applied to deal with the Markov chain with variable duration and differentiate the dynamic cells from the static environment.

Gaussian processes, also known as GRFs, can be applied to deal with inconsistency in maps. The advantage is that maps with any resolutions could be built. The Gaussian Process Occupancy Map (GPOM) [17] is an occupancy representation of static environments in continuous space. With the increasing number of training data, the computational complexity of Gaussian processes will also increase. For large-scale environments, training data can be divided into many clusters and a Gaussian process is applied to each subset [18]. Similarly, local Gaussian processes are used to ensure continuity by overlapping clusters [19]. Gaussian processes and Bayesian Committee Machines are applied in [20] to recursively update occupancy maps and surface meshes. The multi-support kernel proposed in [21] enables traditional covariance functions to accept two-dimensional regions, reduces the size of covariance matrices, and accelerates Gaussian process inference and learning. A nested Bayesian committee machine is proposed to learn online 3D occupancy maps using Gaussian processes [22]. Online continuous mapping is proposed to build a map as the zero level set of a Gaussian process implicit surface [23].

3. HMMs for Dynamic Environments

3.1. HMMs

The main difference between static and dynamic environments is the existence of unpredictable dynamic objects. In dynamic environments, one point c in space may be occupied or free in different time instants. Here, its next state $m_c^{t+1}$ is assumed to only depend on the current one $m_c^t$ and a Markov chain is applied to model the dynamic behaviour. The occupied and free states are denoted by $s_1$ and $s_2$, respectively. The Markov chain is shown in Figure 1, where the probability transition matrix for $m_c$ is denoted by ${\mathit{A}}_c=\left\{a_{ij}^c\right\}$ and assumed to be time-invariant.

Defining a grid cell whose central point is c, this point is measured once if one measurement z passes by this grid cell. The Markov chain is a discrete model in time. Between two time instants, the state is assumed to be constant and may be measured multiple times. The measurements for the current state $m_c^t$ are denoted by $z_i^t$ with the same superscript t and different subscripts, and the measurement sequence is denoted by $y_t=(z_1^t,z_2^t,\cdots )$. Due to sensor noise, the states cannot be observed certainly, and an HMM can be applied. The graphical model of an HMM is shown in Figure 2, where $\zeta$ is the number of the measurement sequences. The corresponding emission probabilities are $p(y_t\mid m_c^t)$. Since robots always move in space, the observation sequences for the states in different time instants may be different, and the emission probabilities are also different. However, they are not unknown parameters. Assuming independent observations, the emission probabilities can be derived by:

$$p(y_t\mid m_c^t)=\prod _{i}p(z_i^t\mid m_c^t),$$

(1)

where $p(z_i^t\mid m_c^t)$ can be directly given [8].

The probability of staying in state $s_i$ is $a_{ii}^c$, and the probability of changing state $s_i$ to the others is $1-a_{ii}^c$. The probability of staying in state $s_i$ for d time steps from time step t is:

$$\begin{array}{cc}\hfill p(s_i,d)& =(1-a_{ii}^c){\left(a_{ii}^c\right)}^{d-1}p(m_c^t=s_i),\hfill \end{array}$$

(2)

where $p(m_c^t=s_i)$ is the probability that the state at time t is $s_i$. The overall expected duration [29] is defined by:

$$\begin{array}{cc}\hfill E\left(d\right)& =\sum _{s_i}\sum _{d}dp(s_i,d)\hfill \\ \hfill & =\sum _i\frac{1}{1-a_{ii}^c}p(m_c^t=s_i).\hfill \end{array}$$

(3)

Besides two transition probabilities, the overall expected duration provides an alternative way to analyse the dynamic behaviour. The state distribution (occupied, free) of one point may change with time and the overall expected duration at different times may be different. For convenience, the overall expected duration at a stationary state is applied and given by:

$$E\left(d\right)={\mathcal{G}}_1\frac{1}{1-a_{11}^c}+{\mathcal{G}}_2\frac{1}{1-a_{22}^c},$$

(4)

where ${\mathcal{G}}_1$ and ${\mathcal{G}}_2$ are the occupancy and free probabilities of the stationary distribution, respectively. The stationary distribution can be obtained by solving:

$$\left\{\begin{array}{c}\left[{\mathcal{G}}_1{\mathcal{G}}_2\right]\left[\begin{array}{cc}{a}_{11}^c& 1-a_{11}^c\\ 1-a_{22}^c& a_{22}^c\end{array}\right]=\left[{\mathcal{G}}_1{\mathcal{G}}_2\right],\hfill \\ {\mathcal{G}}_1+{\mathcal{G}}_2=1.\hfill \end{array}\right.$$

(5)

As a result, the probabilities ${\mathcal{G}}_1$ and ${\mathcal{G}}_2$ are given by:

$$\begin{array}{cc}\hfill & {\mathcal{G}}_1=\frac{1-a_{22}^c}{2-a_{11}^c-a_{22}^c},\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill & {\mathcal{G}}_2=\frac{1-a_{11}^c}{2-a_{11}^c-a_{22}^c}.\hfill \end{array}$$

(7)

Given two transition probabilities $a_{11}^c$ and $a_{22}^c$, the overall expected duration can be computed to indicate how dynamic one point is. High dynamic points will have short overall expected duration.

3.2. Parameter Estimation

Since it is not possible to estimate transition probabilities $a_{11}^c$ and $a_{22}^c$ by maximising likelihood directly, the EM algorithm can be applied to estimate the transition probabilities and the initial state probabilities. The parameters are denoted by ${\theta }_c=\{{\rho }_i^c,a_{ij}^c\}$, where ${\rho }_i^c$ represents the initial state probability $p(m_c^0=s_i)$. Assuming an observation sequence is denoted by $O=(y_0,y_1,\cdots ,y_{\zeta -1})$ and an underlying state sequence is denoted by ${\mathcal{M}}_c=(m_c^0,m_c^1,\cdots ,m_c^{\zeta -1})$, the likelihood function of ${\theta }_c$ given the observation sequence and the underlying state sequence is:

$$\begin{array}{cc}\hfill & p(O,{\mathcal{M}}_c\mid {\theta }_c)=\hfill \\ \hfill & p\left(m_c^0\right)p(z^0\mid m_c^0)\prod _{t=1}^{\zeta -1}p(m_c^t\mid m_c^{t-1})p(y_t\mid m_c^t).\hfill \end{array}$$

(8)

However, an observation sequence $y_t$ does not include all of the space. When $m_c^t$ is not observed, the emission probabilities $p(y_t\mid m_c^t)$ are set to 1. In order to estimate the parameters, the EM algorithm is applied to recursively maximise a Q function given by:

$$\begin{array}{cc}\hfill & Q({\theta }_c,{\theta }_c^{\left(k\right)})=\sum _{{\mathcal{M}}_c}p({\mathcal{M}}_c\mid O,{\theta }_c^{\left(k\right)})\log p({\mathcal{M}}_c,O\mid {\theta }_c)\hfill \\ \hfill & =\sum _{{\mathcal{M}}_c}p({\mathcal{M}}_c\mid O,{\theta }_c^{\left(k\right)})\left(\log p(O\mid {\mathcal{M}}_c,{\theta }_c)+\log p({\mathcal{M}}_c\mid {\theta }_c)\right)\hfill \\ \hfill & =\sum _{{\mathcal{M}}_c}p({\mathcal{M}}_c\mid O,{\theta }_c^{\left(k\right)})\log p(O\mid {\mathcal{M}}_c,{\theta }_c)\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill & +\sum _{{\mathcal{M}}_c}p({\mathcal{M}}_c\mid O,{\theta }_c^{\left(k\right)})\log p({\mathcal{M}}_c\mid {\theta }_c).\hfill \end{array}$$

(10)

The sum is over all the possible state sequence ${\mathcal{M}}_c$. Since the observation sequence O is conditionally independent of the parameters ${\theta }_c$ given the state sequence ${\mathcal{M}}_c$, the probability $p(O\mid {\mathcal{M}}_c,{\theta }_c)$ can be rewritten as $p(O\mid {\mathcal{M}}_c)$ and is a constant. As a consequence, the first term in Equation (10) is a constant and the parameters can be estimated by maximising the second term rewritten as:

$$\begin{array}{cc}\hfill & \sum _{{\mathcal{M}}_c}p({\mathcal{M}}_c\mid O,{\theta }_c^{\left(k\right)})\log p({\mathcal{M}}_c\mid {\theta }_c)\hfill \\ \hfill & =\sum _{i=1}^2{\gamma }_i^c\left(0\right)\log {\rho }_i^c+\sum _t\sum _{i=1}^2\sum _{j=1}^2{\xi }_{ij}^c\left(t\right)\log a_{ij}^c\hfill \\ \hfill & =\sum _{i=1}^2{\gamma }_i^c\left(0\right)\log {\rho }_i^c+\sum _t\sum _{i=1}^2{\xi }_{1i}^c\left(t\right)\log a_{1i}^c+\sum _t\sum _{i=1}^2{\xi }_{2i}^c\left(t\right)\log a_{2i}^c\hfill \\ \hfill & =f\left({\rho }_1^c\right)+f\left(a_{11}^c\right)+f\left(a_{22}^c\right),\hfill \end{array}$$

(11)

where three functions $f\left({\rho }_1^c\right)$, $f\left(a_{11}^c\right)$, and $f\left(a_{22}^c\right)$ are defined by:

$$\begin{array}{cc}\hfill f\left({\rho }_1^c\right)& =\sum _{i=1}^2{\gamma }_i^c\left(0\right)\log {\rho }_i^c\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill & ={\gamma }_1^c\left(0\right)\log {\rho }_1^c+{\gamma }_2^c\left(0\right)\log (1-{\rho }_1^c),\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill & f\left(a_{11}^c\right)=\sum _t\sum _{i=1}^2{\xi }_{1i}^c\left(t\right)\log a_{1i}^c\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill & =\sum _t{\xi }_{11}^c\left(t\right)\log a_{11}^c+\sum _t{\xi }_{12}^c\left(t\right)\log (1-a_{11}^c),\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill & f\left(a_{22}^c\right)=\sum _t\sum _{i=1}^2{\xi }_{2i}^c\left(t\right)\log a_{2i}^c\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill & =\sum _t{\xi }_{21}^c\left(t\right)\log (1-a_{22}^c)+\sum _t{\xi }_{22}^c\left(t\right)\log a_{22}^c.\hfill \end{array}$$

(17)

The variable ${\gamma }_i^c\left(t\right)$ represents $p(m_c^t=s_i\mid O,{\theta }_c^{\left(k\right)})$, which is the probability of being in state $s_i$ at time t given the observation sequence O and the parameters ${\theta }_c^{\left(k\right)}$. The variable ${\xi }_{ij}^c\left(t\right)$ represents $p(m_c^t=s_i,m_c^{t+1}=s_j\mid O,{\theta }_c^{\left(k\right)})$ which is the probability of being in state $s_i$ at time t and state $s_j$ at time $t+1$ given the observation sequence O and the parameters ${\theta }_c^{\left(k\right)}$. The three functions contain different parameters and can be maximised individually. Maximising the three functions gives the estimations of the initial state probabilities ${\rho }_i^c$ and the transition probabilities $a_{ii}^c$,

$${\rho }_i^{c(k+1)}={\gamma }_i^c\left(0\right),$$

(18)

$$a_{ii}^{c(k+1)}=\frac{{\sum }_{t=1}^{\zeta -1}{\xi }_{ii}^c\left(t\right)}{{\sum }_{t=1}^{\zeta -1}{\gamma }_i^c\left(t\right)}.$$

(19)

Computing the probabilities ${\gamma }_i^c\left(t\right)$ and ${\xi }_{ij}^c\left(t\right)$ requires temporary variables ${\alpha }_i^c\left(t\right)$ and ${\beta }_i^c\left(t\right)$. The variable ${\alpha }_i^c\left(t\right)=p(y_0,y_1,\cdots ,y_t,m_c^t=s_i\mid \theta )$ is the probability of seeing the $y_0,y_1,\cdots ,y_t$ and being in state $s_i$ at time t. This step, called forward procedure, is computed recursively from time 0 to t as:

$${\alpha }_i^c\left(0\right)={\rho }_{i}p(y_0\mid m_c^t=s_i),$$

(20)

$${\alpha }_j^c(t+1)=p(y_{t+1}\mid m_c^t=s_j)\sum _{i=1}^2{\alpha }_i^c\left(t\right)a_{ij}^c.$$

(21)

The variable ${\beta }_i^c\left(t\right)=p(y_{t+1},\cdots ,y_{\zeta -1}\mid m_c^t=s_i,\theta )$ is the probability of the ending partial sequence $y_{t+1},\cdots ,y_{\zeta -1}$ given starting state $s_i$ at time t. This step, called backward procedure, is calculated from time $\zeta -1$ to t as:

$${\beta }_i^c(\zeta -1)=1,$$

(22)

$${\beta }_i^c\left(t\right)=\sum _{j=1}^2{\beta }_j^c(t+1)a_{ij}^{c}p(y_{t+1}\mid m_c^t=s_j).$$

(23)

According to Bayes rule, the variables ${\gamma }_i\left(t\right)$ and ${\xi }_{ij}\left(t\right)$ are given as:

$${\gamma }_i^c\left(t\right)=\frac{{\alpha }_i^c\left(t\right){\beta }_i^c\left(t\right)}{{\sum }_{j=1}^2{\alpha }_j^c\left(t\right){\beta }_j^c\left(t\right)},$$

(24)

$${\xi }_{ij}^c\left(t\right)=\frac{{\alpha }_i^c\left(t\right)a_{ij}^c{\beta }_j^c(t+1)b_{j{y}_{t+1}}}{{\sum }_{i=1}^2{\sum }_{j=1}^2{\alpha }_i^c\left(t\right)a_{ij}{\beta }_j^c(t+1)b_{j{y}_{t+1}}}.$$

(25)

During one time instant, the map is assumed to be static. Occupancy grid mapping [30] can be applied to build a temporal occupancy map for every time instant. Since the transition matrix is unknown, the state probabilities $p(m_c^t=s_1)$ and $p(m_c^t=s_2)$ are also unknown. All the state probabilities are temporarily set to 0.5 and the posterior probabilities $p(m_c^t=s_1\mid y_t)$ and $p(m_c^t=s_2\mid y_t)$ can be obtained. However, the probabilities $p(y_t\mid m_c^t=s_1)$ and $p(y_t\mid m_c^t=s_2)$ are required for the HMM. Based on Bayes rule, the posterior probability of the occupied state is given by:

$$p(m_c^t=s_1\mid y_t)=\frac{p(y_t\mid m_c^t=s_1)p(m_c^t=s_1)}{p\left(y_t\right)},$$

(26)

where $p\left(y_t\right)$ is a constant and $p(m_c^t=s_1)$ is the occupancy probability. Similarly, the posterior probability of the free state is given by:

$$p(m_c^t=s_2\mid y_t)=\frac{p(y_t\mid m_c^t=s_2)p(m_c^t=s_2)}{p\left(y_t\right)},$$

(27)

where $p(m_c^t=s_2)$ is the free probability. Since the state probabilities $p\left(m_c^t\right)$ are set to 0.5, the probabilities $p(m_c^t=s_1\mid y_t)$ and $p(m_c^t=s_2\mid y_t)$ are the normalised version of $p(y^t\mid m_c^t=s_1)$ and $p(y^t\mid m_c^t=s_2)$. Replaced by $p(m_c^t\mid y_t)$ to compute ${\alpha }_i^c\left(t\right)$ and ${\beta }_i^c\left(t\right)$ give another two temporary variables:

$${\widehat{\alpha }}_i^c\left(t\right)={\eta }_{\alpha }^c\left(t\right){\alpha }_i^c\left(t\right),$$

(28)

$${\widehat{\beta }}_i^c\left(t\right)={\eta }_{\beta }^c\left(t\right){\beta }_i^c\left(t\right),$$

(29)

where ${\eta }_{\alpha }^c\left(t\right)$ and ${\eta }_{\beta }^c\left(t\right)$ are constants. Using these two new variables directly to calculate ${\gamma }_i^c\left(t\right)$ and ${\xi }_{ij}^c\left(t\right)$ as in Equations (24) and (25), the same ${\gamma }_i^c\left(t\right)$ and ${\xi }_{ij}^c\left(t\right)$ can be obtained,

$$\begin{array}{cc}\hfill {\gamma }_i^c\left(t\right)& =\frac{{\widehat{\alpha }}_i^c\left(t\right){\widehat{\beta }}_i^c\left(t\right)}{{\sum }_{j=1}^g{\widehat{\alpha }}_j^c\left(t\right){\widehat{\beta }}_j^c\left(t\right)},\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill {\xi }_{ij}^c\left(t\right)& =\frac{{\widehat{\alpha }}_i^c\left(t\right)a_{ij}^c{\widehat{\beta }}_j^c(t+1)p(m_c^{t+1}=s_j\mid y^{t+1})}{{\sum }_{i=1}^g{\sum }_{j=1}^g{\widehat{\alpha }}_i^c\left(t\right)a_{ij}^c{\widehat{\beta }}_j^c(t+1)p(m_c^{t+1}=s_j\mid y^{t+1})}.\hfill \end{array}$$

(31)

Finally these constants are cancelled. As a result, the probabilities $p(m_c^t\mid y_t)$ can be used to estimate the parameters conveniently instead of $p(y_t\mid m_c^t)$. For the case that one point is not observed during one time instant, setting the corresponding emission probabilities $p(y_t\mid m_c^t)$ to 0.5 also gives the same result.

4. GRF-Based HMM with Known Poses

In the previous section, each point is associated with two HMM parameters: $a_{11}^c$ and $a_{22}^c$. In this section, GRFs are applied to consider the dependence between the parameters of different points, and the two parameters of one point are assumed to be independent. This means there will be two GRFs, one for each parameter. Given some training points in observed space, the parameters of any test point in continuous space can be predicted.

In the previous section, a grid cell is defined in order to obtain the corresponding observation sequence. In this section, the space is divided into grid cells and the central point of each observed grid cell is chosen as a training point. Meanwhile, the test points are chosen arbitrarily. The coordinate set of training points and test points are denoted by $\mathcal{I}$ and ${\mathcal{I}}_{*}$, respectively. The parameters of training points are denoted by $\mathit{A}=({\mathit{a}}_{11},{\mathit{a}}_{22})$, where ${\mathit{a}}_{11}={[\cdots ,a_{11}^c,\cdots ]}^{\mathrm{T}}(c\in \mathcal{I})$ and ${\mathit{a}}_{22}={[\cdots ,a_{22}^c,\cdots ]}^{\mathrm{T}}(c\in \mathcal{I})$. The parameters of test points are denoted by ${\mathit{A}}_{*}=({\mathit{a}}_{11}^{*},{\mathit{a}}_{22}^{*})$, where ${\mathit{a}}_{11}^{*}={[\cdots ,a_{11}^c,\cdots ]}^{\mathrm{T}}(c\in {\mathcal{I}}_{*})$ and ${\mathit{a}}_{22}^{*}={[\cdots ,a_{22}^c,\cdots ]}^{\mathrm{T}}(c\in {\mathcal{I}}_{*})$. In probabilistic form, the parameter estimation of all the selected points including training and test points is:

$$p(\mathit{A},{\mathit{A}}_{*}\mid O).$$

(32)

The problem can be factorised as:

$$p(\mathit{A},{\mathit{A}}_{*}\mid O)=p({\mathit{A}}_{*}\mid \mathit{A})p(\mathit{A}\mid O),$$

(33)

where $p(\mathit{A}\mid O)$ is an HMM parameter estimation problem for training points and $p({\mathit{A}}_{*}\mid \mathit{A})$ is an HMM parameter prediction problem for test points. The parameter estimation for training and test points are done individually.

4.1. HMM Parameter Estimation

Due to the independence between ${\mathit{a}}_{11}$ and ${\mathit{a}}_{22}$, the prior distribution can be factorised as:

$$p\left(\mathit{A}\right)=p\left({\mathit{a}}_{11}\right)p\left({\mathit{a}}_{22}\right),$$

(34)

where $p\left({\mathit{a}}_{11}\right)$ and $p\left({\mathit{a}}_{22}\right)$ are assumed to have the same distribution. The parameter vector ${\mathit{a}}_{11}$ is taken as an example. The log odds form of $a_{11}^c$ is defined as [31]:

$$\begin{array}{c}\hfill l_{a_{11}}^c=\log \frac{a_{11}^c}{1-a_{11}^c}.\end{array}$$

(35)

The vector of all the $l_{a_{11}}^c$ of training points is denoted by ${\mathit{l}}_{a_{11}}={[\cdots ,l_{a_{11}}^c,\cdots ]}^{\mathrm{T}}(c\in \mathcal{I})$ and can be expressed as:

$${\mathit{l}}_{a_{11}}=\log \frac{{\mathit{a}}_{11}}{\mathbf{1}-{\mathit{a}}_{11}},$$

(36)

where $\mathbf{1}$ is a column of ones and the division is elementwise. The vector ${\mathit{l}}_{a_{11}}$ is assumed to be Gaussian distributed with mean vector ${\mathbf{\mu }}_1$ and covariance matrix ${\mathit{K}}_{\mathcal{I}\mathcal{I}}$,

$$\begin{array}{c}\hfill {\mathit{l}}_{a_{11}}\sim \mathcal{N}({\mathbf{\mu }}_1,{\mathit{K}}_{\mathcal{I}\mathcal{I}}).\end{array}$$

(37)

The covariance function is the Ornstein–Uhlenbeck kernel function as:

$$\mathcal{C}(c,c^{\prime })={\sigma }_f^2\exp \left(-\frac{|c-c^{\prime }|}{\ell }\right),$$

(38)

where ${\sigma }_f^2$ is the signal variance, the parameter ℓ is the length-scale, and the variables c and $c^{\prime }$ are the corresponding coordinates of two random variables. The prior distribution $p\left(a_{11}\right)$ is:

$$p\left({\mathit{a}}_{11}\right)=\frac{1}{\sqrt[]{{\left(2\pi \right)}^n\left|{\mathit{K}}_{\mathcal{I}\mathcal{I}}\right|}}\exp \left(-\frac{1}{2}U\left({\mathit{a}}_{11}\right)\right),$$

(39)

where n is the number of training points and:

$$U\left({\mathit{a}}_{11}\right)={(\log \frac{{\mathit{a}}_{11}}{\mathbf{1}-{\mathit{a}}_{11}}-{\mathbf{\mu }}_1)}^{\mathrm{T}}{\mathit{K}}_{\mathcal{I}\mathcal{I}}^{-1}(\log \frac{{\mathit{a}}_{11}}{\mathbf{1}-{\mathit{a}}_{11}}-{\mathbf{\mu }}_1).$$

(40)

Similarly, the log odds form of $a_{22}^c$ is defined as:

$$\begin{array}{c}\hfill l_{a_{22}}^c=\log \frac{a_{22}^c}{1-a_{22}^c}.\end{array}$$

(41)

The vector of all the $l_{a_{22}}^c$ of training points is denoted by ${\mathit{l}}_{a_{22}}={[\cdots ,l_{a_{22}}^c,\cdots ]}^{\mathrm{T}}(c\in \mathcal{I})$ and also assumed to be Gaussian distributed. The prior distribution $p\left({\mathit{a}}_{22}\right)$ is given in the same way by:

$$p\left({\mathit{a}}_{22}\right)=\frac{1}{\sqrt[]{{\left(2\pi \right)}^n\left|{\mathit{K}}_{\mathcal{I}\mathcal{I}}\right|}}\exp \left(-\frac{1}{2}U\left({\mathit{a}}_{22}\right)\right),$$

(42)

where

$$U\left({\mathit{a}}_{22}\right)={(\log \frac{{\mathit{a}}_{22}}{\mathbf{1}-{\mathit{a}}_{22}}-{\mathbf{\mu }}_2)}^{\mathrm{T}}{\mathit{K}}_{\mathcal{I}\mathcal{I}}^{-1}(\log \frac{{\mathit{a}}_{22}}{\mathbf{1}-{\mathit{a}}_{22}}-{\mathbf{\mu }}_2),$$

(43)

and ${\mathbf{\mu }}_2$ is the mean vector of ${\mathit{l}}_{a_{22}}$.

During one time instant t, the map that consists of the states of training points is denoted by ${\mathcal{M}}^t=\{\cdots ,m_c^t,\cdots \}(c\in \mathcal{I})$ and the states of different training points are assumed to be independent. The distribution of the map is:

$$p\left({\mathcal{M}}^t\right)=\prod _{c\in \mathcal{I}}p\left(m_c^t\right).$$

(44)

A map sequence consists of different maps ${\mathcal{M}}^t$ in time and is denoted by $\mathcal{M}=\{{\mathcal{M}}^0,\cdots ,{\mathcal{M}}^t,\cdots \}$. As shown in Figure 3, the map sequence depends on space and time. Meanwhile, the map sequence also consists of different state sequences ${\mathcal{M}}_c(c\in \mathcal{I})$ in space and can also be expressed as $\mathcal{M}=\{\cdots ,{\mathcal{M}}_c,\cdots \}(c\in \mathcal{I})$. Due to the state independence between different points, the current state of one point only depends on its previous state. The corresponding distribution is computed by:

$$p\left(\mathcal{M}\right)=\prod _{c\in \mathcal{I}}p\left({\mathcal{M}}_c\right).$$

(45)

The likelihood of $\mathit{A}$ given the observation sequence O and the underlying map sequence $\mathcal{M}$ is:

$$\begin{array}{cc}\hfill p(O,\mathcal{M}\mid \mathit{A})& =p(O\mid \mathcal{M},\mathit{A})p(\mathcal{M}\mid \mathit{A})\hfill \end{array}$$

(46)

$$\begin{array}{cc}\hfill & =p(O\mid \mathcal{M})p(\mathcal{M}\mid \mathit{A}).\hfill \end{array}$$

(47)

Given the map sequence $\mathcal{M}$, the observation sequence O is conditionally independent of the parameters $\mathit{A}$. Assuming the measurements are independent of each other, the probability $p(O\mid \mathcal{M})$ can be obtained by:

$$p(O\mid \mathcal{M})=\prod _{t}p(y_t\mid {\mathcal{M}}^t).$$

(48)

The measurement sequence $y_t$ only depends on the map configuration ${\mathcal{M}}^t$ in the same time instant t. The probability $p(y_t\mid {\mathcal{M}}^t)$ can be derived from the sensor model,

$$p(y_t\mid {\mathcal{M}}^t)=\prod _{i}p(z_t^i\mid {\mathcal{M}}^t).$$

(49)

Due to the state dependence between different points, the state sequence ${\mathcal{M}}_c$ only depends on the corresponding parameter $A_c$ at the same coordinate c and $p(\mathcal{M}\mid \mathit{A})$ can be factorised as:

$$p(\mathcal{M}\mid \mathit{A})=\prod _{c\in \mathcal{I}}p({\mathcal{M}}_c\mid A_c).$$

(50)

The likelihood can be given by:

$$p(O\mid \mathit{A})=\sum _{\mathcal{M}}p(O,\mathcal{M}\mid \mathit{A}).$$

(51)

The observation sequence is also conditional on the initial map distribution $p\left({\mathcal{M}}^0\right)$, which requires the initial state probabilities ${\rho }_i^c=p(m_c^0=s_i)$ of training points as in Equation (44). For convenience, the initial probabilities are not written together with $\mathit{A}$.

Based on Bayes rule, the posterior distribution is:

$$p(\mathit{A}\mid O)=\frac{p(O\mid \mathit{A})p\left(\mathit{A}\right)}{p\left(O\right)},$$

(52)

where $p\left(O\right)$ is a normalising constant.

Similar to the HMM problem in the previous section, the EM algorithm is also applied to estimate the parameters. The Q function with the prior distribution [32] is given as:

$$\begin{array}{cc}\hfill & Q(\mathit{A},{\mathit{A}}^{\left(k\right)})=E_{\mathcal{M}\mid O,{\mathit{A}}^{\left(k\right)}}\log p(\mathcal{M},O\mid \mathit{A})+\log p\left(\mathit{A}\right)\hfill \\ \hfill & =E_{\mathcal{M}\mid O,{\mathit{A}}^{\left(k\right)}}\log p(O\mid \mathcal{M},\mathit{A})+E_{\mathcal{M}\mid O,{\mathit{A}}^{\left(k\right)}}\log p(\mathcal{M}\mid \mathit{A})\hfill \\ \hfill & -2\log \left(\sqrt[]{{\left(2\pi \right)}^n\left|K_{\mathcal{I}\mathcal{I}}\right|}\right)-\frac{1}{2}U\left({\mathit{a}}_{11}\right)-\frac{1}{2}U\left({\mathit{a}}_{22}\right),\hfill \end{array}$$

(53)

where ${\mathit{A}}^{\left(k\right)}$ represents the parameters obtained in iteration k. Since the observation sequence O is conditionally independent of the parameters given the map sequence $\mathcal{M}$, the probabilities $p(O\mid \mathcal{M},\mathit{A})$ can be rewritten as $p(O\mid \mathcal{M})$ and is a constant. The normalizer, Z, is also a constant. As a result, the parameters can be obtained by maximising the non-constant terms. The second term is rewritten as:

$$\begin{array}{cc}\hfill & E_{\mathcal{M}\mid O,{\mathit{A}}^{\left(k\right)}}\log p(\mathcal{M}\mid \mathit{A})\hfill \\ \hfill & =\sum _{\mathcal{M}}p(\mathcal{M}\mid O,{\mathit{A}}^{\left(k\right)})\sum _{c\in \mathcal{I}}\log p({\mathcal{M}}_c\mid A_c)\hfill \\ \hfill & =\sum _{c\in \mathcal{I}}\sum _{\mathcal{M}}p(\mathcal{M}\mid O,{\mathit{A}}^{\left(k\right)})\log p({\mathcal{M}}_c\mid A_c)\hfill \\ \hfill & =\sum _{c\in \mathcal{I}}\sum _{{\mathcal{M}}_c}p({\mathcal{M}}_c\mid O,A_c^{\left(k\right)})\log p({\mathcal{M}}_c\mid A_c).\hfill \end{array}$$

(54)

Since the initial state probabilities ${\rho }_i^c$ are not written together with $A_c$, the probability $p({\mathcal{M}}_c\mid O,A_c^{\left(k\right)})$ is the same as $p({\mathcal{M}}_c\mid O,{\theta }_c^{\left(k\right)})$ in Equation (11), where ${\theta }_c$ includes $A_c$ and the initial state probabilities ${\rho }_i^c$. The non-constant terms in this Q function is rewritten as:

$$\begin{array}{cc}\hfill & E_{\mathcal{M}\mid O,{\mathit{A}}^{\left(k\right)}}\log p(\mathcal{M}\mid \mathit{A})-\frac{1}{2}U\left({\mathit{a}}_{11}\right)-\frac{1}{2}U\left({\mathit{a}}_{22}\right)\hfill \\ \hfill & =\sum _{c\in \mathcal{I}}f\left({\rho }_1^c\right)+\sum _{c\in \mathcal{I}}f\left(a_{11}^c\right)+\sum _{c\in \mathcal{I}}f\left(a_{22}^c\right)\hfill \\ \hfill & -\frac{1}{2}U\left({\mathit{a}}_{11}\right)-\frac{1}{2}U\left({\mathit{a}}_{22}\right)\hfill \\ \hfill & =f\left({\mathbf{\rho }}_1\right)+f\left({\mathit{a}}_{11}\right)+f\left({\mathit{a}}_{22}\right),\hfill \end{array}$$

(55)

where ${\mathbf{\rho }}_1$ is defined by ${\mathbf{\rho }}_1={[\cdots ,{\rho }_1^c,\cdots ]}^{\mathrm{T}}$ which is the vector of the initial occupancy probabilities ${\rho }_1^c$ of observed grid cells. The function $f\left({\mathbf{\rho }}_1\right)$, $f\left({\mathit{a}}_{11}\right)$, and $f\left({\mathit{a}}_{22}\right)$ are the vector versions of the ones defined in Equations (13), (15) and (17), defined by:

$$\begin{array}{cc}\hfill f\left({\mathbf{\rho }}_1\right)& =\sum _{c\in \mathcal{I}}f\left({\rho }_1^c\right)\hfill \\ \hfill & ={\mathbf{\gamma }}_1\log {\mathbf{\rho }}_1+{\mathbf{\gamma }}_2\log (\mathbf{1}-{\mathbf{\rho }}_1),\hfill \end{array}$$

(56)

$$\begin{array}{cc}\hfill & f\left({\mathit{a}}_{11}\right)=\sum _{c\in \mathcal{I}}f\left(a_{11}^c\right)-\frac{1}{2}U\left({\mathit{a}}_{11}\right)\hfill \\ \hfill & ={\mathbf{\xi }}_{11}\log {\mathit{a}}_{11}+{\mathbf{\xi }}_{12}\log (\mathbf{1}-{\mathit{a}}_{11})-\frac{1}{2}U\left({\mathit{a}}_{11}\right),\hfill \end{array}$$

(57)

$$\begin{array}{cc}\hfill & f\left({\mathit{a}}_{22}\right)=\sum _{c\in \mathcal{I}}f\left(a_{22}^c\right)-\frac{1}{2}U\left({\mathit{a}}_{22}\right)\hfill \\ \hfill & ={\mathbf{\xi }}_{22}\log {\mathit{a}}_{22}+{\mathbf{\xi }}_{21}\log (\mathbf{1}-{\mathit{a}}_{22})-\frac{1}{2}U\left({\mathit{a}}_{22}\right),\hfill \end{array}$$

(58)

where ${\mathbf{\gamma }}_i=[\cdots ,{\gamma }_i^c\left(0\right),\cdots ](c\in \mathcal{I})$ and ${\mathbf{\xi }}_{ij}=[\cdots ,{\sum }_t{\xi }_{ij}^c\left(t\right),\cdots ](c\in \mathcal{I})$. The three functions can also be maximised individually. The derivatives of $Q(\mathit{A},{\mathit{A}}^{\left(k\right)})$ with respect to ${\mathbf{\rho }}_1$, ${\mathit{a}}_{11}$, and ${\mathit{a}}_{22}$ are, respectively, given by:

$$\begin{array}{c}\hfill \frac{\mathrm{d}}{\mathrm{d}{\mathbf{\rho }}_1}f\left({\mathbf{\rho }}_1\right)={\mathbf{\gamma }}_1^{\mathrm{T}}\oslash {\mathbf{\rho }}_1-{\mathbf{\gamma }}_2^{\mathrm{T}}\oslash (\mathbf{1}-{\mathbf{\rho }}_1),\end{array}$$

(59)

$$\begin{array}{cc}\hfill & \frac{\mathrm{d}}{\mathrm{d}{\mathit{a}}_{11}}f\left({\mathit{a}}_{11}\right)={\mathbf{\xi }}_{11}^{\mathrm{T}}\oslash {\mathit{a}}_{11}-{\mathbf{\xi }}_{12}^{\mathrm{T}}\oslash (\mathbf{1}-{\mathit{a}}_{11})\hfill \\ \hfill & -K_{\mathcal{I}\mathcal{I}}^{-1}(\log \frac{{\mathit{a}}_{11}}{\mathbf{1}-{\mathit{a}}_{11}}-{\mathbf{\mu }}_1)\odot \frac{1}{{\mathit{a}}_{11}\odot (\mathbf{1}-{\mathit{a}}_{11})},\hfill \end{array}$$

(60)

$$\begin{array}{cc}\hfill & \frac{\mathrm{d}}{\mathrm{d}{\mathit{a}}_{22}}f\left({\mathit{a}}_{22}\right)={\mathbf{\xi }}_{22}^{\mathrm{T}}\oslash {\mathit{a}}_{22}-{\mathbf{\xi }}_{21}^{\mathrm{T}}\oslash (\mathbf{1}-{\mathit{a}}_{22})\hfill \\ \hfill & -K_{\mathcal{I}\mathcal{I}}^{-1}(\log \frac{{\mathit{a}}_{22}}{\mathbf{1}-{\mathit{a}}_{22}}-{\mathbf{\mu }}_2)\odot \frac{1}{{\mathit{a}}_{22}\odot (\mathbf{1}-{\mathit{a}}_{22})},\hfill \end{array}$$

(61)

where ⊘ is the elementwise division and ⊙ is Hadamard product. Without prior knowledge, the parameter $f\left({\mathbf{\rho }}_1\right)$ can be maximised directly and the estimation of ${\mathbf{\rho }}_{\mathbf{1}}$ is:

$${\mathbf{\rho }}_{\mathbf{1}}={\mathbf{\gamma }}_1^{\mathrm{T}}.$$

(62)

With the prior $p\left({\mathit{a}}_{11}\right)$ and $p\left({\mathit{a}}_{22}\right)$, it is not easy to maximise $f\left({\mathit{a}}_{11}\right)$ and $f\left({\mathit{a}}_{22}\right)$, or equivalently to minimise $-f\left({\mathit{a}}_{11}\right)$ and $-f\left({\mathit{a}}_{22}\right)$. The line search method (LSM) [33] is used to estimate ${\mathit{a}}_{11}$ and ${\mathit{a}}_{22}$ in range (0,1). The LSM is a gradient-based method and searches for the optimum of the objective function from the initial values of parameters iteratively. Since this estimation is only one step in the whole optimisation process and only the first iterations are numerically relevant, the LSM can be stopped before convergence in order to achieve computationally efficiency.

4.2. HMM Parameter Prediction

The EM algorithm in the previous section does not give the variances of the parameters ${\mathit{a}}_{11}$ and ${\mathit{a}}_{22}$. After the HMM parameter estimation, all the noise in the observations are assumed to be filtered out and the estimates of ${\mathit{a}}_{11}$ and ${\mathit{a}}_{22}$ are assumed to be without noise. Due to the independence between $a_{11}^c$ and $a_{22}^c$, the prediction problem can be divided as:

$$p({\mathit{A}}_{*}\mid \mathit{A})=p({\mathit{a}}_{11}^{*}\mid {\mathit{a}}_{11})p({\mathit{a}}_{22}^{*}\mid {\mathit{a}}_{22}).$$

(63)

Assuming the log odds forms of the parameter vectors ${\mathit{a}}_{11}^{*}$ and ${\mathit{a}}_{22}^{*}$ for test points are denoted by ${\mathit{l}}_{a_{11}}^{*}={[\cdots ,l_{a_{11}}^c,\cdots ]}^{\mathrm{T}}(c\in {\mathcal{I}}_{*})$ and ${\mathit{l}}_{a_{22}}^{*}={[\cdots ,l_{a_{22}}^c,\cdots ]}^{\mathrm{T}}(c\in {\mathcal{I}}_{*})$, respectively. The distributions $p({\mathit{a}}_{11}^{*}\mid {\mathit{a}}_{11})$ and $p({\mathit{a}}_{22}^{*}\mid {\mathit{a}}_{22})$ can be derived from $p({\mathit{l}}_{a_{11}}^{*}\mid {\mathit{l}}_{a_{11}})$ and $p({\mathit{l}}_{a_{22}}^{*}\mid {\mathit{l}}_{a_{22}})$, respectively. The joint distribution of ${\mathit{l}}_{a_{22}}$ and ${\mathit{l}}_{a_{22}}^{*}$ is:

$$\left[\begin{array}{c}{\mathit{l}}_{a_{11}}\\ {\mathit{l}}_{a_{11}}^{*}\end{array}\right]\sim \mathcal{N}\left(\left[\begin{array}{c}{\mathbf{\mu }}_1\\ {\mathbf{\mu }}_1^{*}\end{array}\right],\left[\begin{array}{cc}{\mathit{K}}_{\mathcal{I}\mathcal{I}}& {\mathit{K}}_{\mathcal{I}*}^{\mathrm{T}}\\ {\mathit{K}}_{\mathcal{I}*}& {\mathit{K}}_{**}\end{array}\right]\right),$$

(64)

where ${\mathbf{\mu }}_1^{*}$ is the mean vector of ${\mathit{l}}_{a_{11}}^{*}$, the matrix ${\mathit{K}}_{\mathcal{I}*}$ denotes the covariance matrix between ${\mathit{l}}_{a_{11}}$ and ${\mathit{l}}_{a_{11}}^{*}$, and ${\mathit{K}}_{**}$ is the covariance matrix of ${\mathit{l}}_{a_{11}}^{*}$. The predictive equation with noise-free observations [34] is:

$$\begin{array}{c}\hfill {\mathit{l}}_{a_{11}}^{*}\mid {\mathit{l}}_{a_{11}}\sim \mathcal{N}({\overline{\mathit{l}}}_{a_{11}}^{*},{\widehat{\mathit{K}}}_a^{*}),\end{array}$$

(65)

where the predictive mean vector ${\overline{\mathit{l}}}_{a_{11}}^{*}$ and covariance matrix ${\widehat{\mathit{K}}}_a^{*}$ are given as:

$$\begin{array}{c}\hfill {\overline{\mathit{l}}}_{a_{11}}^{*}={\mathbf{\mu }}_1^{*}+{\mathit{K}}_{\mathcal{I}*}{\mathit{K}}_{\mathcal{I}\mathcal{I}}^{-1}({\mathit{l}}_{a_{11}}-{\mathbf{\mu }}_1),\end{array}$$

(66)

$$\begin{array}{c}\hfill {\widehat{\mathit{K}}}_a^{*}={\mathit{K}}_{**}-{\mathit{K}}_{\mathcal{I}*}{\mathit{K}}_{\mathcal{I}\mathcal{I}}^{-1}{\mathit{K}}_{\mathcal{I}*}^{\mathrm{T}}.\end{array}$$

(67)

The best predictive parameter vector ${\mathit{a}}_{11}^{*}$ of test points is given by the logistic function:

$$\begin{array}{c}\hfill {\mathit{a}}_{11}^{*}=\frac{\mathbf{1}}{\mathbf{1}+\exp \left(-{\overline{\mathit{l}}}_{a_{11}}^{*}\right)},\end{array}$$

(68)

where the division is elementwise. Similarly, the joint distribution of ${\mathit{l}}_{a_{11}}$ and ${\mathit{l}}_{a_{11}}^{*}$ is:

$$\left[\begin{array}{c}{\mathit{l}}_{a_{22}}\\ {\mathit{l}}_{a_{22}}^{*}\end{array}\right]\sim \mathcal{N}\left(\left[\begin{array}{c}{\mathbf{\mu }}_2\\ {\mathbf{\mu }}_2^{*}\end{array}\right],\left[\begin{array}{cc}{\mathit{K}}_{\mathcal{I}\mathcal{I}}& {\mathit{K}}_{\mathcal{I}*}^{\mathrm{T}}\\ {\mathit{K}}_{\mathcal{I}*}& {\mathit{K}}_{**}\end{array}\right]\right),$$

(69)

where ${\mathbf{\mu }}_2^{*}$ is the mean vector of ${\mathit{l}}_{a_{22}}^{*}$. The coordinates of training points and test points for two prediction problems are the same. As a result, the covariance matrix of the joint distribution and the predictive covariance matrix do not change. The predictive equation of ${\mathit{l}}_{a_{22}}^{*}$ is:

$$\begin{array}{c}\hfill {\mathit{l}}_{a_{22}}^{*}\mid {\mathit{l}}_{a_{22}}\sim \mathcal{N}({\overline{\mathit{l}}}_{a_{22}}^{*},{\widehat{\mathit{K}}}_a^{*}),\end{array}$$

(70)

where the predictive mean vector ${\overline{\mathit{l}}}_{a_{22}}^{*}$ is:

$${\overline{\mathit{l}}}_{a_{22}}^{*}={\mathbf{\mu }}_2^{*}+{\mathit{K}}_{\mathcal{I}*}{\mathit{K}}_{\mathcal{I}\mathcal{I}}^{-1}({\mathit{l}}_{a_{22}}-{\mathbf{\mu }}_2).$$

(71)

The best predictive parameter vector ${\mathit{a}}_{22}^{*}$ of test points is:

$$\begin{array}{c}\hfill {\mathit{a}}_{22}^{*}=\frac{\mathbf{1}}{\mathbf{1}+\exp \left(-{\overline{\mathit{l}}}_{a_{22}}^{*}\right)}.\end{array}$$

(72)

5. GRF-Based HMM with Pose Uncertainty

The mapping method in the previous section computes the posterior distribution from a prior term and a likelihood term assuming that the precise position of the robot is known. When the uncertainty of robot poses is considered, the problem is how to incorporate that uncertainty into the two terms. The prior distributions depend on the relative positions between different points. However, the chosen points in observed space are the central points of observed grid cells. The points in unobserved space are chosen arbitrarily. As a result, the prior term does not depend on robot poses. Without pose uncertainty, the likelihood term is derived directly from the measurement model $p(z_i\mid m)$, where m denotes the whole map. When the robot pose ${\mathcal{P}}_k$ at the time step k is uncertain, the measurement model is $p(z_i\mid m,{\mathcal{P}}_k)$. Based on the law of total probability, the uncertainty of the robot pose can be incorporated into the sensor uncertainty by:

$$p(z_i\mid m)=\int p(z_i\mid m,{\mathcal{P}}_k)p\left({\mathcal{P}}_k\right)\mathrm{d}{\mathcal{P}}_k.$$

(73)

In this work, the map is divided into grid cells to obtain observation sequences, however it is not easy to integrate the probability in each grid cell. This problem can be solved by sampling $n_s$ points from the distribution $p({\mathcal{P}}_k\mid {\mathcal{Z}}_0,{\mathcal{Z}}_{\zeta })$ and averaging over all the pose samples. Assuming the samples denoted by ${\mathcal{P}}_k^i$ with associated weights $w^i$, Equation (73) can be rewritten as:

$$p(z_i\mid m)=\sum _{i=1}^{n_s}{w}^{i}p(z_i\mid m,{\mathcal{P}}_k^i).$$

(74)

Assuming m denotes a whole grid map and the state of one grid cell is denoted by $m_c$, the probability $p(z_i\mid m_c)$ can be obtained by:

$$\begin{array}{cc}\hfill p(z_i\mid m_c)& =\sum _{m\setminus m_c}p(z_i\mid m)p(m\setminus m_c\mid m_c)\hfill \\ \hfill & =\sum _{m\setminus m_c}\sum _{i=1}^{n_s}{w}^{i}p(z_i\mid m,{\mathcal{P}}_k^i)p(m\setminus m_c\mid m_c)\hfill \\ \hfill & =\sum _{i=1}^{n_s}\sum _{m\setminus m_c}{w}^{i}p(z_i\mid m,{\mathcal{P}}_k^i)p(m\setminus m_c\mid m_c)\hfill \\ \hfill & =\sum _{i=1}^{n_s}{w}^{i}p(z_i\mid m_c,{\mathcal{P}}_k^i),\hfill \end{array}$$

(75)

where $m\setminus m_c$ means the whole map m without $m_c$. After the uncertainty of robot poses are incorporated, the proposed method in the previous section can be implemented as usual.

6. Experiments

6.1. Experimental Setup

The experimental platform consists of two parts: The robot part and the PC part. On the robot part, a 3pi robot and one XBee communication module are connected to an mbed expansion board, two IR sensors are connected to the mbed. On the PC part, one XBee module is connected to the PC by an XBee Explorer USB. The 3pi robot is controlled by the mbed microprocessor which sends commands to the robot by a pair of serial ports. The IR sensors 1 and 2 are two Sharp GP2Y0A41SK0F IR sensors, which can measure distances to objects and generate an analog voltage signal. The mbed samples the analog voltages of the IR sensors by two ADC ports. The voltage data can be sent to the PC by XBees 1 and 2, which are XBee S1 802.15.4 low-power modules. The mbed sends data to XBee 1 by another pair of serial ports. XBee 2 receives the data from XBee 1 and sends it to the PC. Since the mbed processor has limited computational power, the PC is in charge of the mapping tasks.

6.2. Experimental Environments

To illustrate the algorithm, the experimental map is shown in Figure 4. There are some objects with different shapes and sizes. The coordinates of the map are shown in Figure 5. The objects with labels 1, 3, 4, 6, 8, and 9 appear and disappear from their positions with different frequencies. The object 1 changes its state at every loop and the subsequent dynamic objects change their states every 2, 5, 10, 20, and 50 loops, respectively. The objects 2, 5, and 7 are static.

A 3pi robot equipped with two Sharp GP2Y0A41SK0F IR sensors is used to test the proposed method. Its diameter is 9.5 cm and the width W between two wheels is 8.2 cm. Two IR sensors are mounted on the robot as shown in Figure 6, and the relative orientations are $\pm {30}^{\circ }$ with respect to the robot’s reference frame. The measuring range is 4 to 30 cm. When the distances are more than 20 cm, the output voltage has lower sensitivity and becomes noisier. In the experiments, the maximum distances of IR sensors are set to 20 cm. Since the aperture angle is very small, the two IR sensors do not interface with each other.

6.3. Pose Uncertainty

As the robot explores the environment, its pose uncertainty will increase. A track with a mark is drawn in Figure 4 to decrease the pose uncertainty. Five QTR-RC reflectance sensors in the front of the robot are used to follow the track and detect the mark, and a simple controller is designed to help the robot follow the track. As the robot moves, its pose is predicted by its motion model while the corresponding uncertainty increases. When the robot detects the mark again, the robot closes its trajectory, and all the poses are corrected.

6.3.1. Robot Motion Model

The robot position is represented by the central point between two wheels in world coordinates, and its orientation is relative to the x axis. Its pose vector is denoted by $\mathcal{P}={[x,y,\varphi ]}^{\mathrm{T}}$, which includes the position ($x,y$) and the orientation $\varphi$. Assuming the speeds of the left and right wheels are denoted by $v_l$ and $v_r$, the speed of the robot is modelled as:

$$\left\{\begin{array}{c}\frac{\mathrm{d}}{\mathrm{d}t}x=\frac{v_l+v_r}{2}\cos \varphi \hfill \\ \frac{\mathrm{d}}{\mathrm{d}t}y=\frac{v_l+v_r}{2}\sin \varphi \hfill \\ \frac{\mathrm{d}}{\mathrm{d}t}\varphi =\frac{v_r-v_l}{W}.\hfill \end{array}\right.$$

(76)

When the robot goes straight shown in Figure 7, its orientation does not change. Assuming the pose at time step $k-1$ is denoted by ${\mathcal{P}}_{k-1}=(x_{k-1},y_{k-1},{\varphi }_{k-1})$, the next pose ${\mathcal{P}}_k$ after a time interval $\Delta t$ can be computed based on the Euler method,

$$\left\{\begin{array}{c}{x}_k=x_{k-1}+\frac{v_l+v_r}{2}\Delta t\cos {\varphi }_{k-1}\hfill \\ y_k=y_{k-1}+\frac{v_l+v_r}{2}\Delta t\sin {\varphi }_{k-1}\hfill \\ {\varphi }_k={\varphi }_{k-1}.\hfill \end{array}\right.$$

(77)

When the robot does not move straight shown in Figure 8, direct integration is performed instead of the Euler approximation. Integrating Equation (76) in a time interval $\Delta t$ gives the new pose [31]:

$$\left\{\begin{array}{c}{x}_k=x_{k-1}+\frac{W(v_l+v_r)}{2(v_r-v_l)}(\sin ({\varphi }_{k-1}+\frac{v_r-v_l}{W})-\sin {\varphi }_{k-1})\hfill \\ y_k=y_{k-1}+\frac{W(v_l+v_r)}{2(v_r-v_l)}(-\cos ({\varphi }_{k-1}+\frac{v_r-v_l}{W})+\cos {\varphi }_{k-1})\hfill \\ {\varphi }_k={\varphi }_{k-1}+\frac{v_r-v_l}{W}\Delta t.\hfill \end{array}\right.$$

(78)

The system model with additive process noise $\mathcal{W}$ is rewritten as:

$${\mathcal{P}}_k=F({\mathcal{P}}_{k-1},v_r,v_l)+\mathcal{W},$$

(79)

where $\mathcal{W}$ is Gaussian distributed with zero mean and covariance matrix $\mathcal{R}$. The function $F({\mathcal{P}}_{k-1},v_r,v_l)$ represents the expressions on the right sides of Equations (77) and (78).

6.3.2. Robot Measurement Model

The robot follows the track in a clockwise direction. Once the robot detects the mark, there will be one observation of the position of the robot. The part of the track in front of the mark is straight. When the robot arrives at the mark, the simple controller can adjust the orientation of the robot to be close to $-\pi$. As a result, the position and the orientation become known, although not precisely. However, the observation is not precise. The noisy observation is modelled as:

$${\mathit{Z}}_k={\mathcal{P}}_k+{[-4.75,0,0]}^{\mathrm{T}}+\mathcal{V},$$

(80)

where $\mathcal{V}$ is Gaussian noise with zero mean and covariance matrix $\mathcal{Q}$. The measurement ${\mathit{Z}}_k$ is the pose of the robot head where the reflectance sensors are located and ${\mathcal{P}}_k$ is the pose of the central point of the robot. The pose difference between the head and the central point is ${[-4.75,0,0]}^{\mathrm{T}}$ when the mark is detected.

6.3.3. Pose Smoothing

When the robot starts at the mark and performs a complete loop returning to the mark, only two observations of the pose are available, as shown in Figure 9. The two observations are denoted by ${\mathcal{Z}}_0$ and ${\mathcal{Z}}_{\zeta }$, respectively. Assuming the position of the mark is at coordinates ($x_0$, $y_0$), both observations ${\mathcal{Z}}_0$ and ${\mathcal{Z}}_{\zeta }$ are equal to ($x_0$, $y_0$, –$\pi$). In order to ensure the smoothness of pose estimation along the trajectory, all the poses along the trajectory should be corrected by the observations. The pose smoothing can be obtained by:

$$p({\mathcal{P}}_k\mid {\mathcal{Z}}_0,{\mathcal{Z}}_{\zeta })=\frac{p({\mathcal{Z}}_{\zeta }\mid {\mathcal{P}}_k)p({\mathcal{P}}_k\mid {\mathcal{Z}}_0)}{p({\mathcal{Z}}_{\zeta }\mid {\mathcal{Z}}_0)}.$$

(81)

This formula is divided into two steps: The forward step $p({\mathcal{P}}_k\mid {\mathcal{Z}}_0)$ and the backward step $p({\mathcal{Z}}_{\zeta }\mid {\mathcal{P}}_k)$.

The objective of the forward step is to estimate $p({\mathcal{P}}_k\mid {\mathcal{Z}}_0)$, which in general can be obtained by iterating the equation:

$$\begin{array}{c}\hfill p({\mathcal{P}}_k\mid {\mathcal{Z}}_0)=\int p({\mathcal{P}}_k\mid {\mathcal{P}}_{k-1})p({\mathcal{P}}_{k-1}\mid {\mathcal{Z}}_0)\mathrm{d}{\mathcal{P}}_{k-1}.\end{array}$$

(82)

Since there is no prior knowledge for ${\mathcal{P}}_0$, the posterior distribution $p({\mathcal{P}}_0\mid {\mathcal{Z}}_0)$ is assumed to be Gaussian distributed with mean ($x_0$, $y_0$, –$\pi$) and covariance matrix $\mathcal{Q}$. Based on the previous predicted state distribution $p({\mathcal{P}}_{k-1}\mid {\mathcal{Z}}_0)$, the current predicted state distribution $p({\mathcal{P}}_k\mid {\mathcal{Z}}_0)$ can be obtained. Since the robot model is nonlinear and Equation (82) is difficult to integrate, the forward step is done instead by the scaled unscented transformation [35]. The dimension L of the pose vector $p\left({\mathcal{P}}_k\right)$ is 3 and $2L+1$ sigma points should be sampled from the distribution $p({\mathcal{P}}_{k-1}\mid {\mathcal{Z}}_0)$, which is assumed to be Gaussian distributed with mean vector ${\overline{\mathcal{P}}}_{k-1}^{-}$ and covariance matrix ${\mathit{P}}_{k-1}^{-}$. The sigma points ${\mathcal{P}}_{k-1}^{i-}$ are given by:

$$\begin{array}{cc}\hfill {\mathcal{P}}_{k-1}^{0-}& ={\overline{\mathcal{P}}}_{k-1}^{-},\hfill \\ \hfill {\mathcal{P}}_{k-1}^{i-}& ={\overline{\mathcal{P}}}_{k-1}^{-}+{\left(\sqrt{(L+\lambda ){\mathit{P}}_{k-1}^{-}}\right)}_i\mathrm{for}i=1,\cdots ,L,\hfill \\ \hfill {\mathcal{P}}_{k-1}^{i-}& ={\overline{\mathcal{P}}}_{k-1}^{-}-{\left(\sqrt{(L+\lambda ){\mathit{P}}_{k-1}^{-}}\right)}_{i-L}\hfill \\ \hfill & \mathrm{for}i=(L+1),\cdots ,2L.\hfill \end{array}$$

(83)

The corresponding mean weights $w_m^i$ and covariance weights $w_c^i$ are given by:

$$\begin{array}{cc}\hfill w_m^0& =\frac{\lambda }{L+\lambda },\hfill \\ \hfill w_c^0& =\frac{\lambda }{L+\lambda }+1-{\alpha }^2+\beta ,\hfill \\ \hfill w_m^i& =w_c^i=\frac{1}{2(L+\lambda )}i=1,\cdots ,2L,\hfill \end{array}$$

(84)

where $\alpha$, $\beta$, and $\kappa$ are parameters, the variable $\lambda ={\alpha }^2(L+\kappa )-L$. The formula ${\left(\sqrt{(L+\lambda ){\mathit{P}}_{k-1}^{-}}\right)}_i$ is the $i_{th}$ column of $\sqrt{(L+\lambda ){\mathit{P}}_{k-1}^{-}}$. The parameter $\alpha$ is a positive scaling parameter and its range (0,1). The parameter $\kappa$ is a non-negative scaling parameter and its range is [0, ∞). Normally $\kappa$ is set to 0 [36]. The parameter $\beta$ is used to incorporate prior knowledge of the distribution of $P_{k-1}$. For Gaussian distributions, the optimal choice is 2 [35].

Based on these sigma points, the next pose can be predicted by projecting sigma points through the motion model and new sigma points ${\mathcal{P}}_k^i$ will be obtained. However, they should be augmented to include the system noise. The first augmented sigma point ${\mathcal{P}}_k^{0-}$ is the same as ${\mathcal{P}}_k^0$. The other augmented sigma points are given by:

$${\mathcal{P}}_k^{i-}=\left\{\begin{array}{cc}{\mathcal{P}}_k^i+{\left(\sqrt{(L+\lambda )\mathcal{R}}\right)}_i\hfill & \mathrm{for}i=1,\cdots ,L,\hfill \\ {\mathcal{P}}_k^i-{\left(\sqrt{(L+\lambda )\mathcal{R}}\right)}_{i-L}\hfill & \mathrm{for}i=(L+1),\cdots ,2L.\hfill \end{array}\right.$$

(85)

The mean vector and covariance matrix of the distribution $p({\mathcal{P}}_k\mid {\mathcal{Z}}_0)$ can be respectively approximated as:

$${\overline{\mathcal{P}}}_k^{-}\approx \sum _{i=0}^{2L}{w}_m^i{\mathcal{P}}_k^{i-},$$

(86)

$${\mathit{P}}_k^{-}\approx \sum _{i=0}^{2L}{w}_c^i({\mathcal{P}}_k^i-{\overline{\mathcal{P}}}_k^{-}){({\mathcal{P}}_k^{i-}-{\overline{\mathcal{P}}}_k^{-})}^{\mathrm{T}}.$$

(87)

The objective of the backward step is to estimate $p({\mathcal{Z}}_{\zeta }\mid {\mathcal{P}}_k)$, which in general is given by iterating the equation:

$$\begin{array}{c}\hfill p({\mathcal{Z}}_{\zeta }\mid {\mathcal{P}}_k)=\int p({\mathcal{Z}}_{\zeta }\mid {\mathcal{P}}_{k+1})p({\mathcal{P}}_{k+1}\mid {\mathcal{P}}_k)\mathrm{d}{\mathcal{P}}_{k+1}.\end{array}$$

(88)

This equation means $p({\mathcal{Z}}_{\zeta }\mid {\mathcal{P}}_k)$ can also be obtained recursively based on the robot motion model.

As in the forward step, Equation (88) is difficult to integrate due to the nonlinear characteristics of the motion model, and the same approach based on the unscented transformation is used. The sigma points for the predicted observation are given by:

$${\mathbf{z}}_{\zeta }^i={\mathcal{P}}_{\zeta }^i+{[-4.75,0,0]}^{\mathrm{T}}.$$

(89)

The mean vector and covariance matrix of the predicted measurement are approximated, respectively, by:

$${\overline{\mathbf{z}}}_{\zeta }\approx {\overline{\mathcal{P}}}_{\zeta }^{-}+{[-4.75,0,0]}^{\mathrm{T}},$$

(90)

$${\mathit{P}}_z\approx {\mathit{P}}_{\zeta }^{-}+\mathcal{Q}.$$

(91)

For the pose ${\mathcal{P}}_k$, every sigma point ${\mathcal{P}}_k^i$ corresponds to a sigma point ${\mathbf{z}}_{\zeta }^i$ at the end of the time horizon. The corrected sigma points ${\mathcal{P}}_k^{i+}$ used to approximate the corrected distribution $p({\mathcal{P}}_k\mid {\mathcal{Z}}_0,{\mathcal{Z}}_{\zeta })$, which already include the backward step, are computed by:

$${\mathcal{P}}_k^{i+}={\mathcal{P}}_k^i+\mathcal{K}({\mathcal{Z}}_{\zeta }-{\mathbf{z}}_{\zeta }^i).$$

(92)

The Kalman gain $\mathcal{K}$ and cross variance ${\mathit{P}}_{kz}$ are given by:

$$\mathcal{K}={\mathit{P}}_{kz}{\mathit{P}}_z^{-1},$$

(93)

$${\mathit{P}}_{kz}=\sum _{i=0}^{2L}{w}_c^i({\mathcal{P}}_k^i-{\overline{\mathcal{P}}}_k){({\mathbf{z}}_{\zeta }^i-{\overline{\mathbf{z}}}_{\zeta })}^{\mathrm{T}}.$$

(94)

The mean vector and covariance matrix of the distribution $p({\mathcal{P}}_k\mid {\mathcal{Z}}_0,{\mathcal{Z}}_{\zeta })$ are finally estimated as:

$$\begin{array}{cc}\hfill {\overline{\mathcal{P}}}_k^{+}& =\sum _{i=0}^{2L}{w}_m^i{\mathcal{P}}_k^{i+}\hfill \\ \hfill & ={\overline{\mathcal{P}}}_k^{-}+\mathcal{K}({\mathcal{Z}}_{\zeta }-{\overline{\mathbf{z}}}_{\zeta }),\hfill \end{array}$$

(95)

$${\mathit{P}}_k^{+}={\mathit{P}}_k^{-}-\mathcal{K}{\mathit{P}}_z{\mathcal{K}}^{\mathrm{T}}.$$

(96)

Taking advantage of corrected sigma points ${\mathcal{P}}_k^{i+}$, Equation (74) can be rewritten as:

$$p(z_i\mid m)=\sum _{i=0}^{2L}{w}_m^{i}p(z_i\mid m,{\mathcal{P}}_k^{i+}).$$

(97)

The position of the mark is set to be at coordinates (45, 25), and the coordinates of the track are shown in Figure 5. The two observations ${\mathcal{Z}}_0$ and ${\mathcal{Z}}_{\zeta }$ are (45, 25, $-\pi$). The covariance matrices $\mathcal{R}$ and $\mathcal{Q}$ are set to:

$$\mathcal{R}=\left[\begin{array}{ccc}{10}^{-5}& 0& 0\\ 0& {10}^{-5}& 0\\ 0& 0& 5\times {10}^{-5}\end{array}\right],$$

$$\mathcal{Q}=8\times {10}^{-6}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right].$$

The other three parameters are set to $\alpha =0.00001$, $\beta =2$, and $\kappa =0$. The predicted position of the robot before reaching the mark is shown in Figure 10, and the ellipses represent the position uncertainty of the robot (level curve of the distribution at three standard deviations). As time goes by, the uncertainty increases. After a complete loop, the estimated position has drifted to a wrong position, however it is still inside the ellipse. After pose smoothing, the position correction is shown in Figure 11. The estimated positions on the top are out of the outer line of the track with large variances, however the estimated positions near the initial and final positions are more accurate, as expected.

6.4. Results

Regarding the beam model of the two IR sensors, the occupancy and free probabilities $p(z_i\mid m_c)$ of the occupied grid cells in the measurement ranges are set to 0.998 and 0.002, respectively. The two probabilities of the free grid cells in the measurement ranges are set to 0.008 and 0.992. The two probabilities for grid cells outside the measurement ranges are set to 0.5.

The robot follows the track for 100 loops, and an occupancy grid map is built for every loop. Grid cells with posterior occupancy probabilities larger than 0.5 are assumed to be observed occupied. While posterior occupancy probabilities less than 0.5 are assumed to be observed free. The total times the grid cells are observed free and occupied are shown in Figure 12 and Figure 13, respectively. Most of the space in the middle is observed free many times. The space behind the dynamic objects has about half a chance to be observed, and the space behind the static objects is never observed. For the static objects, their borders are observed partially. Due to the uncertainty of sensors and robot poses, the space around the objects is sometimes observed to be occupied. Similarly to most of the observed space, the number of observed times for dynamic object 6 in Figure 13 is close to 0, possibly due to the small size of the object combined with the fact that the robot is turning when the range finder crosses the object.

The central points of grid cells in observed space are the training data to test the proposed method, and the test points are only chosen from unobserved space. The initial values of the probabilities are set to 0.5, the length-scale ℓ of the covariance function is set to 3, and the signal variance ${\sigma }_f^2$ is set to 25. The mean vectors ${\mathbf{\mu }}_1$ and ${\mathbf{\mu }}_1^{*}$ of the occupied-to-occupied probabilities for the training and test points are set to $log9$, which corresponds to a probability of 0.9. The mean vectors ${\mathbf{\mu }}_2$ and ${\mathbf{\mu }}_2^{*}$ are set to $log99$, which corresponds to a probability of 0.99. It means the prior knowledge of the environment is slow dynamic. The maximum number of iterations of the optimisation process is set to 800, and the results are shown in Figure 14 and Figure 15, where the unobserved space is covered by asterisks and has no estimates of transition probabilities. For most of the free space, all the observations are free. The parameter $a_{11}$ of most of the observed free space in Figure 14 are close to the initial value 0.5, which should be close to 0. In this area, most of the observations are free. In contrast, the other observed free space with more unknown observations has a better parameter estimate.

In order to discover the reason, several free points without occupied observations are chosen from the observed space and their parameters are individually estimated using the method in Section 3.

Table 1. Observation numbers of the selected free points.

Free Points	1	2	3	4
Free observation number	19	55	89	100
Occupied observation number	0	0	0	0

shows the numbers of observations of four free points selected. All of them have no occupied observations and different numbers of free observations. The optimisation processes of their parameters are shown in Figure 16 and Figure 17, where the optimisation processes with more free observations converge faster. The derivatives of $Q({\theta }_c,{\theta }_c^{\left(k\right)})$ with respect to $a_{11}$ and $a_{22}$ are shown in Figure 18 and Figure 19, respectively. Even though there is no occupied observation, the derivatives of the Q function with respect to $a_{11}$ are not always zeros and converge to zeros. When the parameters $a_{22}$ converge to 1, the derivatives of Q function with respect to $a_{22}$ converge to nonzero constants. The best estimates of $a_{22}$ for these free points are 1. When $a_{22}$ reaches 0.9, the derivative for corresponding $a_{11}$ is close to 0. For the space without unknown observation, the corresponding parameter $a_{11}$ converge very quickly and $a_{11}$ has lesser chance to be optimised. With different numbers of observations, the estimation processes converge at different speeds.

In order to decrease the convergence speed of $a_{22}$, for the space with more than 95 free observations, 45 of them are replaced by 45 fake observations corresponding to unknown space. the derivative of Q with respect to $a_{11}$ for occupied space and the derivative of Q with respect to $a_{22}$ for free space never converge to zeros. The LSM will always search for the estimates for them even though their estimates are close to 1. In order to give more chances to other parameters, the optimisation process halts searching for them when their estimates reach 0.995. The grid cells with more observations will converge faster. For the border of observed space and unobserved space, there are fewer or no observations, and it will take a long period of time to converge.

Since the points with more observations converge quickly, it will take a long period of time to obtain the best estimates for the points with fewer observations. The maximum number of iterations of the optimisation process is set to 1500. The new results are shown in Figure 20 and Figure 21, where most of the observed free space has low $a_{11}$ and high $a_{22}$. This means that the state stays free for a long time and changes from occupied to free quickly. The static objects 2, 5, and 7 have the opposite behaviour and parameters. The state of the dynamic object 1 alternates between free and occupied quickly. For the dynamic objects 3 and 4, the colour becomes darker corresponding to slower dynamics. Due to the lack of observations, the dynamic object 6 is estimated as free space. The other two dynamic objects 8 and 9 change their states slowly. The space behind these dynamic objects has fewer observations, the corresponding areas are darker than the space with more free observations. The space behind static objects 2 and 5 is never observed, and therefore there is no estimate. Due to the uncertainty of the robot pose, the space behind the static object 7 has a similar estimate to free space.

The parameter prediction for unobserved space is shown in Figure 22 and Figure 23. The border of the observed space in Figure 22 is a little fuzzy and the estimate of most of the unobserved space is similar to the prior. In Figure 23, most of the parameters on the borders of observed space are close to 1, and the parameters of the unobserved space near these areas are also predicted to be close to one. Since the parameters of the borders of observed space near the static objects are close to 0.5, the darkness near these areas is lighter. Similarly to Figure 22, the prediction of the other unobserved space is similar to the prior.

Based on parameters $a_{11}$ and $a_{22}$, the space in dynamic environments can be classified as

Table 2. Classification for the dynamic environments. When the space does not belong to any class in the previous four, it is classified as high dynamic.

Classification	Free	Occupied	Low Dynamic	Unknown	High Dynamic
Parameters	$a_{11}<0.6$	$a_{11}>0.85$	$a_{11}>0.85$	$0.53>a_{11}>0.47$	Others
	$a_{22}>0.85$	$a_{22}<0.6$	$a_{22}>0.85$	$0.53>a_{22}>0.47$

. Based on the table, the objects 1, 3, 4, and 6 in the experimental map are high dynamic, and the objects 8 and 9 are low dynamic. The classification results are shown as Figure 24 and

Table 3. Classification results for the dynamic experimental environment. TF = True free, TO = True occupied, FF = False free, FO = False occupied, TLD = True low dynamic, FLD = False low dynamic, THD = True high dynamic, FHD = False high dynamic, UN = Unknown.

Classification	TF	FF	TO	FO	TLD	FLD	THD	FHD	UN
	4648	146	16	11	0	752	2	725	0

. Due to pose uncertainty, the positions of the objects 8 and 9 are different from the truth. Even though they are obvious in Figure 24, all the True Low Dynamic (TLD) space is wrong. For the same reason, the dynamic objects 3 and 6 are estimated as free space. Due to the prior low dynamic assumption, there is more False Low Dynamic (FLD) space. As the proposed method can smooth the map, there must be more FHD space. The prediction variance increases with the distance to the observed space. As a result, only the predictions near the observed space are more believable. Moreover, the proposed method can predict more free space correctly. The classification accuracy for observed space is 96%.

Given the transition probabilities, the overall expected duration is shown in Figure 25 and the version in log scale is shown in Figure 26. The observed free space, the static objects, and the low dynamic objects have long overall expected expectations. The dynamic objects 3 and 6 are mostly invisible and have similar overall expected durations to the free space. The dynamic objects with higher switching frequencies, 1 and 4, have shorter overall expected durations and are clearly visible in log scale. For the unobserved space behind the static objects, the overall expected duration is short. The remaining unobserved space has a long overall expected duration.

7. Conclusions

In this paper, a GRF-based mapping method for dynamic environments is proposed, where the dynamic behaviour is modelled by HMMs. The HMM with normalised emission probabilities is introduced and used to conveniently estimate the parameters. In order to deal with the inconsistency in the parameter maps, GRFs are applied to consider the correlations between different points in continue space. The parameter estimation is factorised to reduce computational complexity. The EM algorithm is used to estimate the parameters for the observed space, where the Q function is optimised by the line search method. The predictive equation of the Gaussian process is used to deal with the parameters for the unobserved space. The pose uncertainty is incorporated into the measurement model and a 3pi robot with two IR sensors is used to evaluate the proposed method. Experiment results show that parameter estimation depends on robot poses. Even though the proposed method identifies the low dynamic objects, the number of TLD is 0. The classification accuracy for observed space is 96%. Compared with the state-of-the-art approaches, the proposed method takes the parameter dependence into consideration and builds smooth maps for observed space. Moreover, the dynamic behaviour near the observed space can be predicted, which is significant for path planning. The main disadvantage is that it takes a long period of time to search for the parameters of points with fewer observations. In future work, we plan to reduce the computational complexity.