Application of Matrix Pencil Algorithm to Mobile Robot Localization Using Hybrid DOA/TOA Estimation

Abstract

Localization plays an important role in robotics for the tasks of monitoring, tracking and controlling a robot. Much effort has been made to address robot localization problems in recent years. However, despite many proposed solutions and thorough consideration, in terms of developing a low-cost and fast processing method for multiple-source signals, the robot localization problem is still a challenge. In this paper, we propose a solution for robot localization with regards to these concerns. In order to locate the position of a robot, both the coordinate and the orientation of a robot are necessary. We develop a localization method using the Matrix Pencil (MP) algorithm for hybrid detection of direction of arrival (DOA) and time of arrival (TOA). TOA of the signal is estimated for computing the distance between the mobile robot and a base station (BS). Based on the distance and the estimated DOA, we can estimate the mobile robot's position. The characteristics of the algorithm are examined through analysing simulated experiments and the results demonstrate the advantages of our method over previous works in dealing with the above challenges. The method is constructed based on the low-cost infrastructure of radio frequency devices; the DOA/TOA estimation is performed with just single value decomposition for fast processing. Finally, the MP algorithm combined with tracking using a Kalman filter allows our proposed method to locate the positions of multiple source signals.

1. Introduction

In recent years, advances in technology have led to widespread uses of robots in human society. The motivation stems from the ability of robots to perform tasks more cheaply and reliably than humans do in particular task domains. However, the problem of exploring an environment is a significant problem for mobile robots. When a robot executes its tasks, the robot has to answer the following fundamental question, “Where am I?” To answer this, the robot must be integrated with localization capabilities to locate itself with respect to a reference coordinate system while traversing a free-ranging path or navigating to reach a goal.

Localization information is essential for a broad range of mobile robot applications, particularly those where the behaviour of the robot may depend on its position. Thus, localization was early on recognized as being one of the most fundamental problems in mobile robotics and still remains a primary research issue in robotics. From the early days, the use of the global positioning system (GPS) made the problem of localization easier, however, GPS might face possible loss or corruption of GPS signals. Moreover, when the mobile robots are operating indoors, using GPS for localization may be unfeasible leading to the poor accuracy of localization. For instance, Copper et al. [1] took advantage of GPS for land-vehicle localization. Due to the aggregated errors of combining various techniques, it is difficult to obtain high localization accuracy with their method in practical applications. Besides, a careful choice of landmarks needs to be considered to reduce estimation errors and complex systems are required in the case of using GPS devices. Other approaches have been proposed to address localization problems. Location estimation based on odometric measurements in wheeled robots was one of the approaches. These systems calculated the new position of the robot given the initial position and the information about the movement of the robot is provided by odometric sensors. Unfortunately, sensors are typically noisy and errors accumulate over time. This method can be used only for local movements, but is not reliable for long-term or global localization, due to compounding of error.

Several methods have been proposed to cope with odometry limitations. Many are based on finding landmarks of known environmental positions and integrating this information over time to get more reliable position estimation [2]. However, in a real environment a mobile robot faces two problems: i) some angles are measured with small errors and ii) some landmarks are misidentified.

Currently, some research focuses on triangulations as well as Wifi for the localization of a mobile robot. Sugihara [3] addressed a version of the localization problem where the positions of a mobile robot in the workspace are considered identical points. The robot measures the direction of arrival signals transmitted from a position of the robot pierced through an observed point. The authors have developed a new algorithm with O(n³ lgn) complexity to find the position and orientation of a robot such that each signal pierces at least one among n points in the working environment of the robot. Avis and Imai [4], and Krotov [5] extended Sugihara's method into a situation when angle measurements consist of errors. Sutherland and Thompson [6] addressed the localization problem using distinguishable landmarks in the working space of a robot. They showed that the configuration of landmarks plays a critical role in reducing localization errors.

