International Journal of Scientific & Engineering Research, Volume 4, Issue 12, December-2013 65

ISSN 2229-5518

The Effect of Features Number on

Extended Observation-Cooperative SLAM

Gayas Asaad, Alireza Babaei, Mohammad H. Ferdowsi, Hossein Bolandi

Abstract— This paper investigates the effect of the number of environment features on a cooperative approach of simultaneous localization and mapping (SLAM). The tested cooperative SLAM approach is the Extended Observation-Cooperative SLAM (EO-CSLAM) algorithm which depends on additional, indirect correlated observations of the features (landmarks). The performance gain due to additional correlated observations means that additional features will have similar positive effect. However, as EO-CSLAM adopts extended Kalman filter-simultaneous localization and mapping (EKF-SLAM) solution, the number of environment features will have an important role in the computational burden. Simulation results show that the performance gain provided by EO-CSLAM is more obvious in tha less features cases.

Index Terms— Autonomous Navigation, Cooperative SLAM, Unmanned Surface Vehicles.

—————————— ——————————

hatever the task of an autonomous vehicle is, the accurate localization has an essential role for efficient achievement

and accurate results. For instance, in underwater environmental modelling, accurate navigation is very important to provide sensors with referential transformations matrix and high-accuracy

tween the local submaps, while the non-common features do not contribute to the improvement due to the uncorrelated nature of local submaps.

Consequently, in the case of absence of common features in the overlapped areas, which isa possible situation in large-

IJSER

pose [1]. On the other hand, while GPS can be used for

localization, its data can be inaccurate or inaccessible due to many possible reasons, such as atmospheric changes, noisy environments, multi-path errors, deliberate jamming, spoofing or confined areas where observing sufficient number of satellites can be difficult [2]. To solve these problems, simultaneous localization and mapping (SLAM) framework can be a proper alternative or incorporated to GPS [3]. Moreover, exploiting SLAM in a cooperative approach can provide more improvement in localization accuracy of USVs, in addition to the mentioned advantages of cooperative manners.

For the cooperative SLAM implementation with USVs, a Multi-USV-based CSLAM approach has been proposed in [5] using laser sensors. The research adopted the Constrained Local Submap Filter (CLSF) approach, which had been presented in [6] and [7] to improve computational efficiency and data association. In CLSF approach, local submaps are fused periodically into a single global map; the common (duplicated) feature estimates are processed by a constraining operation as a weighted projection to produce a recovered estimate for each common feature. Therefore, in theCLSF- based cooperative SLAM, the performance gain (versus the single-USV case) depends only on the common features be-

————————————————

• *Dr. Alireza Babaei, Department of Aerospace, Amirkabir University of*

Technology, Iran. E-mail: arbabaei@aut.ac.ir

• *Dr. Mohammad H. Ferdowsi, Department of Electrical Engineering, Malek*

ashtar University of Technology, Iran. E-mail: ferdowsi@mut.ac.ir

• *Dr. Hossein Bolandi, School of Electrical Engineering, Iran University of*

Science and Technology. . E-mail: h_bolandi@iust.ac.ir

scale environments, there will not be improvement in localiza-

tion accuracy and mappingperformance, and CLSF-based

CSLAM will act the same level of accuracy of Mono-SLAM or

lower.

In our previous work [8], the Extended Observation-

Cooperative SLAM (EO-CSLAM) has been presented, this

approach allows vehicles to improve localization accuracy and

mapping performance even with no common features, profit-

ing from all observed features, which makes it a proper meth- od for USVs using radar sensors.

In this paper more investigation on this algorithm is pre- sented showing the effect of environment features number on the resulting performance gain and computational complexity.

This paper is organized as follows: Section 2 provides the

general framework for the EO-CSLAM approach and its formulation, while section 3 evaluates (using simulations) the effect of features number on the performance gain and compu- tational burden. Finally, conclusions are presented in section 4.

