Joan Solà

André Monin
email: monin@laas.fr

Michel Devy
email: michel@laas.fr

Thomas Lemaire
email: tlemaire@laas.fr

Undelayed Initialization in Bearing Only SLAM

Keywords: SLAM, vision, initialization, bearing only, undelayed

Most solutions to the SLAM problem in robotics have utilized Range and Bearing sensors as the provided perception data is easy to incorporate, allowing immediate landmark initialization. This is not the case when using Bearing-Only information because the distance to the perceived landmarks is not directly provided. A whole estimate of a landmark position will only be possible via a set of measurements taken from different points of view. The vast majority of contributions to this problem utilize a parallel task to get this estimate, and hence the landmark initialization is delayed.

We give a new insight to the problem and present a method to avoid this delay by initializing the whole ray that defines the direction of the landmark. We utilize a minimal and computationally efficient form to represent this ray and a new strategy for the subsequent updates. Simulations have been carried out to validate the proposed algorithms.

I. Introduction

The Simultaneous Localisation and Mapping problem (SLAM) is fundamental in mobile robotics. It consists of incrementally building a map of a previously unknown environment from measurements taken from the robot as it moves, and getting localized in it. The original solution [START_REF] Smith | On the representation and estimation of spatial uncertainty[END_REF] utilized an Extended Kalman Filter (EKF) to fuse data acquired by a laser range scanner or other range and bearing sensors, leading to Range and Bearing EKF-SLAM.

Today, many other solutions to Range and Bearing SLAM exist that perform fairly well in real time, in large environments, even in three dimensions. But the sensors they rely on are not convenient: they tend to be delicate, big and expensive. Consider vision instead: a cheap, small and reliable camera is capable of providing a huge amount of spacial information, at the price of losing one dimension of the world we want to observethe distance to the perceived objects. Using such a sensor leads to Bearing-Only SLAM.

