# An Adaptive Unscented Particle Filter Algorithm through Relative Entropy for Mobile Robot Self-Localization

Authors: Wentao Yu; Jun Peng; Xiaoyong Zhang; Shuo Li; Weirong Liu
Journal: Mathematical Problems in Engineering
 (2013)

---

## Abstract

Self-localization is a basic skill for mobile robots in the dynamic environments. It is usually modeled as a state estimation problem for nonlinear system with non-Gaussian noise and needs the real-time processing. Unscented particle filter (UPF) can handle the state estimation problem for nonlinear system with non-Gaussian noise; however the computation of UPF is very
high. In order to reduce the computation cost of UPF and meanwhile maintain the accuracy, we propose an adaptive unscented particle filter (AUPF) algorithm through relative entropy. AUPF can adaptively adjust the number of particles during filtering to reduce the necessary computation and hence improve the real-time capability of UPF. In AUPF, the relative entropy is used to measure the distance between the empirical distribution and the true posterior distribution. The least number of particles for the next step is then decided according to the relative entropy. In order to offset the difference between the proposal distribution, and the true distribution the least number is adjusted thereafter. The ideal performance of AUPF in real robot self-localization is demonstrated.


---

## Body

## 1. Introduction

Autonomous mobile robots have increasingly been used in a wide range of applications [1, 2]. Self-localization is a fundamental and challenging task for the autonomous mobile robots [3]. In order to successfully make decisions or perform the missions, the mobile robots have to know their poses precisely [4]. The self-localization of mobile robot can be defined as the estimation for the pose of the mobile robot relative to the environment [5].Over the past years many different solutions for mobile robot self-localization have been presented, and they can be divided into two classes: the sensor-based method and the sensor assisted probabilistic method. The sensor-based method only uses the sensor information to realize the mobile robot self-localization. Reference [6] presents a natural landmark-based indoor localization algorithm. The natural landmarks are extracted from the input image which is obtained by the vision sensor. Then the map positions of the natural landmarks are identified by comparing the input image with the reference data set to acquire the mobile robot self-localization. Reference [7] has suggested an algorithm that only utilizes the Bluetooth device to realize the self-localization of the mobile robot. Although the sensor-based method is easy to implement, it has an inherent limitation. The range and resolution of the sensors are subject to the physical law, and the sensor measurements can be perturbed by the noises. These limitations influence the sensors' ability to extract the useful information from the measurements. On the other hand the sensor-based probabilistic method can utilize probabilistic law to deal with the noise and the uncertainty of the sensor information [5].Bayesian filter is the most important probabilistic method for self-localization which is a state estimation problem for nonlinear system with non-Gaussian noise [8, 9]. Bayesian filter is a class of statistical filter techniques via Bayesian inference, and it can recursively calculate the posterior probability, the estimated state, and the predicted state [10]. Bayesian filter is easy to implement and has a wide range of applications, so it has been extensively studied in recent years [11, 12].There are several commonly used Bayesian filter algorithms that can be applied in the mobile robot self-localization. Kalman Filter (KF) is a recursive algorithm for the linear system state estimation with Gaussian noise. KF has high computational efficiency and good accuracy [13]. In [14], the applicability of KF to the mobile robot self-localization is analyzed. This algorithm is only suitable for linear systems. Extended Kalman Filter (EKF) is another kind of Bayesian filter for the mobile robot self-localization. In [15] EKF is used to localize the four-wheeled mobile robot equipped with the wheel encoders and a laser-rangefinder sensor. EKF is derived for the nonlinear systems, which linearizes the measurements and evolution models using the Taylor series expansions. EKF only uses the first-order term of the Taylor series expansions of the nonlinear functions. This linearization often induces large errors in the estimated statistics of the posterior distributions. This is especially evident when the models are highly nonlinear, and it can lead to filter divergence [16]. Unlike EKF, the Unscented Kalman Filter (UKF) does not approximate the nonlinear process and the observation model. It uses the true nonlinear models. UKF approximates the distribution of the state random variables by the scaled unscented transformation [17]. In [18], the performances of EKF and UKF are compared for the localization of an autonomous robot. The conclusion is that UKF has shown better performance in terms of average localization accuracy. UKF does not need to solve the Jacobian matrix, so it has fewer computations and higher precision.Self-localization is a state estimation problem for nonlinear system with non-Gaussian noise. Neither EKF nor UKF can deal with non-Gaussian noise, but particle filter (PF) can handle the state estimation problem of nonlinear system with non-Gaussian noise [19]. The key idea of PF is to use particles to represent the posterior distribution of the state. As new information arrives, these particles are reallocated to update the estimation of the state of the system [20]. As a useful tool for dealing with the nonlinear and non-Gaussian problems, PF has been widely used in various fields [21, 22].In PF, the particles are usually sampled from the proposal distribution which neglects the new observation, and this can cause particle degeneracy and influence the accuracy of PF. Then the unscented particle filter (UPF) algorithm is proposed to resolve the problem. In UPF, UKF is used to generate the new proposal distribution [23]. This proposal can incorporate the latest observation and match the true posterior distribution more closely, so UPF improves the accuracy of state estimation evidently. In traditional UPF the number of particles is fixed, and in each step each particle utilizes UKF to obtain the new importance proposal that can bring huge computation and influence the real-time capability of the filter. During filtering fixed number of particles may bring unnecessary computation, so the computation will be reduced if the number of particles can be dynamically adjusted according to the estimation quality during filtering.Mobile robots usually work under a dynamic environment; real-time self-localization is thus needed to be realized [24]. So UPF should reduce its computation to accommodate mobile robots self-localization. To the best of our knowledge there is no related work about how to reduce the computation cost of UPF and meanwhile maintain accuracy. The computation cost of UPF depends on the number of particles, so in order to reduce the computation cost of UPF the number of particles should be adaptively adjusted during filtering.In this paper an adaptive unscented particle filter algorithm through relative entropy (AUPF) is proposed to enhance the real-time capability of UPF and meanwhile ensure its accuracy. In AUPF, the relative entropy is used to measure the distance between the empirical distribution and the true posterior distribution; the least number of particles for the next step is then decided according to the relative entropy. In order to offset the difference between the proposal distribution and the true distribution the least number is adjusted thereafter. AUPF can adaptively adjust the number of particles during filtering to reduce the necessary computation and hence improve the real-time capability of UPF.The rest of the paper is organized as follows. Section2 describes the dynamic state-space model of mobile robot self-localization problem. In Section 3 the concept of UPF is introduced. In Section 4 an adaptive UPF algorithm through relative entropy is proposed. Then Section 5 shows the experiment results which include the simulation results and the utilization of AUPF in a real robot self-localization. Finally, Section 6 gives the conclusions.