In the general framework for the EO-CSLAM algorithm, it is assumed that a team of vehicles perform a collective task in an unknown environment. While moving through the environment, the vehicles use their sensors to obtain relative observations (measurements) of the features and vehicles within their fields of view (FOV). Assuming that the collaborating vehicles have the ability to share required information, each vehicle shares its observations and control signals with the collaborating vehicles.

For the marine environments case considered in this paper,

the collaborating vehicles are a team of USVs with radar

IJSER © 2013 http://www.ijser.org

International Journal of Scientific & Engineering Research, Volume 4, Issue 12, December-2013 66

ISSN 2229-5518

sensors. To demonstrate the algorithm, let’s consider the case of two USVs denoted a and b as shown pictorially in Fig.1

involving only three features for simplifying. The features on

updated values, for k = 1,2,3...

Prediction:

sea surface can be artificial and/or naturally occurring**s**ˆ a − =

(**s**ˆ a + , **u**a ,**0**)

(2)

elements [4], [5].Considering the surface planar motion, the

start points of vehicles are firstly stored as initial positions

k f k − k −

with respect to a single global reference frame (XOY). Each

a − =

pk −1

ppk −1

pk −1

a +T

k −1

aT

k −1

k −1

pk −1

a +

pMk −1

(3)

vehicle initializes its global map in this frame with zero initial position uncertainty and then, continues (through the

where a

a

unknown environment) observing the features and vehicles in

Fpk −1 and L k −1 are the Jacobeans of motion model [9]:

its FOV. The relative observations and control data are shared between the vehicles; the shared features' observations are

As control data **u **v

b−

are shared, and having ( **s**ˆ b+ ,

b−

b+

k −1

), the

used with the vehicle-vehicle (v-v) observations to generate

vehicle a obtains sˆ k

and **C**k

of *b *using (2) and (3), so the v-

additional correlated observations, such as**z** ab = **z** a + **z** b in

v observation a

1 *b *1

Fig.1, while the shared control data are used by each USV to

zb is distinguished via data association (being

assigned to **q**ˆ b − of **s**ˆ b− ), and the shared local observations **z **v

k k ik

estimate and update the second vehicle's location. The

additional correlated observations are called extended observations (EO). Next subsection explains the details of the EO-CSLAM algorithm.

are used to obtain extended observations. See [8] for more details.

For all these types of observation, data association is performed: for each observation associated with previous

mapped feature, the innovationν a

and its covariance Eik are

FOV of a

Y f2

computed as follows:

For local observations **z **a :

IJSER

**a **a a ˆ a

a (ˆ a − , ˆ a )

z**2 **ν ik = zik − zik = zik − h sk mk

ba

2

(4a)

ab a

a − aT

a

Eik = Hik Ck

Hik

+ **R **k

(5a)

ab zb b

b f1

1

For extended observations **z **ab :

a ab

ˆ ab

ab (ˆ a − , ˆ a )

a ba

0 0

ν ik = zik − zik

= zik − *h *sk mk

(4b)

b a a − aT e

f0 z0

Eik = H ik Ck

H ik

+ **R **ik

(5b)

FOV of b

where **H **a

= ∂*h *∂**s **is the Jacobean of the observation model *h *:

O X a

∂*h *∂*h *

Fig. 1: EO-CSLAM by two USVs (*a*, *b*), solid arrows refer to local observations (of features in the FOV) while dashed arrows refer to

extended observations, such as ab .

Hik = 01

∂**p **pˆ a −

Kalman gain:

.. **0**i −1

∂**f **ˆ a ik

0i +1

.. **0 **Nak

(6)

z1 a =

a − aT −1

K ik

Ck Hik Eik

(7)

2.1 EO-CSLAM algorithm formulation

The main quantities and their used symbols are defined in [8]

Update:

a + a − a a

together with way of obtaining extened observations.

sˆ k

= **s**ˆ k

+ K ik ν ik

(8)

Therefore, it is enough (for this paper’s goal) to review the

a + =