Nowadays, with the advent of new and developing 3D sensing hardware and technology, the 3D localization problem is emerging in mobile robotics. Cahier et al. [7] provided a technique to obtain 3D information of environments by using Time-of-Flight (TOF) camera. However, the use of TOF technology leads to a major drawback in that the TOF camera is highly subjected to frame-to-frame noises, especially on reflective surfaces. Alternatively, two groups have used 3D laser scanners [8, 9]. A 3D laser scanner generates consistent 3D data points within a single 3D scan. For instance, the RESOLV project is aimed at modelling interiors for virtual reality and tele-presence [9] where a RIEGL laser range finder [10] is used on two robots. In [11], the Iterative Closet Points (ICP) algorithm was applied for scan matching and a perception-planning module to minimize occlusions. Alternatively, the AVENUE project developed a robot for modelling an urban environment [8] using a CYRAX laser scanner. They matched 3D scans with camera images to autonomously yield a textured model. Nevertheless, these mentioned techniques are usually manual and computationally expensive, therefore not applicable in real time.

Conventionally, radio signals (RF) are widely used and integrated to a robot for remote control with the advantages of high speed processing and low cost in hardware deployments. Thus, the localization methods developed for robots based on RF signals tends to be applicable in a wide range of practical and daily applications. Previously, there has been an approach using the multiple signal classification (MUSIC) algorithm [12] to address localization problems. In order to estimate both DOA and TOA, different values of DOA and TOA are evaluated by the MUSIC algorithm to draw a 2D plane of pseudo spectrum. Consequently, an appropriate value of DOA and TOA is correspondent to a peak in the pseudo spectrum. Obviously, the drawback of using the MUSIC algorithm is that if a large number of DOA angles are chosen for testing, the computational time needed for localization is prolonged. Otherwise, a small number of an evaluated DOA leads to inaccurate results of DOA estimation.

In this paper, we present an application of the MP algorithm and a hybrid DOA/TOA technique on mobile robot localization. The mobile robot in our system is mounted with a radio frequency (RF) card to transmit radio signals. Another robot or device playing the role of BS is fixed with a uniform linear array (ULA) antenna to receive and estimate the DOA and TOA of a transmitted signal. Let P=[x, y, θ] present the pose of a robot in a given referential system where (x, y) is the coordinates of a robot in 2D and θ is the orientation of the robot. Localization is the process of finding both (x, y) and θ. However, with the geometric relationship between (x, y) and θ depicted in Figure 1, the localization problem turns into the problem of estimating the value of θ and measuring the distance between the robot and BS. To estimate the distance from the robot to BS, we need to know the time that a RF signal transmits from the robot to the BS. This means that TOA needs to be estimated. In this work we introduce a hybrid DOA/TOA technique using the MP algorithm. Different to MUSIC, a DOA/TOA technique using the MP algorithm aims at locating the position of the robot by applying single value decomposition (SVD). In addition, the uses of MP can help us to address the problems of multiple-source signals. Note that each pair of DOA and TOA represents one feasible position of the robot. Thus, the right trajectories of multiple robots are found by combining our proposed algorithm with a Kalman filter to track separated trajectories of different robots. Moreover, additional variables of velocity are integrated into a Kaman filter to formulate the robot transition from one location to another to overcome missing trajectories of the mobile robots.

Figure 1.

Referential coordinate system.

This paper is organized as follows. Section 2 presents our problem statements. Section 3 describes the model of our localization system. Section 4 explains how to apply the MP algorithm for the localization problem. How to use a Kalman filter for the extension of our proposed method is then addressed. Section 5 illustrates the experimental results to validate our proposed method. Finally, Section 6 concludes the paper and presents our future works.

2. Problem Statements

In Margrit et al. [2] raised a question “Why is mobile robot localization important?” When a robot locates itself, it can perform its assigned tasks accurately. With an autonomous robot, some researchers considered it as a human traveller; meanwhile others considered it as a mobile vehicle. In this work, we liken a mobile robot to a mobile cellular. Because the localization problem of a mobile robot is the localization problem of a mobile transmitting RF signal, we attempt to deal with the problem of mobile robot localization based on positioning an RF signal source. The method used for RF positioning is a hybrid TOA/DOA technique with an MP algorithm. The MP algorithm is a novel method used for image and signal processing in mobile localization. In addition, some authors have used the MP algorithm for acoustic localization. This is a good idea for the applications of sound signal positioning in mobile robot localization. Moreover, the MP algorithm can also work with multiple signal sources. Therefore, the MP algorithm is dynamic and suitable in the localization of mobile robotics.

3. System Descriptions

Our localization system consists of a mobile robot mounted with an RF card and a ULA antenna array, depicted in Figure 2 and Figure 3, respectively. The antenna can detect the direction and the TOA signal transmitted by the mobile robot. The robot sweeps K frequency points in the frequency domain. Therefore, we have the bandwidth of the frequency domain given by