## 2. Dynamic State-Space Model of Mobile Robot Self-Localization

To solve the mobile robot self-localization problem, we need a self-localization model for robot. In general, such a model can be described using a set of discrete-time equations given as follows:(1)xt=f(xt-1,wt),(2)zt=h(xt,vt),
where xt∈Rnx denotes the self-localization state vector of the mobile robot at time step t; nx means the dimension of xt; wt∈Rnw denotes the process noise at time step t which is usually non-Gaussian; zt∈Rnz is the observation vector at time step t; nz is the dimension of zt; vt∈Rnv is the measurement noise at time step t which is also usually non-Gaussian; nv is the dimension of vt; the states follow a first-order Markov process, and the measurements are assumed to be independent. The function f represents the state transition model, f:Rnx*Rnw↦Rnx. The function h represents the state measurement model, h:Rnx*Rnv↦Rnz.


## 3. Unscented Particle Filter Algorithm

In PF, it is hard to design the proposal distribution which can approximate the true posterior distribution reasonably well. The common strategy is to sample the particles according to the state transition. This strategy cannot consider the new observations, so it can not generate an ideal proposal distribution. UPF is proposed to resolve this problem. In UPF, UKF is introduced to produce the proposal distribution of particle filter which considers the new observation, and the produced proposal distribution is close to the true posterior distribution.UKF is a minimum mean square error estimator for non-linear system. It utilizes the scaled unscented transformation to calculate the statistics of a random variable. UKF has higher accuracy than EKF especially when the models are highly nonlinear. In UPF, before sampling of the particles UKF is utilized to generate the proposal distribution which can consider the new observation. The proposal distribution generated by UKF has a bigger overlap with the true posterior distribution than PF, so UPF has relatively high accuracy, while, in UPF, at each time step each particle will utilize UKF to generate the proposal distribution, so the computation cost of UPF is huge.The basic UPF algorithm is described as in Algorithm1:
(3)x~t=1N∑i=1N‍x~t(i).Algorithm 1
(1) Initialization:t=0
i=1
do
draw the particles x0i from the prior p(x0) and set
x_0(i)=E[x0(i)]
P0(i)=E[(x0(i)-x_0(i))(x0(i)-x_0(i))T]
x
_
0
(
i
)
a
=
E
[
x
(
i
)
a
]
=
[
(
x
_
0
(
i
)
)
T
0
0
]
T
P0(i)a=E[(x0(i)a-x_0(i)a)(x0(i)a-x_0(i)a)T]
=
[
P
0
(
i
)
0
0
0
Q
0
0
0
R
]
// N is the number of particles in UPF. x_0(i) is the initial mean value of x0(i).
// P0(i) is the initial covariance of x0(i). x_0a is the initial mean value of x0a.
// Q is the covariance of wt. R is the covariance of vt.
While (i++=<N )
(2) Fort=1,2,…,tf
//tf is the run time of UPF.
(1) Importance sampling
i=1
do
Update the particles with UKF, obtain x_t(i) and Pt(i).
x^t(i)~q(xt(i)∣x0:t-1(i),z1:t)=N(x_t(i),Pt(i))
// The particles x^t(i) are sampled from N(x_t(i),Pt(i)).
while (i++=<N )
(2) Computing the importance weights
i=1
do
wt(i)∝p(zt∣x^t(i))p(x^t(i)∣xt-1i)q(x^t(i)∣x0:t-1(i),z1:t)
//wt(i) is the importance weight of the ith particle at the time step t.
// The state transition probabilityp(x^t(i)∣xt-1i) can be obtained from (1).
// The observation modelp(zt∣x^t(i)) can be obtained from (2).
//q(x^t(i)∣x0:t-1(i),z1:t) is the proposal distribution.
while (i++=<N )
(3) Normalize the importance weights
i=1
do
w~t(i)=wt(i)∑I=1Nwt(i)
while (i++=<N )
(4) Resampling step
i=1
do
Multiply/Suppress particlesx^t(i) according to the importance weights w~t(i) to obtain N
random particlesx~t(i).
while(i++=<N )
(5) Output
The outputx~t is the state estimation of x at the time step t. It can be approximated by (3).


## 4. An Adaptive Unscented Particle Filter Algorithm through Relative Entropy

It can be seen that UPF described in Section3 may provide high accuracy for estimation. However, it is computationally expensive due to the utilization of UKF for each particle at each time step. In this section, in order to reduce the computation cost and meanwhile maintain the accuracy of UPF, we propose an adaptive unscented particle algorithm by adaptively adjusting the number of particles during the process. Specifically, we introduce the relative entropy to bind the error induced by sample-based representation of UPF and correspondingly determine the number of particles for each filter iteration.