Landmark Initialization in Bearing-Only EKF-SLAM is a difficult task. EKF requires Gaussian representations for all the involved random variables that form the map (the robot pose and all landmark's positions). Moreover, their variances need to be small to be able to approximate all the non linear functions with their linearized forms.

From one bearing measurement, we cannot establish an estimate of the landmark position that satisfies this fundamental rule. This estimation is only possible via successive measures from different points of view, when enough baseline has been accumulated (Fig. 1). This reasoning leads to systems that have to wait for this baseline to be available. Ref. [START_REF] Davison | Real-time simultaneous localisation and mapping with a single camera[END_REF] uses a separate Particle Filter to estimate the distance, which is not correlated with the rest of the map. Initialization is deferred until range variance is small enough to consider a Gaussian estimate. In [START_REF] Bailey | Constrained initialisation for bearing-only slam[END_REF] past poses of the robot are stacked in the map, together with associated measures, until baseline is sufficient to permit a Gaussian initialization. Once initialized, the batch of observations is used to refine and correct the whole map. These methods suffer from two drawbacks: they need a criteria to decide whether or not the baseline is enough, and they introduce a delay in the landmark initialization until this criteria is validated.

Avoiding both criteria and delay is an interesting issue. The criteria is often expensive to calculate, and the system becomes more robust without it as no binary decisions have to be taken. Without the delay, having the bearing information of the landmark in the map permits its immediate use as an angular reference. It also allows the use of landmarks that lie close to the direction of travel of the robot, for which baseline would take too long to grow. This is crucial in outdoor navigation where straight trajectories are common and vision sensors will naturally look forward.

To our knowledge, only [START_REF] Kwok | An efficient multiple hypothesis filter for bearing-only slam[END_REF] proposes an undelayed method. It defines a set of hypothesis for the position of the landmark, and includes them all inside the map from the beginning. On successive observations, sequential ratio test (SRT) based on likelihoods is used to prune bad hypothesis, and the one with maximum likelihood is used to correct the map. The way these hypothesis are initialized is not detailed, and convergence and consistency issues are not discussed.

We will show that the theoretically motivated solution to the undelayed initialization implies the abandonment of the EKF (Fig. 2 left) and that, following a multi hypothesis reasoning, the proper way to include all the information in the map is the creation of a set of weighted maps, one for each hypothesis (Fig. 2 center ). But this leads to untreatable algorithms such as the Gaussian Sum Filter (GSF) [START_REF] Alspach | Nonlinear bayesian estimation using gaussian sum approximations[END_REF], for which computational load grows multiplicatively.

The method we present is an approximation of the GSF that permits undelayed initialization with simply an additive growth of the problem size (Fig. 2 right). At the first observation, the robot only knows the optical ray on which the landmark is located. This ray, with associated covariances, define a conic probability distribution function (PDF) for its position. A minimal representation of this PDF is introduced as a geometric series of Gaussians; they are all included in one single EKF-SLAM map. As with all approximations, this representation has the risks of inconsistency and divergence, which we discuss.

To minimise these risks we propose a strategy for all the subsequent updates which we name Federated Information Sharing (FIS). We define a very simple criteria for pruning the less likely members of the ray. Simulation results are provided to demonstrate the pertinence of all these choices.

The rest of this paper is as follows. Section II states the problem. Section III develops all the contributions. Section IV presents the simulation results and section V closes with a discussion.

II. Undelayed Bearing Only SLAM

A. Range and Bearing EKF-SLAM

A random state vector containing robot pose and landmark positions will be our map:

X = X v X M . (1)
where

X v = [r v , q v ]
is the robot state containing position and orientation and X M = [x 1 , . . . , x n ] is the set of landmark positions. In the EKF framework, the a posteriori density is approximated by a Gaussian density with mean and covariances matrix defined by

X = Xv XM P = P vv P vM P M v P M M (2)
The evolution of the robot and the measure of landmark i are defined by the functions

X + v = f (X v , u) y i = h(X v , x i ) + υ ( 3
)
where u is a vector of controls assumed to be Gaussian with mean û and variance U, and υ is a white Gaussian noise with variance R. We get the prediction step1

X+ v = f ( Xv , û) P + vv = F v • P vv • F v + F u • U • F u P + vM = F v • P vM (4)
and the correction step at observation of landmark i

Z i = H i PH i + R K i = PH i • Z -1 i P + = P -K i Z i K i X+ = X + K i • (y i -h( Xv , xi )) (5)
where

2 F v = ∂f ∂X v ( Xv ,û), F u = ∂f ∂u ( Xv ,û) and H i = ∂h ∂X ( X).
Landmark initialisation consists of stacking its position x p into the map as

X + = X x p (6)
and defining the PDF of this new state (the resulting map) conditioned to observation y p . This task is easily performed from the first observation given by y p = h(X v , x p ) + υ as all the components of x p are observed. The classic method [START_REF] Newman | On the Structure and Solution of the Simultaneous Localisation and Map Building Problem[END_REF] performs the variable change

w p = h(X v , x p ) ( 7
)
so measurement is now y p = w p + υ. Then it defines the function g, inverse of h, in order to obtain an explicit expression of

x p x p = g(X v , w p ). ( 8
)
Assuming that P vv and R are small enough we can write

x p ≈ g( Xv , y p ) + G v (X v -Xv ) + G w (w p -y p ) (9)
where

G v = ∂g ∂X v ( Xv ,yp), G y = ∂g ∂w p ( Xv ,yp)
and P vX = P vv P vM . Then x p can be considered approximately Gaussian with mean and covariances matrices defined by xp = g( Xv , y p )

P pX = G v • P vX P pp = G v • P vv • G v + G y • R • G y (10)
and the augmented map is finally specified by

X+ = X xp P + = P P pX P pX P pp . (11)
B. A proper solution for Undelayed Initialization in Bearing-Only SLAM

In the Bearing-Only case the measurement is lacking the range information and the initialization procedure is not that straightforward. We separate range s from bearing b p and write

w p = b p s ( 12
)
so measurement is now y p = b p + υ. This leads to the re-definition of g

x p = g(X v , b p , s) (13)
where all but range s can be safely considered Gaussian.

The a priori values of s cover the interval s ∈ (0, ∞), but knowledge on the current application can reduce it to s ∈ [s min , s max ]. This interval defines a uniform PDF p(s) which is not small enough. The linear approximation of g is not valid and the landmark initialization procedure of EKF-SLAM no longer holds. To solve the problem, we must define a non-Gaussian characterization of p(s), and look for an alternative to EKF to manage it. We propose the Gaussian sum approximation

p(s) ≈ Ng j=1 c j • Γ(s -s j ; σ 2 j ) (14)
where Γ(s -s j ; σ 2 j ) = exp((s -s j ) 2 /2(σ j ) 2 )/ √ 2πσ j . The Gaussian sum approximation may be viewed as a two-step distribution. First, one has to choose j with probability P (j) = c j and, conditionally to j, the range s is Gaussian with mean s j and variance σ 2 j . Hence, we can use this information to initialize an hypothesis for a map with a landmark x j p at a certain range s j . Using (13) and the standard procedure of section II-A we get xj p = g( Xv , y p , s j )

P j pX = G j v P vX P j pp = G j v P vv G j v + G j y RG j y + G j s σ 2 j G j s . (15)
with

G j v = ∂g ∂X v ( Xv ,
p(X + |y p ) = Ng j=1 c j • Γ(X + -Xj ; P j ). (17)
In conclusion, we will have N g maps for every new landmark, and we will have as much as N m g maps in the case we initialise m new landmarks. The map management would have to utilize the standard GSF, but such a multiplicative increase of the problem size makes this solution untreatable.

III. Ray Initialisation using FIS

We need to find a computationally compelling alternative to GSF. Following the same multi-hypothesis reasoning, we can consider that each hypothesis corresponds to a different landmark. We can then initialize them all in one single Gaussian map using the standard EKF-SLAM procedure of section II-A. The result (Fig. 2 right) is a map that has grown in an additive way, avoiding the undesired multiplicative effect.

The problem is that, at the arrival of a second observation, we will not know to which hypothesized landmark it corresponds. As all hypothesis are correlated with the same map, using this observation to correct the map at the wrong hypothesis would probably make the whole map diverge. Alternatively, as EKF requires observation noises to be mutually uncorrelated, using the same measurement to correct at each hypothesis would make the map inconsistent.

The hereafter proposed FIS technique relies on likelihood evaluations of the hypothesis to weight the effect of the different corrections. This allows the minimization of both divergence and inconsistency risks. Aggregated likelihoods are also used to progressively eliminate the wrong hypothesis.

This section first proposes a minimal representation for the Gaussian sum that define the range's PDF. This will minimise the number of hypothesis. Then it goes on detailing our FIS initialization method, which could be seen as a shortcut to the more proper GSF-SLAM.

A. The Ray: a geometric series of Gaussians

We look for a minimal implementation of (14). For that, we start by giving the general realization of (8)

x p = r v + s • R v (q v ) • dir(b p ) ( 18
)
where dir(b p ) is a direction vector in robot frame defined by b p ; R v (q v ) is the rotation matrix associated with the

s 1 s min = α 1 σ s 1 s 1 s 3 = β 2 s 2 = β s 1 = α 2 σ s 2 = α 3 σ s 3
s max 0 s Fig. 3. The conic Ray: a geometric series of gaussian distributions robot orientation; and s is the range, now unknown. 3 We then recall that the observed b p is inversely proportional to s. It is shown in [START_REF] Kronham | Bearing only target motion analysis based on a multihypothesis kalman filter and adaptative ownship motion control[END_REF], [START_REF] Peach | Bearing-only tracking using a set of rangeparametrised extended kalman filters[END_REF] that in such cases EKF is only relevant if the ratio α j = σ j /s j is small enough (up to 30% in practice). This ratio determines the validity of the linearization. This leads to define p(s) as a geometric series with α j = α = constant:

p(s) = Ng j=1 c i • Γ(s -β j-1 s 1 , (β j-1 σ 1 ) 2 ). ( 19
)
An overview of the series with its parameters is shown in Fig. 3. From the bounds [s min ,s max ], and the choice of the ratio α and the geometric base β, we need to determine the first term (s 1 , σ 1 ) and the number of terms N g . We impose the conditions s 1 -σ 1 = s min and s Ng + σ Ng ≥ s max to get

s 1 = (1 -α) -1 • s min σ 1 = α • s 1 N g = 1 + ceil log β 1 -α 1 + α • s max s min (20)
where ceil(x) is the next integer to x.

The geometric base β determines the sparseness of the series. Fig. 4 shows plots of the obtained PDF for different values of α and β. A more detailed discussion on the effect of these parameters can be found in [START_REF] Lemaire | Experiments with a bearing-only slam algorithm[END_REF]. The couple (α, β) = (0.3, 3) defines a series that is somewhat far from the original uniform distribution, but experience showed that the overall performance is not degraded and the number of terms is minimized.

Table I shows the number of gaussians for three typical applications. Note how, thanks to the geometric series, increasing s max /s min by a factor 10 implies the addition of just two members.

B. Map management

The aim of the initialization procedure is twofold: We want to choose the gaussian in the ray that best represents the real landmark, while using at the same time the angular information this ray provides. It consists of three main operations: the inclusion of all the members of the ray into the map; the subsequent updates using Federated Information Sharing; and the successive pruning of bad members. Fig. 5 gives a compact view of the whole process.

1) Iterated Ray Initialization: As discussed earlier, we include all landmark hypothesis that conform the ray in a single Gaussian map. All ray members are stacked in the same random state vector as if they were different landmarks:

X + =      X x 1 p . . . x Ng p      (21)
An iterated method is used to construct its mean and covariances matrix (Fig. 6). Landmark hypothesis are (22) Initially, all hypothesis are given the same credibility so their weighting must be uniform. We will discuss later the evolution of these weights, that will reflect the Aggregated Likelihood (AL) of each hypothesis with the measurements. By now, let us write the uniform AL vector that we have to initialize associated with the newly added ray:

Λ = Λ 1 • • • Λ Ng ; Λ j = 1/N g . (23)
2) Map updates via Federated Information Sharing: This is the most delicate stage. We have a fully correlated map with all hypothesis in it, so a correction step on one hypothesis has an effect over the whole map. If the hypothesis is wrong, this effect will cause the map to diverge.