Figure 2.

System model.

Figure 3.

ULA antenna model.

$W_{BW}=\mathrm{\Delta }fK$

(1)

When a BS receives the m^th arrival signal, the received signal at a receive array antenna can be modelled as

$y(n,k)={\displaystyle \sum _{m=1}^M}{R}_m{e}^{2j\pi \{(n-1)\widehat{d}\text{cos}{\theta }_m+(k-1)\mathrm{\Delta }f{\tau }_m\}}+w(n,k)$

(2)

where M is the source number, R_m is a residue(complex amplitude), n∈[1,N], N is the number of antenna d̂ = d/λ, d is the inter-element spacing, λ is the wavelength of RF signal, θ _m is heading angle, k ∈ [1,K], τ _m is delay transmission and w(n,k) is Gaussian noise.

4. Matrix Pencil Algorithm

The MP algorithm [13] can be stated as constructing two matrices in such a way that the desired poles are the rank reducing numbers (i.e., the generalized eigenvalues) of the corresponding MP function. A matrix function is called an MP function, if it is formulated as a function of two matrices.

$F\left(t,\beta \right)\hspace{0.17em}=G\left(t\right)–\beta H\left(t\right)$

(3)

where G(t), H(t) are matrices of t and β is a scalar parameter.

We formulate an application of the MP algorithm for DOA estimation. The TOA estimation can be performed similarly. In general, the received signal from a mobile robot to the ULA antenna in DOA estimation can be formulated as

$y_n={\displaystyle \sum _{m=1}^M}{a}^n({\theta }_m)R_m+w_n,$

(4)

where M is the number of transmitted signals, N is the number of antenna elements, y_n is the received signal at an antenna n, a_n = e^{j2πd̂cos(θ _m )} is a directional component, d and R_m are defined as similar to (2), and w_n is Gaussian noise.

Meanwhile, the MP algorithm can help us to express a function y(t) in the sum of complex exponentials as

$y(t)={\displaystyle \sum _{m=1}^M}{R}_m{e}^{s_{m}t}+w(t)\mathrm{,0}\le t\le T$

(5)

where [0, T] is a sampling interval, y(t) a received signal, w(t) is the noise of system and S_m= α _m +jβ_m is a pole - where α_m is a damping factor and β _m is an angular frequency.

The main idea of applying the MP algorithm to estimate DOA and TOA is that we present y(t) as a sequence of N discrete values y(t) = [y₁ y₂…y_N]^T. Alternatively, here y _n is not the values of a signal at time index n, but the data received at an antenna n. In this way the sequence of the nth incoming signal y_n can be written as

$y_n={\displaystyle \sum _{m=1}^M}{R}_m{z}_m^n+w_n$

(6)

where z_m = a(θ_m), similar to equation (4). The MP method begins by choosing a free parameter L, called the pencil parameter (or a moving window length). This number is in range M ≤ L ≤ N – L if N is an even number and in the range M ≤ L ≤ N – L + 1 if N is an odd number. The data of (8) can be written as

$[Y]={\left(\begin{array}{cccc}y(0)& y(1)& \dots & y(L)\\ y(1)& y(2)& \dots & y(L+1)\\ ⋮& ⋮& \dots & ⋮\\ y(N-L-1)& y(N-L)& \dots & y(N-1)\end{array}\right)}_{(N-L)\times (L+1)}$

(7)

From [Y], two matrices [Y₁] and [Y₂] are defined as follows

$[Y_1]={\left(\begin{array}{cccc}y(0)& y(1)& \dots & y(L-1)\\ y(1)& y(2)& \dots & y(L)\\ ⋮& ⋮& \dots & ⋮\\ y(N-L-1)& y(N-L)& \dots & y(N-2)\end{array}\right)}_{(N-L)\times L}$

(8)

and

$[Y_2]={\left(\begin{array}{cccc}y(1)& y(2)& \dots & y(L)\\ y(2)& y(3)& \dots & y(L+1)\\ ⋮& ⋮& \dots & ⋮\\ y(N-L)& y(N-L+1)& \dots & y(N-1)\end{array}\right)}_{(N-L)\times L}$

(9)

[Y₁] and [Y₂] are equivalent as

$\begin{array}{c}[Y_1]=Y(:\mathrm{,1}:L)\\ [Y_2]=Y(:\mathrm{,2}:L+1)\end{array}$