### 4.1. Relative Entropy Based Adaptive Adjustment of UPF

The sample-based representation of UPF will bring the error. If the error can be bounded, the accuracy of UPF will be insured. So the adaptive adjustment of the number of particles needs to ensure that the error introduced by the sample-based representation must be within a bound. The error can be measured by the distance between the empirical distribution and the true posterior distribution. In order to maintain the accuracy of UPF, it is assumed that the error must be within a prespecified valueε.The relative entropy is a measure of the distance between two distributions [25]. The relative entropy between two probability functions p1 and p2 can be defined by
(4)K(p1,p2)=∑‍p1logp1p2.In UPFN particles are drawn from the proposal distribution with k different bins. X_ denotes the number of particles drawn from each bin, X_=(X1,…,Xk). p_ means the true distribution. p_ specifies the probability of each bin, p_=(p1,…,pk). The maximum likelihood estimate of p_ is given by
(5)p^_=N-1X_.The likelihood ratio statisticλN for testing p_ is given by
(6)logλN=∑j=1k‍Xjlogp^jpj=N∑j=1k‍p^jlogp^jpj=NK(p^_,p_).Reference [25] gives a conclusion that the likelihood ratio statistic can satisfy a chi-square distribution convergence:
(7)2logλN⟶χk-12n⟶∞.So(8)P(K(p^_,p_)≤ε)=P(2NK(p^_,p_)≤2Nε)=˙P(χk-12≤2Nε).Then we can obtain(9)2Nε>χk-1,1-δ2,
where δ is a given probability; χk-1,1-δ2 can be obtained according to δ and k(10)N>χk-1,1-δ22ε.Equation (10) is transformed from (9). From (10) it's found that when the number of particles N is big enough the relative entropy between the maximum likelihood estimate and the true distribution is less than ε with the probability 1-δ.


### 4.2. Compensation for the Influence of the Importance Proposal

In Section4.1, p_ is assumed to be the true distribution, while the true distribution cannot be obtained, and it is approximately described by the proposal distribution. Furthermore the quality of the match between the proposal distribution and the true distribution is one of the main elements that determines the accuracy of the filter, hence the suitable number of particles. To fix this problem, we need a way to quantify the degradation in the estimation using samples from the proposal distribution.In [26], Vartrue can measure the influence of sampling from the true distribution p(x), and it is calculated by
(11)Vartrue=Varp(x)N,(12)Varp(x)=Ep(x2)-Ep(x)2≈∑i=1Nxi2ωi∑i=1Nωi-Ep(x)2,(13)Ep(x)=∑i=1Nxiωi∑i=1Nωi.In [27], when the particles come from the proposal distribution q(x), the variance of the estimator VarIS can be given by
(14)VarIS=σIS2NIS,
where NIS is the number of particles coming from the proposal distribution,
(15)σIS2≈∑i=1Nxi2ωi2∑i=1Nωi-∑i=1Nxiωi2Ep(x)2∑i=1Nωi+∑i=1Nωi2Ep(x)2∑i=1Nωi.In order to obtain the similar accuracy, let (11) be equal to (14):
(16)σIS2NIS=Varp(x)N⟹N=Varp(x)NISσIS2,2Nε>χk-1,1-δ2⟹2Varp(x)NISσIS2>χk-1,1-δ2⟹NIS>σIS22εVarp(x)χk-1,1-δ2.So the minimum number of particlesNISmin can be obtained according to
(17)NISmin=⌈σIS22εVarp(x)χk-1,1-δ2⌉.


### 4.3. An Adaptive Unscented Particle Filter Algorithm through Relative Entropy

Figure1 illustrates the relationship between several basic methods.Figure 1
The relationship between several basic filter methods.The adaptive unscented particle filter algorithm through relative entropy is shown in Algorithm2.Algorithm 2
Inputs:St-1={(xt-1i,wt-1i)i=1,…,n}; the control measurement ut-1; the observation zt;
the bound ε and the probability σ; the bin size ℏ; St=ϕ; n=0; k=0; α=0.
Generate the particles:
i
=
1
do
Sample a particle indexj(n) from the discrete distribution according to the weights in St-1.
Obtain the particlesxtn from the particles xt-1j(n) according to (1).
Compute the importance weight:
wtn=p(zt∣xtn)
α=α+wtn
Insert particles into the particle set:
St=St∪{(xtn,wtn)}
if (xtn falls into empty bin)
k=k+1
end if
n=n+1
while(n<(σIS2/2εVarp(x))χk-1,1-δ2)
Normalize the importance weight:
fori=1,…,n
wti=wti/α
returnSt


## 4.1. Relative Entropy Based Adaptive Adjustment of UPF