a − − a

vT =

− a a a −

main EKF recursive procedure.

Ck Ck

K ik Eik K ik

(I K ik Hik )Ck

(9)

Initialization:

**s**ˆ a + = **p**ˆ a + = [ *x*ˆ a + , *y*ˆ a + , φˆa + ]T

(1)

while for each observation belongs to a new observed feature,

the location estimate fˆ a is computed as follows:

0 0 0 0 0

− *x*ˆ a− + *r*

.cos(φˆa− + θ )

with covariance

Ca + = Ca +

, where no features have been

fˆ a

= *g *(**p**ˆ k

, **z**ik ) = k ik

k ik a −

(10)

0 *pp*0

ik

*y*k

+ *r*ik .sin(φˆ

+ θik )

mapped yet ( **m**a = ∅). The vehicle a stores the initial data of

T a ab

both vehicles, i.e. ( **s**ˆ a+ , **C**a + ) and ( **s**ˆ b+ , **C**b + ). In the following,

where **z**ik = [*r*ik ,θik ]

represents zik *or *zik , and then, the new

0 0 0 0

a a− a −

the notation (⋅) − refers to predicted values, while (⋅) +

refers to

estimate is combined to

mˆ k

within

sˆ k

, while **C**k is

k k

IJSER © 2013 http://www.ijser.org

International Journal of Scientific & Engineering Research, Volume 4, Issue 12, December-2013 67

ISSN 2229-5518

augmented, thus

designed a simple virtual marine environment so that allows inserting point features and pre-planned trajectories to be fol-

a+ [

a− T

aT ˆ aT ]T

lowed by the USVs. Fig. 2 shows an example of

sˆ k

= **(p**ˆ k )

**C**a −

, **m**ˆ k

Ca −

, **f**ik

(J a Ca −

)T

(11a)

a 10km × 10km area with an arbitrary coast line and ten

ppk

pMk

p ppk

inserted features. The point features are assumed to be

Ca + = Ca − T

Ca −

(J a Ca −

)T

(11b)

outputs of signal processing algorithms [3] and extraction

k pMk

J a Ca −

MMk

J a Ca −

p pMk

Ca +

routines that detect the point targets while suppress land

p

where

ppk

p pMk

iik

a

reflections [4]. Two USVs ( a and b ) were inserted and driv- en along two closed trajectories of about 1.7 km-long. Dashed circles show the range/field-of-view FOV of each vehicle at

J a =

∂*g*

∂**p**a

1 0

=

0 1

− *r*ik .sin(φk

r .cos(φ a

+ θik )

+ θik )

(12)

the start time. The 5km-range of radar sensor was chosen

agreeing with actual detection capability on the sea surface

such as in [4].

and

a +

iik

is computed for local observations using R k :

a + a a − aT a aT

(13a)

Ciik

= J p C ppk J p

+ J z R k J z

and using **R **e

for extended observations

a + a a − aT

a e aT

(13b)

Ciik = J p C ppk J p

where

+ J z R ik J z

a

a a

a __ __ ∂g

cos(φk

+ θik )

− *r*ik .sin(φk

+ θik ) **b**

J z =

= a

a (14)

∂**z**ik

sin(φk

+ θik )

rik .cos(φk

+ θik )

and so, the next period ( k + 1 ) starts with (2) and (3)

continuing with the same manner.

2.2 Computational complexity

It has been mentioned that the Constrained Local Submap Filter (CLSF), which is an effective method for reducing the computational burden in small-scale environment, will not be useful for USVs with long-range sensors such as radar where the difference between the local and global map will be small. Therefore, among the methods used for reducing the compu- tational burden of SLAM algorithms, the state augmentation technique has been adopted in the EO-CSLAM algorithm, see (8) and (11 a,b); this technique reduces both the EKF predic- tion step and the process of adding new feature from calcula- tions with cubic complexity in the number of features to calcu- lations that are linear [10]. However, due the decentralized manner of EO-CSLAM and performing the SLAM procedure in each vehicle for additional observations increases the com- putational complexity of the algorithm. Section 3 compares the computational burden of EO-CSLAM with that of Mono- SLAM and illustrates the features’ number effect on the per- formance gain.