(10)

or

$\begin{array}{c}[Y_1]=[Z_1][R][Z_2]\\ [Y_2]=[Z_1][R][Z_0][Z_2]\end{array}$

(11)

where

$\begin{array}{l}[Z_0]=diag[z_1,z_2,\dots ,z_M]\\ [R]=diag[R_1,R_2,\dots ,R_M]\end{array}$

(12)

$[Z_1]={\left(\begin{array}{ccc}1& \dots & 1\\ z_1& \dots & z_M\\ ⋮& \dots & ⋮\\ z_1^{N-L-1}& \dots & z_M^{N-L-1}\end{array}\right)}_{(N-L)\times M}$

(13)

$[Z_2]={\left(\begin{array}{ccc}1& \dots & z_1^{L-1}\\ 1& \dots & z_2^{L-1}\\ ⋮& \dots & ⋮\\ 1& \dots & z_M^{L-1}\end{array}\right)}_{M\times L}$

(14)

Now, we consider the matrix pencil function

$[Y_2]-\beta [Y_1]=[Z_1][R]\left\{[Z_0]-\beta [I]\right\}[Z_2]$

(15)

Here [I] is an M × M identify matrix. The poles z_m can be found as the eigenvalues of the matrix pencil function by using a singular value decomposition operator.

Therefore, by let k=1 in equation (2), the DOA of received signals can be estimated by

${\theta }_m={{\displaystyle \text{cos}}}^{-}1\left[\frac{\Im [ln{z}_m]}{2\pi \widehat{d}}\right]$

(16)

where ℑ is the imaginary part operator of z_m. In a similar way, we can set the value of n = 1 in equation (2) to estimate the transmission time of signals

${\tau }_m=\frac{\Im [ln{z}_m]}{2\pi \mathrm{\Delta }f}\mathrm{.}$

(17)

Based on estimated TOA, the distance between the mobile robot and BS can be measured by

$r=c{\tau }_m$

(18)

where c=3.10⁸m/s is the velocity of light and r is the distance between the mobile robot and BS. The position of the mobile robot is determined as

$\left[\begin{array}{c}x\\ y\\ \theta \end{array}\right]=\left[\begin{array}{c}r\cos {\theta }_m\\ r\sin {\theta }_m\\ {\theta }_m\end{array}\right]$

(19)

For the methods presented throughout this section, the position of a single source signal can be found without extensions. However, considering situations where the antenna receives signals from multiple sources, the localization problem tends to be more complex. Obviously, applying DOA estimation as above, we can calculate m values of DOA of incoming signals, θ₁, θ₂ …, θ_m. In the same way, there will be m values of estimated TOA τ₁, τ₂,…, τ_m available. Because the combination order of DOA and TOA values is not known in advance, there will be m² combination of DOA/TOA pairs: (θ₁,τ₁), (θ₁,τ₂),…(θ_mτ_m) rather than just m combination of DOA/TOA pairs (θ₁,τ₁), (θ₂,τ₂),…, (θ_m,τ_m) as expected.

In this paper, to address the above disadvantages, we suggest a solution using a Kalman filter [14] to track the movements of different robots. Correspondingly, the transition of a robot from one location to another can be monitored, eliminating the ambiguous combinations of TOA and DOA values.

Let X =[x,y,ẋ,ẏ] be the state variables of a robot. Here, considering the moving directions and the velocity of a robot, in addition to the coordinate, the deviations(ẋ,ẏ) of (x,y) are added into the state vector. Let Δt be the interval distinguishing the two consecutive times of sending RF signals by a robot. If the value of heading angle θ changes rapidly with regards to the changes in t, the state vector is also varied from t to t + Δt. Therefore, every time a robot is driven to another direction with a high angular velocity, we need to increase the rates of sending RF signals from a robot to a BS. As consequently, increasing the rates of sending RF signals makes the changes of θ smaller.

From the hidden states X of a robot, the observation calculated by the MP algorithm is given by Z = [x̂,ŷ]. The value of X and Z presented at a specific time index k is described by notations X_k and Z_k. The relationship between X_k, X_k-1, and Z_k is expressed by

$\begin{array}{l}{X}_k=F{X}_{k-1}+w\\ Z_k=H{X}_k+v\end{array}$

(20)