Of course we would like to use the observation to correct the map at the right hypothesis. As we don't know which one it is, we are obliged to actuate on all of them. This involves the risk of inconsistency: if we incorporate multiple times the same information (remark that we have a unique observation for all hypothesis), the map covariance P will shrink according to the multiple application of the EKF correction equations ( 5), leading to an overconfident estimate of the map X.

The proposed FIS method is inspired by the Federated Filter (FF) in [START_REF] Foxlin | Generalized architecture for simultaneous localization, auto-calibration, and map-building[END_REF] to address these problems. FF is a decentralized Kalman filter that allows a parallelled processing of the information. In the case this information comes from a unique source, as it is our case, FF applies the Principle of Measurement Reproduction [START_REF] Tupysev | A generalized approach to the problem of distributed kalman filtering[END_REF] to overcome inconsistency. This principle can be resumed as follows: The correction of the estimate of a random variable by a set of measurement tuples {y; R j } is equivalent to the unique correction by {y; R} if This is what is done by FIS. The idea (Fig. 7) is to share the information given by the observation tuple {y p ; R} among all hypothesis. Doing R j = R/ρ j , condition (24) is satisfied if Σρ j = 1.

R -1 = ΣR -1 j (24) { , R y p / } ρ 1 { , R y p / } ρ j { , R y p / } ρ N { ,
The divergence risk is also addressed by FIS. We need to choose a particular profile for ρ j that privileges the corrections on more likely hypotheses. A flexible way to do so is by taking ρ j ∝ λ n j , where λ j is the likelihood of hypothesis j given the observation y p

λ j = exp(-0.5 • z j Z -1 j z j ) 2π|Z j | (25)
with z j = y ph( Xv , xj p ), Z j = H j P j H j + R and H j = ∂h ∂X ( Xv ,x j p ). These two conditions over ρ j lead to

ρ j = λ n j /Σ N i=1 λ n i . ( 26
)
The parameter n is a measure of how much we want to privilege strong hypothesis over weak ones, with appropriate values between n = 1 and n = 3.

3) Ray member pruning: The divergence risk also calls for a criteria for pruning those members with very low likelihood. This will in turn allow the ray to collapse to a single Gaussian.