-200

-220

-240

-260

-280

-300

-320

1360 1380 1400 1420 1440 1460

Fig. 2: A simple marine environment including two USVs (*a *and *b*) on two closed trajectories (zooming in on *b *at the start point) with 10 features.

First, both Mono-SLAM and EO-CSLAM have been per- formed for the case of Fig 2, but with 6 features rather than 10. Considering the norm of positional error covariance as a total criterion for SLAM performance, Fig. 3 illustrates a comparison of norms of position covariance in both cases, Mono-SLAM and EO- CSLAM. It is obvious that the uncertainty in the vehicle position estimate via EO-CSLAM decreases noticeably versus Mono- SLAM. This improvement is due to the additional correlated fea- ture estimates generated by each vehicle through the extended observation way. Fig. 3 shows also the improvement ratio ( IR ) for each USV, which represents the reduction in the uncertainty as a percentage of the Mono case, defined as

This section demonstrates (using simulation) the positive role of additional features and observations on the localization accuracy using SLAM, in addition to their negative effect on computational burden, and how will be their role in the im- provement of EO-CSLAM approach.

In order to perform SLAM algorithm simulation, we have

|| **C**qq ||Mono − || **C**qq ||EO

IR = × 100 %

|| **C**qq ||Mono

(15)

IJSER © 2013 http://www.ijser.org

International Journal of Scientific & Engineering Research, Volume 4, Issue 12, December-2013 68

ISSN 2229-5518

the vehi cle a

8

6

4

Mono

EO

the vehi cle a

the vehi cle a

6

4

Mono

EO

the vehi cle a

2

2

0

50

40

30

20

10

0

0 500 1000 1500

Time [se c]

0 500 1000 1500

Time [se c]

0

50

40

30

20

10

0

0 500 1000 1500

Time [se c]

0 500 1000 1500

Time [se c]

Fig. 3: Comparison of two vehicles’ norm of covariance in Mono-SLAM

case with EO-CALM, and the improvement ratio for 6-Feature case.

Next, the process has been repeated for 10-feature case and

40-feature case (Fig. 4). Fig. 5 illustrates the comparison for the

Fig. 5: Comparison of two vehicles’ norm of covariance in Mono-SLAM

case with EO-CALM, and the improvement ratio for 10-Feature case.

10-feature case, and Fig. 6 illustrates the 40-feature case. 3

Considering tha only the norm of vovariance and caomparing

it between the three cases (6, 10, and 40 features), it can be seen

that the increment in features number reduces the localization 1

errors for both, Mono-SLAM and EO-CSLAM, and despite that

EO-CSLAM is always better than Mono-SLAM, the performance 0

gain will be higher for the less features case; notice that the im-

the vehi cle a

Mono

EO

the vehi cle a

IJSE30 R

provement ratio (*IR*) has reached 50% for the 6-feature case, while

for the 10- and 40-feature cases reached about 40% and 30%,

respectively. Furthermore, due to the correlation between the

vehicle postion eastimate and features locations estimates, the same effect and comparison will be satisfied for the mapping per-

20

10

0

0 500 1000 1500

Time [se c]

0 500 1000 1500

Time [se c]

formance.

Fig. 6: Comparison of two vehicles’ norm of covariance in Mono-SLAM

case with EO-CALM, and the improvement ratio for 40-Feature case.

On the other hand, in the purpose of evaluating the computa- tional burden effect of the EO approach and its components, the running time elapsed (in MATLAB) for a single recursion period has been extracted for each method: Mono and EO-CSLAM.* a *Fig. 7 shows this time for the vehicle b . As expected, the run-

b ning time increases in the cooperative approaches where a great- er amount of information is processed; while Mono-SLAM has the lowermost running time due to the least amount of shared