where w and v are zero mean Gaussian noises with covariance matrices Q and R respectively, and the two matrices F and H are defined as

$\begin{array}{l}F=\left[\begin{array}{cccc}1& 0& \mathrm{\Delta }t& 0\\ 0& 1& 0& \mathrm{\Delta }t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\\ H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\end{array}$

(21)

The predictions are performed by

$\begin{array}{l}{X}_{k|k-1}=F{X}_{k-1}\\ P_{k|k-1}=F{P}_{k-1}{F}^T+Q\end{array}$

(22)

Meanwhile, a set of update equations are given by

$\begin{array}{l}{S}_k=H{P}_{k|k-1}{H}^T+R\\ K_k=P_{k|k-1}{H}^T{S}_k^{-1}\\ X_k=X_{k|k-1}+K_k(Z_k-H{X}_{k|k-1})\\ P_k=(I-K_{k}H)P_{k|k-1}\end{array}$

(23)

In order to track multiple robots, each Kalman filter is used to monitor the path of each robot. Prediction equations given in (22) show the next location of a robot with regards to the moving time Δt. Using predicted locations of a robot, a set of location candidates closest to these locations are retained. The remaining candidate locations of robots are eliminated from the computations, helping us to locate the positions of a robot accurately.

5. Experimental Setup and Results

The performance of applying an MP algorithm on DOA/TOA estimation is evaluated in this paper through simulations. As shown before, the TOA estimation is mostly similar to the DOA estimation, but in DOA the changes in the phases are caused by the delays of incoming RF signals in different antenna elements. In TOA, the phase changes are related to the frequency changes of different incoming RF signals. Thus, in order to compare the efficiency of both the MP and MUSIC algorithms on localization, focus is placed on the DOA estimation with a single source in Section 5.1. In Section 5.2, the overall performance of our DOA/TOA estimation with a single source signal is tested with various situations. Finally, in Section 5.3 we extend the proposed algorithm into a general case of multiple source localization.

5.1 Comparisons between MP and MUSIC on DOA Estimation

In this work, the MP algorithm has been implemented to compute the DOA of RF signals. We compute the values of DOA with regards to the changes of signal-to-noise ratio (SNR) and positions of robots. Here, a robot is randomly distributed in a 2D plane in such a way that the heading angle created by the robot and the BS antenna obtains a value among 45, 55, 65 and 75 degrees. Simulated signals emitted by a robot are generated based on equation (2) with the addition of the Gaussian noises at the SNR level. The experiment is performed 1000 times and the mean of estimated DOA and the deviations between the estimated DOA and the ground-truth DOA are calculated and described in Table 1. The results in Table 1 show that the use of the MP algorithm ensures a high accuracy of estimating exact DOA values of RF signals, especially of 20dB to 30dB SNR signals. Typically, the radio signals in real environments contain noises with an SNR of 30dB. Therefore, this shows the potential to deploy the proposed algorithm in real and practical applications.

Table 1.

DOA estimation by the MP algorithm with a single source signal.

Ground truth θ (°)	SNR	Mean DOA (°)	Deviation (°)
45	20	45.0014	0.0014
55		55.0007	0.0007
65		64.9955	0.0045
75		74.9994	0.0006
45	25	44.9949	0.0051
55		54.9956	0.0044
65		65.UU35	0.0035
75		74.993U	0.0070
45	30	45.UU37	0.0037
55		54.9982	0.0018
65		64.9995	0.0005
75		74.9995	0.0005

We extend our work with performance comparisons between the MP and MUSIC algorithms on the localization problems. The MUSIC algorithm [12] (as can be seen in Appendix A) has been utilized so far in RF localization where different values of DOA are tested to search for the best compatible angle correspondent to a peak in a 2D plane of pseudo spectrum. We test the performances of MUSIC and MP on DOA estimation. An aforementioned method is used to create simulation environments. However, here the heading angles between the robot and the BS location randomly obtain a value among 50, 60, 70 and 80 degrees rather than among 45, 55, 65 and 75 degrees as performed in the previous experiment. In terms of accuracy, the MP algorithm can estimate the DOA values with an RMSE approximately equivalent to that of the MUSIC algorithm as illustrated in Figure 4. However, from the computational time aspect, the MP algorithm is superior to the MUSIC algorithm as shown in Table 2. The use of just one singular value decomposition aims at reducing the processing time of our MP-based localization method.

Table 2.

Computational time of MP and MUSIC on DOA estimation.

Arrival angle (θ)	50°	60°	70°	80°
Average computational time of MP	9.7 × 10−4 s	7.4 × 10−4 s	9.1 × 10−4 s	7.6 × 10−4 s
Average computational time of MUSIC	1.07 × 10−1 s	1.06 × 10−1 s	1.07 × 10−1 s	1.05 × 10−1 s

Figure 4.

RMSE of MP and MUSIC algorithm with DOA estimation with regards to different ground truth heading angles (a) 50o, (b) 60o, (c) 70o and (d) 80o.

5.2 Hybrid DOA/TOA Estimation

To determine the coordinate of a point in 2D space, both the distance to the point and the angles created between the point and x-or y-axis need to be known. Computing these two values is equivalent to estimating the TOA and DOA of RF signals emitted from the point. The method to compute the TOA value is extended from the method to compute DOA value as illustrated in Section 4. An equation to determine the location of a point from TOA and DOA is given by (16) and (17) respectively. Different to Section 5.1, we assume that a robot moves in a continuous trajectory as depicted in Figure 5. The moving trajectory of a robot is created randomly by using the random-walk mechanism: at each location, the robot will continuously move one unit length in four directions: move up, move down, move left or move right with an equal probability of ¼. An averaging filter is then applied to make the trajectory smooth. Finally, based on the assumptions of the moving velocity of the robot, a set number of points along the defined trajectory are selected to draw the trajectory of a robot as depicted by a dot-line in Figure 5. Note that more than one hundred points have been used for a simulated trajectory.

Figure 5.

Comparison between RMSE of hybrid DOA/TOA and TOA estimation using multiple beacons, (a) Hybrid DOA/TOA, (b) TOA estimation with three beacons, and (c) TOA estimation with four beacons.

Correspondingly, applying the hybrid DOA/TOA algorithm, we draw an estimated trajectory of a robot in a red colour within the same figure and calculate the root mean square error (RMSE) of our proposed method. A particular number of points of the estimated trajectory marked with O are shown to compare them with their real locations marked with X along the ground-truth trajectory.

Previously, there have been algorithms developed to find a potential region of a robot based on the interference of several circles if the distances from a robot to beacons are known. The drawback of that method is the requirement to know the location of at least three beacons and calculate the distances to them by sending signals. Here, in Figure 5 we show that our proposed hybrid DOA/TOA estimation can achieve high accuracy of localization by using a single reference (a beacon).

In the experiment, three beacons and four beacons are setup in several places to locate a mobile robot. The distances from a robot to different beacons are estimated from TOA given by equation (17). A level of noise of 10dB is assumed to be the same in all RF signals. Due the effects of noise, rather than just a single point, a set of points forming a region where a robot can be found are described in Appendix B. In this paper, a centre of this region can be assumed to be the point closest to a mobile robot. All experiments are repeated 20 times and the means and standard deviations of RMSE are estimated by the methods using DOA/TOA, TOA with three beacons and TOA with four beacons (see Table 3). From the experimental results, hybrid DOA/TOA estimation can obtain the same accuracy as that acquired by the TOA methods using multiple beacons, although when increasing the number of beacons, the location of a mobile robot can be determined more exactly. Alternatively, the proposed approach with a hybrid DOA/TOA utilizes just a single reference and is thus more advanced compared to the TOA methods based on multiple beacons.

Table 3.

Means and standard deviations of RMSE estimated by the methods using DOA/TOA, using TOA with three beacons and using TOA with four beacons.

Method	DOA/TOA	TOA with 3 beacons	TOA with 4 beacons
Mean RMSE (m)	0.42	1.03	0.81
Standard deviation (m)	0.009	0.012	0.010

5.3 Multiple Source Localization with a Kalman Filter

From the equation expanded in Section 4, the MP algorithm has been generalized with a Kalman filter to solve the localization problems with multiple source signals. The trajectories of two robots are defined in Figure 6 with the moving of robots in two opposite directions. Here, a mechanism similar to Section 5.2 is utilized again to simulate the moving trajectories of the two robots. Our experiment is repeated 20 times to allow us to calculate the mean and standard deviation of the estimated RMSEs.

Figure 6.

Multiple robot localization where X is used to mark the real robot location and O is used to mark the estimated robot location.

The experimental results show the proposed DOA/TOA algorithm still obtains high localization accuracy with an average RMSE= 0.71 and a standard deviation σ= 0.02, even when a BS receives signals from multiple locations.

6. Conclusions and Future Works

In this paper, we propose a promising algorithm using the MP algorithm for estimating both the position and orientation of the mobile robot given noisy input signals. The MP algorithm has been conventionally applied in a communication network, but not widely used in other practical applications, especially in mobile robot localization. This work presents a new way of using the MP algorithm to estimate both the DOA and TOA simultaneously of a mobile robot for the localization problem. The proposed system requires only one reference signal, while the conventional systems based on popular geometric method generally deploy more than three reference signals. Moreover, the proposed method requires less complexity of computations as compared with the MUSIC algorithm and can work with multiple received signals. The experiment results proved that the proposed algorithm has performed well with the signals containing noises in different levels. In cases of multiple signals localization, our proposed method can estimate two incoming signals with small differences. In other words, the method of using the MP algorithm for DOA estimation can locate two mobile robots at two positions, in close proximity to each other. Therefore, there is an opportunity to utilize the proposed method in a mobile robot team. In addition, the proposed method may be applied to address mobile robot localization with the requirement of high accuracy localization.

In future work, we plan to apply the MP algorithm using hybrid DOA/TOA for the localization problems of a mobile robot team. Additionally, the MP algorithm is very effective for exponential signal and thus suitable for sound signal processing. The MP algorithm is applicable for mobile robot localization based on sounds. Finally, we will make the building of a local map for a mobile robot based on estimated positions as a part of our next work.

7. Appendix A: Multiple Signal Classification (MUSIC)

In this appendix, the application of the MUSIC algorithm to estimate an appropriate value of heading angles of RF signals coming from a robot is presented. Assume that the signal received at an antenna is presented by y(t). Correspondingly, a correlation matrix of input signals is given by R_yy=yy^T. From the correlation matrix R_yy, M-p eigenvectors v_i found from the correlation matrix correspondent to M-p smallest eigenvalues of R_yy will be the basic components of noises in a null space. An appropriate value of heading angle θ will make a vector

$e=[\begin{array}{ccccc}1& e^{-j\pi \sin \theta }& e^{-j2\pi \sin \theta }& \dots & e^{-jm\pi \sin \theta }\end{array}]$

constructed from θ orthogonal to these M-p eigenvectors.

${\displaystyle \sum _{i=p+1}^M{\left|e^H{v}_i\right|}^2\to 0}$

(24)

where M is the size of y(t) and p is an order of the algorithm defined manually. However, the hard constraints defined by (24) cannot always be satisfied with real signals. Given that noises cannot be totally uncorrelated to the main signal components, in practice, a constraint in (24) is relaxed by a loose constraint on a heading angle. A value of heading angle will be chosen to maximize the inverse value of ${\displaystyle \sum _{i=p+1}^M{\left|e^H{v}_i\right|}^2}$ instead.

8. Appendix B: TOA Localization with Multiple Beacons

The localization method in a communication network is conventionally performed by multiple reference signals emitted by multiple beacons. The distances from a mobile robot to at least three beacons are calculated by the TOA of arrival signals. Assume that there are N beacons available. For a point of a mobile with a coordinate (x,y), we can estimate N distance d₁,d₂,…,d_N (TOA estimation) to N beacons. For each pair of beacons, we are able to draw two circles to find their intersections with two points, assuming A and B respectively. However, which one of the two points will be the closest to the position of a mobile? From the two points A and B, the distances to N beacons are calculated, assuming d₁ ^A ,d₂ ^A ,…,d_N^A and d₁ ^B ,d₂ ^B ,..,d_N^B accordingly. One point between A and B is selected to minimize the total mean square error

$\begin{array}{l}rms{e}^A=\sqrt{(d_1-d_1^A)+(d_2-d_2^A)+\dots +(d_N-d_N^A)}\\ rms{e}^B=\sqrt{(d_1-d_1^B)+(d_2-d_2^B)+\dots +(d_N-d_N^B)}\end{array}$

(25)

or equivalently from two beacons we can find one point closest to the location of a mobile. In general, from N beacons, we are able to find maximum N/(N-1)/2 points possibly closest to the mobile locations. The mean of all the points can be used to determine the position of a robot approximately as depicted in Figure 7.

Figure 7.

Finding a centre point.