The sample-based representation of UPF will bring the error. If the error can be bounded, the accuracy of UPF will be insured. So the adaptive adjustment of the number of particles needs to ensure that the error introduced by the sample-based representation must be within a bound. The error can be measured by the distance between the empirical distribution and the true posterior distribution. In order to maintain the accuracy of UPF, it is assumed that the error must be within a prespecified valueε.The relative entropy is a measure of the distance between two distributions [25]. The relative entropy between two probability functions p1 and p2 can be defined by
(4)K(p1,p2)=∑‍p1logp1p2.In UPFN particles are drawn from the proposal distribution with k different bins. X_ denotes the number of particles drawn from each bin, X_=(X1,…,Xk). p_ means the true distribution. p_ specifies the probability of each bin, p_=(p1,…,pk). The maximum likelihood estimate of p_ is given by
(5)p^_=N-1X_.The likelihood ratio statisticλN for testing p_ is given by
(6)logλN=∑j=1k‍Xjlogp^jpj=N∑j=1k‍p^jlogp^jpj=NK(p^_,p_).Reference [25] gives a conclusion that the likelihood ratio statistic can satisfy a chi-square distribution convergence:
(7)2logλN⟶χk-12n⟶∞.So(8)P(K(p^_,p_)≤ε)=P(2NK(p^_,p_)≤2Nε)=˙P(χk-12≤2Nε).Then we can obtain(9)2Nε>χk-1,1-δ2,
where δ is a given probability; χk-1,1-δ2 can be obtained according to δ and k(10)N>χk-1,1-δ22ε.Equation (10) is transformed from (9). From (10) it's found that when the number of particles N is big enough the relative entropy between the maximum likelihood estimate and the true distribution is less than ε with the probability 1-δ.


## 4.2. Compensation for the Influence of the Importance Proposal

In Section4.1, p_ is assumed to be the true distribution, while the true distribution cannot be obtained, and it is approximately described by the proposal distribution. Furthermore the quality of the match between the proposal distribution and the true distribution is one of the main elements that determines the accuracy of the filter, hence the suitable number of particles. To fix this problem, we need a way to quantify the degradation in the estimation using samples from the proposal distribution.In [26], Vartrue can measure the influence of sampling from the true distribution p(x), and it is calculated by
(11)Vartrue=Varp(x)N,(12)Varp(x)=Ep(x2)-Ep(x)2≈∑i=1Nxi2ωi∑i=1Nωi-Ep(x)2,(13)Ep(x)=∑i=1Nxiωi∑i=1Nωi.In [27], when the particles come from the proposal distribution q(x), the variance of the estimator VarIS can be given by
(14)VarIS=σIS2NIS,
where NIS is the number of particles coming from the proposal distribution,
(15)σIS2≈∑i=1Nxi2ωi2∑i=1Nωi-∑i=1Nxiωi2Ep(x)2∑i=1Nωi+∑i=1Nωi2Ep(x)2∑i=1Nωi.In order to obtain the similar accuracy, let (11) be equal to (14):
(16)σIS2NIS=Varp(x)N⟹N=Varp(x)NISσIS2,2Nε>χk-1,1-δ2⟹2Varp(x)NISσIS2>χk-1,1-δ2⟹NIS>σIS22εVarp(x)χk-1,1-δ2.So the minimum number of particlesNISmin can be obtained according to
(17)NISmin=⌈σIS22εVarp(x)χk-1,1-δ2⌉.


## 4.3. An Adaptive Unscented Particle Filter Algorithm through Relative Entropy

Figure1 illustrates the relationship between several basic methods.Figure 1
The relationship between several basic filter methods.The adaptive unscented particle filter algorithm through relative entropy is shown in Algorithm2.Algorithm 2
Inputs:St-1={(xt-1i,wt-1i)i=1,…,n}; the control measurement ut-1; the observation zt;
the bound ε and the probability σ; the bin size ℏ; St=ϕ; n=0; k=0; α=0.
Generate the particles:
i
=
1
do
Sample a particle indexj(n) from the discrete distribution according to the weights in St-1.
Obtain the particlesxtn from the particles xt-1j(n) according to (1).
Compute the importance weight:
wtn=p(zt∣xtn)
α=α+wtn
Insert particles into the particle set:
St=St∪{(xtn,wtn)}
if (xtn falls into empty bin)
k=k+1
end if
n=n+1
while(n<(σIS2/2εVarp(x))χk-1,1-δ2)
Normalize the importance weight:
fori=1,…,n
wti=wti/α
returnSt


## 5. Experimental Results

In this section, the results of two experiments are presented. In the first experiment the performance of AUPF is compared with UPF and PF through a simulation to validate the effectiveness of AUPF. Then the utilization and performance of AUPF for a real robot self-localization are also illustrated in detail.


### 5.1. Simulation Results