data, EO-CSLAM takes longest time. Thus, since the increment in processed data in SLAM increases the computational burden, more features means longer computating time for both Mono- SLAM and EO-CSLAM, and additionally, the longer computat- ing time is the price of the performance gain obtained using EO- CSLAM.

Fig. 4: The same marine environment case with 40 features.

IJSER © 2013 http://www.ijser.org

International Journal of Scientific & Engineering Research, Volume 4, Issue 12, December-2013 69

ISSN 2229-5518

Fig. 7: Running time elapsed (in MATLAB) for a single recursion period for the 10 features’ case and vehicle b .

This paper has explaind the role of features number in the

REFERENCES

[1] H. Ferreira, C. Almeida, A. Martins, J. Almeida, A. Dias, G. Silva, E.

Silva “Environmental modeling with precision navigation using ROAZ autonomous surface vehicle,” in *Proc. IROS – Intelligent Robots and Systems, Workshop on Robotics for Environmental Monitoring*, Porto, Portugal, 2012.

[2] J. C. Leedekerken, M. F. Fallon and J. J. Leonard. “Mapping Complex

Marine Environments with Autonomous Surface Craft”, Massachu-

setts Institute of Technology, 77 Massachusetts Avenue, Cambridge,

MA 02139, 2010.

[3] G. Dissanayake, P. Newman, S. Clark, H.F. Durrant-Whyte, and M.

Csorba. “A solution to the simultaneous localisation and map build- ing (SLAM) problem,” *IEEE Trans. Robotics and Automation,*

17(3):229-241, 2001H. Poor, “A Hypertext History of Multiuser Di- mensions,” MUD History, http://www.ccs.neu.edu/home/pb/mud- history.html. 1986. (URL link *include year)

[4] J. Mullane, S. Keller, A. Rao, M. Adams, A. Yeo, F. Hover‡ and N.

Patrikalakis. “X-band Radar based SLAM in Singapore’s Off-shore,”

in Proc. IEEE, Int. Conf. Control, Automation, Robotics and Vision,

2010, pp. 398-403.

[5] M. Moratuwage, W. S. Wijesoma,B. Kalyan, J. Dong,and P. Namal

Senarathne. “Collaborative Multi-Vehicle Localization and Mapping in Marine Environments”, in Proc IEEE, Oceans’10, 2010.

[6] S.B. Williams. “Efficient Solutions to Autonomous Mapping and

IJSER

performance gain of the extended observation-cooperative SLAM

(EO-CSLAM). This role appears in two contradictory effects, a positive effect on the localization accuracy and mapping performaence (more features cause more accuracy for both Mono and EO-CSLAM), while the negative effect is the computational burden which incresea with the number of observed feature. While EO-CSLAM is always better than Mono-SLAM, the performance gain will be higher for the cases with less number of environment features.

Navigation Problems”, PhD thesis, University of Sydney, Australian

Centre for Field Robotics, 2001.

[7] S.B. Williams, G Dissanayake, and H Durrant-Whyte. “An Efficient

Approach to the Simultaneous Localisation and Mapping Problem”,

IEEE Proceedings. ICRA'02- Robotics and Automation, 2002.

[8] G. Asaad, A. Babaei, M. H. Ferdowsi, H. Bolandi, “Extended Obser-

vation-Cooperative SLAM for Unmanned Surface Vehicles.” Interna- tional Journal of Scientific & Engineering Research (IJSER), Vol. 4, Is- sue 8, (pp. 792–800) August 2013.

[9] Dan Simon, Optimal State Estimation, John Wiley & Sons, Inc., Ho-

boken, New Jersey, 2006. pp. 407-409.

[10] T. Bailey and H. Durrant-Whyte. “Simultaneous localization and mapping (SLAM): Part II, state of the art”, IEEE Robotics and Auto- mation Magazine 13(3), 108-117, 2006

IJSER © 2013 http://www.ijser.org