As in the standard GSF, the weight of each hypothesis is successively updated with its measure of likelihood λ j , leading to the notion of Aggregated Likelihood (AL). Evolution of the AL Λ j with likelihood λ j is given by

Λ + j = Λ j • λ j . ( 27
)
AL vector Λ is then normalized so that Σ j Λ j = 1.

For pruning, we use a simple threshold on the AL which is dependent on the actual number N of remaining members. Ray member j is deleted if

Λ j < τ /N ( 28
)
where τ is in the range 0.001 0.1 , typically 0.01. For obvious efficiency reasons, we will apply pruning before updating. When N = 1, we say the ray has collapsed to a single Gaussian and so we are back to standard EKF-SLAM.

IV. Simulation results

Simulations for a two-dimensional implementation have been carried out to validate the proposed methods. The following figures illustrate the results of these Three different scenarios have been simulated. In the first (Fig. 8), a robot makes two turns following a circular trajectory inside a square cloister of some 20m in size, where the columns are treated as landmarks. The linear and the angular speeds are 1m/s and 1rad/s. Odometry errors are simulated by corrupting these values with white Gaussian noises with standard deviations of 0.3m/s and 0.3rad/s respectively. Bearings are acquired every 100ms with a sensor that is looking forward with a field of view of ±45 • and an accuracy of 1 • . A consistency test that plots the 3σ-bound estimated error for the robot position against the true error is given in Fig. 9 The second scenario (Fig. 10) simulates an outdoor area of 140x140m, populated with 60 randomly distributed landmarks. The robot follows a circular trajectory of some 80m of diameter at a linear speed of 2m/s and an angular speed of 0.05rad/s. Odometry errors are 0.3rad/s and 0.3rad/s respectively. Bearings are acquired every 100ms with a sensor with a field of view of ±30 • and an accuracy of 0.5 • . Odometry integration trajectory is plotted in dash-and-dot line for this simulation.