To illustrate the effectiveness of the proposed AUPF, a scalar nonlinear system is considered which is shown in (18) and (19). The simulation is carried out in an HP computer with 4 G memory and Intel Core i2-2120 CPU
(18)xt=2+sin(0.02πt)+0.3xt-1+wt-1,(19)zt={0.2xt2+vt-1,t≤30,0.5x+vt-1,t>30.Equation (18) is the state transition model. Equation (19) is the state observation model. wt is white Gaussian noise, and its covariance is 1.5; vt is white Gaussian noise, and its covariance is 1.We compare the AUPF with traditional PF and UPF. The number of particles in PF and UPF is set to be 100. The initial number of particles in AUPF is also set to be 100. In the experiment we use a value 0.99 forδ and a fixed bin size Δ 0.02. Random 50-step state estimation results are shown in Figure 2.Figure 2
The estimation result of PF, UPF, and AUPF.The root mean square error (RMSE) is an important value that can be used to reflect the accuracy of state estimation. RMSE can be calculated by(20)RMSE=∑t=1tf(x-x-)2tf.We have run AUPF, UPF, and PF for 10000 steps; the RMSE of the three algorithms is shown in Table1. Meanwhile the average number of particles of the three algorithms is shown in Table 2.Table 1
RMSE of AUPF, PF, and UPF.
Algorithm
AUPF
PF
UPF
RMSE
0.30687
0.73439
0.28767Table 2
Average number of particles of AUPF, PF, and UPF.
Algorithm
AUPF
PF
UPF
Average number of particles
69
100
100From Figure2 and Table 1 we can see that the estimation result of PF is not satisfying. Particle filter uses a set of particles to represent the posterior distribution, so the accuracy of PF is decided by the number of particles. In this experiment the number of particles is 100, and these particles cannot approximate the true posterior density well especially when the nonlinearity of the system is strong. The performance of UPF is the most excellent, because in this algorithm the latest observation is incorporated into the proposal distribution, so 100 particles are enough to approximate the true posterior density. In Table 2 the average number of particles of PF and UPF is 100; it is a fixed number of these algorithms, while in AUPF this number is adaptively adjusted according to (17). According to the adaptive adjustment of the number of particles the runtime of AUPF is reduced which is proportional to this number.


### 5.2. AUPF for Real Mobile Robot Self-Localization

We hereafter experimentally validate the effectiveness of AUPF for mobile robot self-localization. The robot “E-Robot” which is developed by Chinese Academy of Sciences is utilized for mobile robot self-localization. E-Robot is shown in Figure3.Figure 3
The E-Robot used in the experiments.During the mobile robot self-localization, the odometer information and the visual information are used. The picture is obtained by the ultrahigh definition color gun camera AMPSON DSC-B468C, and it is processed by FPGA Cyclone II EP2C20. The image pixel of the picture obtained by the camera is 256* 256. In one second about 30 pictures can be processed.


#### 5.2.1. Mobile Robot Motion Model

The odometer can obtain the offset of robot pose (Δxr, Δyr, and Δθr) from time t-1 to time t, so the motion model of mobile robot can be represented by
(21)xrt=xrt-1+Δxr+Δxr*δxr,yrt=yrt-1+Δyr+Δyr*δyr,θrt=θrt-1+Δθr+Δθr*δθr,
where (xrt, yrt) is the position at time step t; θrt is the orientation at time step t; δxr, δyr, and δθr are non-Gaussian noise.


#### 5.2.2. Mobile Robot Observation Model

In our experiment, the mobile robot can move under an environment with many features, such as the white line and the two-dimension codes. All the features can be extracted from robot vision information.In this paper, the special two-dimension (2D) codes have been used as the features or landmarks. As shown in Figure4, the outer contour of each 2D code is regular pentagon. A different 2D code has different dots inside the regular pentagon. The 2D code is developed by Institute of Automation, Chinese Academy of Sciences. It is easy to be processed and can bring plenty of information. In [28], the image processing for 2D code is explained in detail.Some examples of 2D code developed by Chinese Academy of Science.
(a)
Example 1
(b)
Example 2
(c)
Example 3
(d)
Example 4In our experiments, each 2D code is different from the others. Each 2D code is located at a fixed position, and it brings the location information. So once the mobile robot detects one 2D code, it can obtain one numerical value of self-localization with the help of image processing.Assume that the robot can detectn two-dimension codes d1,…,dn, then n self-localization value lv1,…,lvn can be obtained accordingly.In our experiment, the mobile robot self-localization result is represented by a set of particles,sjt=〈lpjt,pjt〉, where lpjt means the self-localization value maintained by the jth particle at time step t, and pjt is the importance weight of the jth particle at time step t. Utilizing the mobile robot motion model and lpjt-1, the jth particle can obtain an original self-localization numerical value lpojt. If the robot is localized at lpojt, the distance between lpojt and lvi can be used to calculate the probability P(di∣lpojt-1) with which the robot localized at lpojt can observe the feature di. The probability will become bigger along with the decrease of the distance
(22)P(di∣lpojt-1)=e-|lvi-lpojt|.The extraction of the features just depends on the robot's position and orientation, so the features are mutually independent. The observation modelP(d∣lpojt-1) is formulated in
(23)P(d∣lpojt-1)=P(d1∣lpojt-1)*⋯*P(dn∣lpojt-1)=e-|lv1-lpojt|*⋯*e-|lvn-lpojt|.Actually, the mobile robot can detect at least one feature in our experiment, and no more than two features will be chosen which are the nearest to the robot.


#### 5.2.3. The Realization of AUPF for Mobile Robot Self-Localization

In the experiment, the initial number of particles of AUPF is set to be 400. The number of particles during filtering at time instantt is represented by Npt. The process of the realization for mobile robot self-localization is reported as follows.(1)
Obtain the initial position of the mobile robotl0 according to the robot vision.(2)
Aroundl0 a field of 2 * 2 meters is chosen, and 400 particles are uniformly distributed in this field; the importance weight of each particle is 1/400.(3)
Obtain new particles according to UKF algorithm and the mobile robot motion model which utilizes the odometer information.(4)
Obtain the vision information, extract the features, and update the importance weight of each particle according to (17).(5)
Output the estimation of mobile robot position according to (18), then calculate the orientation of the robot according to (24), and then return to step 3 at the next time step.(6)
Calculate the number of particlesNpt.(7)
ResampleNpt particles according to their importance weight.(8)
Set all the importance weight of particles to be 1/Npt.The robot position (xrt, yrt) is obtained by AUPF; then the orientation θrt can be calculated according to
(24)xltr=(xl-xrt)cosθrt-(yl-yrt)sinθrt,yltr=(xl-xrt)sinθrt+(yl-yrt)cosθrt,
where (xl, yl) is the position of one 2D code in the world coordinate, and this position is already known. All the positions used above are in the world coordinate. (xltr, yltr) is the position of the 2D code in the robot coordinate, and this position can be easily obtained by the image processing.


#### 5.2.4. Experiment Results and Analysis

In order to illustrate the effectiveness of AUPF, we designed a mobile robot self-localization experiment. In the experiment, the E-Robot randomly moved on the ground, and a trajectory was generated. Then the trajectory was recorded. The trajectory is depicted in Figure5 by a black line. From Figure 5 we can see that the 2D codes are located on the ground as landmarks. When the robot moved, AUPF was run to calculate the robot position at each time step. The effect of AUPF can be measured by the difference between the calculated position and the trajectory.Figure 5
The real trajectory of the E-Robot in the experiment.Then we adopt AUPF, UPF, and PF to obtain the estimation of self-localization value. The estimation results of the three algorithms are shown in Figure6.Figure 6
The estimation results of PF, UPF, and AUPF.The mobile robot self-localization result can be measured by the mean relative position error (MRPE) which is defined in(25)MRPE=∑t=1tf(xtruet-x^t)2+(ytruet-y^t)2tf,
where xtruet and ytruet are the true robot positions in Cartesian coordinates at time step t, tf is the total time instant of the three algorithms, and x^t and y^t are the estimation results.From Figure6 and Table 3 we can see that the MRPE of AUPF is better than PF, and the MRPE of UPF is the best. That is, because in UPF each particle moves according to a new proposal distribution which utilizes the new observation, while in PF the particles cannot move appropriately without the direction of new observation. We also find that AUPF can acquire relatively ideal accuracy. The number of particles in AUPF is reduced, and the reduction is according to the estimation quality, so the reduction will not cause severe loss of accuracy.Table 3
Experiment results of AUPF, PF, and UPF.
Algorithm
AUPF
PF
UPF
MRPE
9.62
18.91
7.86
Average run time (ms)
3.396
2.262
4.635
Average number of particles
298
400
400


## 5.1. Simulation Results

To illustrate the effectiveness of the proposed AUPF, a scalar nonlinear system is considered which is shown in (18) and (19). The simulation is carried out in an HP computer with 4 G memory and Intel Core i2-2120 CPU
(18)xt=2+sin(0.02πt)+0.3xt-1+wt-1,(19)zt={0.2xt2+vt-1,t≤30,0.5x+vt-1,t>30.Equation (18) is the state transition model. Equation (19) is the state observation model. wt is white Gaussian noise, and its covariance is 1.5; vt is white Gaussian noise, and its covariance is 1.We compare the AUPF with traditional PF and UPF. The number of particles in PF and UPF is set to be 100. The initial number of particles in AUPF is also set to be 100. In the experiment we use a value 0.99 forδ and a fixed bin size Δ 0.02. Random 50-step state estimation results are shown in Figure 2.Figure 2
The estimation result of PF, UPF, and AUPF.The root mean square error (RMSE) is an important value that can be used to reflect the accuracy of state estimation. RMSE can be calculated by(20)RMSE=∑t=1tf(x-x-)2tf.We have run AUPF, UPF, and PF for 10000 steps; the RMSE of the three algorithms is shown in Table1. Meanwhile the average number of particles of the three algorithms is shown in Table 2.Table 1
RMSE of AUPF, PF, and UPF.
Algorithm
AUPF
PF
UPF
RMSE
0.30687
0.73439
0.28767Table 2
Average number of particles of AUPF, PF, and UPF.
Algorithm
AUPF
PF
UPF
Average number of particles
69
100
100From Figure2 and Table 1 we can see that the estimation result of PF is not satisfying. Particle filter uses a set of particles to represent the posterior distribution, so the accuracy of PF is decided by the number of particles. In this experiment the number of particles is 100, and these particles cannot approximate the true posterior density well especially when the nonlinearity of the system is strong. The performance of UPF is the most excellent, because in this algorithm the latest observation is incorporated into the proposal distribution, so 100 particles are enough to approximate the true posterior density. In Table 2 the average number of particles of PF and UPF is 100; it is a fixed number of these algorithms, while in AUPF this number is adaptively adjusted according to (17). According to the adaptive adjustment of the number of particles the runtime of AUPF is reduced which is proportional to this number.


## 5.2. AUPF for Real Mobile Robot Self-Localization

We hereafter experimentally validate the effectiveness of AUPF for mobile robot self-localization. The robot “E-Robot” which is developed by Chinese Academy of Sciences is utilized for mobile robot self-localization. E-Robot is shown in Figure3.Figure 3
The E-Robot used in the experiments.During the mobile robot self-localization, the odometer information and the visual information are used. The picture is obtained by the ultrahigh definition color gun camera AMPSON DSC-B468C, and it is processed by FPGA Cyclone II EP2C20. The image pixel of the picture obtained by the camera is 256* 256. In one second about 30 pictures can be processed.


### 5.2.1. Mobile Robot Motion Model

The odometer can obtain the offset of robot pose (Δxr, Δyr, and Δθr) from time t-1 to time t, so the motion model of mobile robot can be represented by
(21)xrt=xrt-1+Δxr+Δxr*δxr,yrt=yrt-1+Δyr+Δyr*δyr,θrt=θrt-1+Δθr+Δθr*δθr,
where (xrt, yrt) is the position at time step t; θrt is the orientation at time step t; δxr, δyr, and δθr are non-Gaussian noise.


### 5.2.2. Mobile Robot Observation Model

In our experiment, the mobile robot can move under an environment with many features, such as the white line and the two-dimension codes. All the features can be extracted from robot vision information.In this paper, the special two-dimension (2D) codes have been used as the features or landmarks. As shown in Figure4, the outer contour of each 2D code is regular pentagon. A different 2D code has different dots inside the regular pentagon. The 2D code is developed by Institute of Automation, Chinese Academy of Sciences. It is easy to be processed and can bring plenty of information. In [28], the image processing for 2D code is explained in detail.Some examples of 2D code developed by Chinese Academy of Science.
(a)
Example 1
(b)
Example 2
(c)
Example 3
(d)
Example 4In our experiments, each 2D code is different from the others. Each 2D code is located at a fixed position, and it brings the location information. So once the mobile robot detects one 2D code, it can obtain one numerical value of self-localization with the help of image processing.Assume that the robot can detectn two-dimension codes d1,…,dn, then n self-localization value lv1,…,lvn can be obtained accordingly.In our experiment, the mobile robot self-localization result is represented by a set of particles,sjt=〈lpjt,pjt〉, where lpjt means the self-localization value maintained by the jth particle at time step t, and pjt is the importance weight of the jth particle at time step t. Utilizing the mobile robot motion model and lpjt-1, the jth particle can obtain an original self-localization numerical value lpojt. If the robot is localized at lpojt, the distance between lpojt and lvi can be used to calculate the probability P(di∣lpojt-1) with which the robot localized at lpojt can observe the feature di. The probability will become bigger along with the decrease of the distance
(22)P(di∣lpojt-1)=e-|lvi-lpojt|.The extraction of the features just depends on the robot's position and orientation, so the features are mutually independent. The observation modelP(d∣lpojt-1) is formulated in
(23)P(d∣lpojt-1)=P(d1∣lpojt-1)*⋯*P(dn∣lpojt-1)=e-|lv1-lpojt|*⋯*e-|lvn-lpojt|.Actually, the mobile robot can detect at least one feature in our experiment, and no more than two features will be chosen which are the nearest to the robot.


### 5.2.3. The Realization of AUPF for Mobile Robot Self-Localization

In the experiment, the initial number of particles of AUPF is set to be 400. The number of particles during filtering at time instantt is represented by Npt. The process of the realization for mobile robot self-localization is reported as follows.(1)
Obtain the initial position of the mobile robotl0 according to the robot vision.(2)
Aroundl0 a field of 2 * 2 meters is chosen, and 400 particles are uniformly distributed in this field; the importance weight of each particle is 1/400.(3)
Obtain new particles according to UKF algorithm and the mobile robot motion model which utilizes the odometer information.(4)
Obtain the vision information, extract the features, and update the importance weight of each particle according to (17).(5)
Output the estimation of mobile robot position according to (18), then calculate the orientation of the robot according to (24), and then return to step 3 at the next time step.(6)
Calculate the number of particlesNpt.(7)
ResampleNpt particles according to their importance weight.(8)
Set all the importance weight of particles to be 1/Npt.The robot position (xrt, yrt) is obtained by AUPF; then the orientation θrt can be calculated according to
(24)xltr=(xl-xrt)cosθrt-(yl-yrt)sinθrt,yltr=(xl-xrt)sinθrt+(yl-yrt)cosθrt,
where (xl, yl) is the position of one 2D code in the world coordinate, and this position is already known. All the positions used above are in the world coordinate. (xltr, yltr) is the position of the 2D code in the robot coordinate, and this position can be easily obtained by the image processing.


### 5.2.4. Experiment Results and Analysis

In order to illustrate the effectiveness of AUPF, we designed a mobile robot self-localization experiment. In the experiment, the E-Robot randomly moved on the ground, and a trajectory was generated. Then the trajectory was recorded. The trajectory is depicted in Figure5 by a black line. From Figure 5 we can see that the 2D codes are located on the ground as landmarks. When the robot moved, AUPF was run to calculate the robot position at each time step. The effect of AUPF can be measured by the difference between the calculated position and the trajectory.Figure 5
The real trajectory of the E-Robot in the experiment.Then we adopt AUPF, UPF, and PF to obtain the estimation of self-localization value. The estimation results of the three algorithms are shown in Figure6.Figure 6
The estimation results of PF, UPF, and AUPF.The mobile robot self-localization result can be measured by the mean relative position error (MRPE) which is defined in(25)MRPE=∑t=1tf(xtruet-x^t)2+(ytruet-y^t)2tf,
where xtruet and ytruet are the true robot positions in Cartesian coordinates at time step t, tf is the total time instant of the three algorithms, and x^t and y^t are the estimation results.From Figure6 and Table 3 we can see that the MRPE of AUPF is better than PF, and the MRPE of UPF is the best. That is, because in UPF each particle moves according to a new proposal distribution which utilizes the new observation, while in PF the particles cannot move appropriately without the direction of new observation. We also find that AUPF can acquire relatively ideal accuracy. The number of particles in AUPF is reduced, and the reduction is according to the estimation quality, so the reduction will not cause severe loss of accuracy.Table 3
Experiment results of AUPF, PF, and UPF.
Algorithm
AUPF
PF
UPF
MRPE
9.62
18.91
7.86
Average run time (ms)
3.396
2.262
4.635
Average number of particles
298
400
400


## 5.2.1. Mobile Robot Motion Model

The odometer can obtain the offset of robot pose (Δxr, Δyr, and Δθr) from time t-1 to time t, so the motion model of mobile robot can be represented by
(21)xrt=xrt-1+Δxr+Δxr*δxr,yrt=yrt-1+Δyr+Δyr*δyr,θrt=θrt-1+Δθr+Δθr*δθr,
where (xrt, yrt) is the position at time step t; θrt is the orientation at time step t; δxr, δyr, and δθr are non-Gaussian noise.


## 5.2.2. Mobile Robot Observation Model

In our experiment, the mobile robot can move under an environment with many features, such as the white line and the two-dimension codes. All the features can be extracted from robot vision information.In this paper, the special two-dimension (2D) codes have been used as the features or landmarks. As shown in Figure4, the outer contour of each 2D code is regular pentagon. A different 2D code has different dots inside the regular pentagon. The 2D code is developed by Institute of Automation, Chinese Academy of Sciences. It is easy to be processed and can bring plenty of information. In [28], the image processing for 2D code is explained in detail.Some examples of 2D code developed by Chinese Academy of Science.
(a)
Example 1
(b)
Example 2
(c)
Example 3
(d)
Example 4In our experiments, each 2D code is different from the others. Each 2D code is located at a fixed position, and it brings the location information. So once the mobile robot detects one 2D code, it can obtain one numerical value of self-localization with the help of image processing.Assume that the robot can detectn two-dimension codes d1,…,dn, then n self-localization value lv1,…,lvn can be obtained accordingly.In our experiment, the mobile robot self-localization result is represented by a set of particles,sjt=〈lpjt,pjt〉, where lpjt means the self-localization value maintained by the jth particle at time step t, and pjt is the importance weight of the jth particle at time step t. Utilizing the mobile robot motion model and lpjt-1, the jth particle can obtain an original self-localization numerical value lpojt. If the robot is localized at lpojt, the distance between lpojt and lvi can be used to calculate the probability P(di∣lpojt-1) with which the robot localized at lpojt can observe the feature di. The probability will become bigger along with the decrease of the distance
(22)P(di∣lpojt-1)=e-|lvi-lpojt|.The extraction of the features just depends on the robot's position and orientation, so the features are mutually independent. The observation modelP(d∣lpojt-1) is formulated in
(23)P(d∣lpojt-1)=P(d1∣lpojt-1)*⋯*P(dn∣lpojt-1)=e-|lv1-lpojt|*⋯*e-|lvn-lpojt|.Actually, the mobile robot can detect at least one feature in our experiment, and no more than two features will be chosen which are the nearest to the robot.


## 5.2.3. The Realization of AUPF for Mobile Robot Self-Localization

In the experiment, the initial number of particles of AUPF is set to be 400. The number of particles during filtering at time instantt is represented by Npt. The process of the realization for mobile robot self-localization is reported as follows.(1)
Obtain the initial position of the mobile robotl0 according to the robot vision.(2)
Aroundl0 a field of 2 * 2 meters is chosen, and 400 particles are uniformly distributed in this field; the importance weight of each particle is 1/400.(3)
Obtain new particles according to UKF algorithm and the mobile robot motion model which utilizes the odometer information.(4)
Obtain the vision information, extract the features, and update the importance weight of each particle according to (17).(5)
Output the estimation of mobile robot position according to (18), then calculate the orientation of the robot according to (24), and then return to step 3 at the next time step.(6)
Calculate the number of particlesNpt.(7)
ResampleNpt particles according to their importance weight.(8)
Set all the importance weight of particles to be 1/Npt.The robot position (xrt, yrt) is obtained by AUPF; then the orientation θrt can be calculated according to
(24)xltr=(xl-xrt)cosθrt-(yl-yrt)sinθrt,yltr=(xl-xrt)sinθrt+(yl-yrt)cosθrt,
where (xl, yl) is the position of one 2D code in the world coordinate, and this position is already known. All the positions used above are in the world coordinate. (xltr, yltr) is the position of the 2D code in the robot coordinate, and this position can be easily obtained by the image processing.


## 5.2.4. Experiment Results and Analysis

In order to illustrate the effectiveness of AUPF, we designed a mobile robot self-localization experiment. In the experiment, the E-Robot randomly moved on the ground, and a trajectory was generated. Then the trajectory was recorded. The trajectory is depicted in Figure5 by a black line. From Figure 5 we can see that the 2D codes are located on the ground as landmarks. When the robot moved, AUPF was run to calculate the robot position at each time step. The effect of AUPF can be measured by the difference between the calculated position and the trajectory.Figure 5
The real trajectory of the E-Robot in the experiment.Then we adopt AUPF, UPF, and PF to obtain the estimation of self-localization value. The estimation results of the three algorithms are shown in Figure6.Figure 6
The estimation results of PF, UPF, and AUPF.The mobile robot self-localization result can be measured by the mean relative position error (MRPE) which is defined in(25)MRPE=∑t=1tf(xtruet-x^t)2+(ytruet-y^t)2tf,
where xtruet and ytruet are the true robot positions in Cartesian coordinates at time step t, tf is the total time instant of the three algorithms, and x^t and y^t are the estimation results.From Figure6 and Table 3 we can see that the MRPE of AUPF is better than PF, and the MRPE of UPF is the best. That is, because in UPF each particle moves according to a new proposal distribution which utilizes the new observation, while in PF the particles cannot move appropriately without the direction of new observation. We also find that AUPF can acquire relatively ideal accuracy. The number of particles in AUPF is reduced, and the reduction is according to the estimation quality, so the reduction will not cause severe loss of accuracy.Table 3
Experiment results of AUPF, PF, and UPF.
Algorithm
AUPF
PF
UPF
MRPE
9.62
18.91
7.86
Average run time (ms)
3.396
2.262
4.635
Average number of particles
298
400
400


## 6. Conclusions

In UPF the number of particles is fixed, and the computation cost of UPF is relatively large which influences its realtime capability. In order to accommodate the mobile robot self-localization under dynamic environment, an adaptive unscented particle filter algorithm through relative entropy has been proposed in this paper. This algorithm utilizes the concept of relative entropy to measure the distance between the empirical distribution and the true posterior; then the difference between the proposal distribution and the true distribution is offset to obtain the least number of particles of UPF for the next step. This algorithm can effectively reduce the computation cost and enhance the real-time capability for UPF. In the future, we will research on how to further reduce the computation of UPF at the least cost of the accuracy. Moreover, the influence of the accuracy will be investigated and analyzed.



---

*Source: 567373-2013-09-12.xml*