The third simulation (Fig. 11) uses the same previous outdoor scenario, except that the trajectory is now a straight line.

V. Conclusions

In the present work we have proposed an undelayed method to initialize landmarks within the Bearing-Only EKF-SLAM framework. Care has been taken to show that a complete stochastic representation of the state of a map with a newly initialized ray is in the form of a weighted sum of maps. This representation leads to filtering algorithms that are untreatable on-line. As today a SLAM system must be intended to work in real time, such algorithmic solutions fall out of interest. We have proposed a method to solve the problem by generating a multi-hypothesised Gaussian map that includes the whole ray that represents the PDF of the landmark's position. Such a method is suitable to work in real time but, as an approximation of the proper solution, it has some risks. Those risks have been identified and discussed in order to give means to minimise them and make the proposed algorithms sufficiently safe for their use in real robotics tasks.

The relatively high degree of mathematical insight, which is not usually found in many other works on the subject, will allow the prospection of other shortcuts that may eventually beat the performances of the here proposed FIS-SLAM. In this paper we made use of the ideas that are at the base of the Federated Filter, notably the Principle of Measurement Reproduction, with excellent results. A Gaussian approximation that keeps the mean and the covariance of the original non-gaussian map is another option that we have already discovered.

Simulations showed that the undelayed initialization is suitable for robots that use vision sensors with a very narrow field of view and that look in the direction of travel. These are very delicate situations that have been often avoided by previous works, but that turn out to be very interesting for real outdoor applications, not only for robots but also -and very particularly-for intelligent vehicles in road environments.

Real experiments that use vision and the full 3D FIS-SLAM algorithm are being carried out at the date of submission of this paper. Results are expected to confirm the pertinence of this work.

Fig. 1 .

 1 Fig. 1. Landmark initializations. Left: Range and Bearing SLAM. Right: Acquiring baseline b in Bearing-Only SLAM.

Fig. 2 .

 2 Fig. 2. Landmark initializations. Left: EKF-SLAM; Center : GSF-SLAM; Right: FIS-SLAM.

Fig. 4 .

 4 Fig. 4. Geometric distributions for s min /smax = 10: Left: (α, β) = (0.2, 1.8). Center: (α, β) = (0.3, 2). Right: (α, β) = (0.3, 3). Dotted line is at smax.

Fig. 5 .^PFig. 6 .

 56 Fig. 5. Ray updates on 4 consecutive poses. Grey level indicates Aggregated Likelihood that is used to discard bad hypothesis. Dash and dot line is the true distance to the landmark

Fig. 8 .Fig. 9 .

 89 Fig. 8. Indoor simulation.

Fig. 10 .Fig. 11 .

 1011 Fig. 10. Large outdoor area simulation.

  yp,sj ), G j y = ∂g ∂w p ( Xv ,yp,sj ) and G j s = ∂g ∂s ( Xv ,yp,sj ). The hypothetic map j is then

	Xj =	X xj p	P j =	P P j pX	P j pX pp P j	(16)
	and the PDF of the obtained map state is the weighted
	sum

TABLE I

 I Number of gaussians for α = 0.3 and β = 3.

	Scenario	s min (m)	smax (m)	smax s min	Ng
	Indoor	0.5	5	10	3
	Outdoor	1	100	100	5
	Long range	1	1000	1000	7

The notation A + means the updated value of A, for any A.

The vertical slash | stands for evaluated at.

Let bp = (u, v) be the metric coordinates of a pixel in a camera with focal length f . We have dir(bp) = [u/f, v/f, 1] . Landmark depth is denoted by s.