Robotic Perception and Action:

Vehicle SLAM Assignment

Mariolino De Cecco

Mariolino De Cecco, Mattia Tavernini

1

CONTENTS Vehicle SLAM Assignment

Contents

Assignment Scenario 3

Odometry Localization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

Matrix CXk. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

Matrix JΦwk. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

Matrix Cwk. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

Matrix Swk. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

Matrix Ik . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

Data Provided . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

Aruco Localization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

UWB Positioning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

Problem 1 8

(Odometry Incremental Localization) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

Problem 2 8

(Odometry and Aruco Incremental Localization) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

Problem 3 9

(Odometry and Aruco SLAM) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

Problem 4 9

(Odometry, Aruco and UWB SLAM) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

Results 10

Page 2 of 10

Vehicle SLAM Assignment

Assignment Scenario

Consider a Differential Drive Vehicle ables to move in an unknown environment with the following sensors:

• Rotational encoders on both the wheel drive (Odometry Localization);

• RGB Camera capable to detect a certain number of artificial landmarks placed in the environment

in unknown positions and orientations. An onboard computer is capable to collect the images and to

extract the bi-dimensional pose of the landmarks identified (Aruco Localization);

• Ultra Wide Band Positioning, and indoor positioning system based on the radio technology with a set

of receivers placed in the environment and a reference tag on the vehicle (UWB Positioning).

Figure 1: Differential drive kinematic model and development robot

The aim of the work is to perform the localization of the vehicle and when it is possible, to map the artificial

landmark placed in the environment. In the next paragraphs there are more details on the localization

system and the data provided.

Odometry Localization

Odometry (Incremental) Localization can be done with the knowledge of the vehicle kinematic model, the

vehicle kinematic parameters and the encoder readings. Since the vehicle is controlled by the two driving

wheels, the rotational encoders are mounted on the wheel. It can be assumed that both the encoders have

the same characteristics. The measurements acquired are the incremental number of counts on wheels. The

vehicle is localized by using the discrete form of odometric navigation reported in [1] and [2]:xk+1 = xk + π · nRkRR+nLkRL

n0· cos(δk)

yk+1 = yk + π · nRkRR+nLkRL

n0· sin(δk)

δk+1 = δk + 2π · nRkRR−nLkRL

b·n0

(1)

where:

• xk+1 is the update vehicle x position starting from the previous knowledge xk,

• yk+1 is the update vehicle y position starting from the previous knowledge yk,

• δk+1 is the update vehicle δ attitude starting from the previous knowledge δk,

• nRk is the incremental counter of the encoder on the right wheel,

• nLk is the incremental counter of the encoder on the left wheel,

• n0 is the counter per revolution of both the encoder (assuming that they have same characteristics),

Assignment Scenario continued on next page. . . Page 3 of 10

Odometry Localization Vehicle SLAM Assignment Assignment Scenario (continued)

• RR is the radius of the right wheel,

• RL is the radius of the left wheel,

• b is the vehicle wheelbase.

In the same article is reported the method to use in order to estimate the odometric localization uncer-

tainty, starting from the uncertainty of the wheel radii, the wheelbase and the initial vehicle attitude. This

formulation already include the uncertainty propagation, and id performed in this way:

CXk+1= CXk

+ JΦwk· Cwk

· JTΦwk+ JΦwk

· Swk· ITk + Ik · Swk

· JTΦwk(2)

in which:

Ik = Ik−1 + JΦk−1 · Swk−1 (3)

In the next paragraphs are described these matrices.

Matrix CXk

The matrix CXkrepresents the covariance matrix of the vehicle pose Xk at the discrete time step k. With

the previous formulation it is possible to estimate the covariance of the vehicle pose at each step, taking

already into account the propagation uncertainty (and so the drift).

Matrix JΦwk

The matrix JΦwkis the Jacobian of the non linear function Φwk

that permits to perform the pose update:

Xk+1 = Xk + Φwk

wk is the vector of the variables that affect the accuracy of the vehicle pose, in this case it is:

wk = [RR, RL, b, δk]

This means that the parameters that affect the accuracy on vehicle pose estimation are the uncertainty on

wheel radii, the wheelbase and the attitude at the previous step.

The Jacobian JΦwkhas to be computed on the discrete vehicle pose update of equation (1).

JΦwk=

∂(xk+1−xk)

∂RR

∂(xk+1−xk)∂RL

∂(xk+1−xk)∂b

∂(xk+1−xk)∂δk

∂(yk+1−yk)∂RR

∂(yk+1−yk)∂RL

∂(yk+1−yk)∂b

∂(yk+1−yk)∂δk

∂(δk+1−δk)∂RR

∂(δk+1−δk)∂RL

∂(δk+1−δk)∂b

∂(δk+1−δk)∂δk

The algebraic formulation of the derivatives in the Jacobian are always the same, the Jacobian must be

evaluated each time with the current values.

Matrix Cwk

This matrix take care of the uncertainties of the variables on vector wk. Assuming that all this variables are

independents, it can be simplified as:

Cwk=

σ2RR

0 0 0

0 σ2RL

0 0

0 0 σ2b 0

0 0 0 σ2δk

The element σ2

RR, σ2

RLand σ2

b are always the same and are given in the parameter section. On the contrary,

the value of σ2δk

must be update at every iteration with the previous value that can be found in CXk.

Assignment Scenario continued on next page. . . Page 4 of 10

Aruco Localization Vehicle SLAM Assignment Assignment Scenario (continued)

Matrix Swk

The elements of the matrix Swkare simply the square roots of Cwk

elements.

Matrix Ik

This matrix represents the integral terms of the encoder uncertainty propagation, it can be computed by

using the equation (3). It can be initialized as < 3 × 4 > zeros matrix at the first step.

Data Provided

The data provided for this localization is the encoderReading.txt file, that is organized as follow: The other

pose number Right encoder counter tick Left encoder counter tick

1 0 0

2 10 9

.. .. ..

n ∆tickR ∆tickL

Table 1: encoderReading.txt file organization

parameters for the vehicle localization are:

Parameters Description Value

RR Radius of the right wheel [meters] 0.1

RL Radius of the left wheel [meters] 0.1

b Vehicle wheelbase [meters] 0.4

n0 Count per revolution (both encoders) 8000

σRRStandard deviation on Right wheel radius [meters] 1e-4

σRLStandard deviation on Right wheel radius [meters] 1e-4

σb Standard deviation on wheelbase [meters] 1e-4

σδk Initial Standard deviation on vehicle attitude [rads] 1e-4

Table 2: Vehicle kinematic model parameters

Aruco Localization

The Aruco Localization is based on a calibrated camera and a set of known markers on the ceiling as depicted

on (Figure 2). Once the transformation between the camera sensor and the vehicle is known it is possible

to estimate the pose of the Aruco marker in the vehicle reference system.

The Aruco Localization is achieved by means of a computer vision algorithm that is capable to detect the

Aruco marker and to estimate their 3D pose (Figure 3).

Since the camera has a planar motion (it is mounted on the vehicle identified by its pose X = (x, y, δ))

and the markers are assumed to be parallel to the camera motion, the 3D pose can be translated into a

2D pose. The data provided for this localization are, for each vehicle pose and for each Aruco marker, the

marker unique-id, its estimated bi-dimensional pose in the vehicle reference system and its uncertainty. This

information are defined as follow:

• i is the vehicle pose index i = 1..n,

Assignment Scenario continued on next page. . . Page 5 of 10

Aruco Localization Vehicle SLAM Assignment Assignment Scenario (continued)

(a) Camera (b) Aruco Marker on ceiling

Figure 2: Camera mounted on vehicle (a) used to detect the Aruco marker on ceiling (b)

Figure 3: Software to detect the Aruco marker and estimated their 3D pose

• j is the landmark index j = 1..m,

• Zij = (xj , yj , δj)i

is the j − th Aruco marker pose measurement from the i− th vehicle pose,

• CZij

=

σ2xx σ2

xy σ2xδ

σ2yx σ2

yy σ2yδ

σ2δx σ2

δy σ2δδ

i

j

is the covariance matrix associated to the measurement Zij .

All this information are collected in the text file ArucoReading.txt.

pose id landmark id xij yij δij σxx σxy σxδ σyy σyδ σδδ

1 12 1.3 2.1 0.1 0.01 0.02 .. .. .. ..

1 3 5 3 -0.5 0.003 0.031 .. .. .. ..

i j .. .. .. .. .. .. .. .. ..

n j .. .. .. .. .. .. .. .. ..

Table 3: ArucoReading.txt file organization

Assignment Scenario continued on next page. . . Page 6 of 10

UWB Positioning Vehicle SLAM Assignment Assignment Scenario (continued)

UWB Positioning

The Ultra Wide Band (UWB) Positioning is a method that in the last years has been improved. Its

configuration is very similar to the GPS Positioning, with the only difference that in this case is the tag that

emits a UWB signal to the antennas positioned in the environment (Figure 4).

Figure 4: Similarity between GPS positioning and UWB positioning

The antennas can only return the distance of the tag (placed on the vehicle). The vehicle position has to

be evaluated by using a triangulation algorithm as depicted in Figure 5. It must be noticed that the tag

is described only by its position: with triangulation it is not possible to evaluate the vehicle attitude. For

this reason the method is called Positioning (that means find the position (xi, yi) of the vehicle)and not

Localization (that means find the pose (xi, yi, δi) of the vehicle).

Figure 5: Position estimation by triangulation

The information achievable from this measurement system are:

• i is the vehicle pose index i = 1..p,

• k is the antenna index k = 1..m,

• rik is the j − th antenna distance measurement from the i− th vehicle pose,

• σrik is the standard deviation associated to the distance measurement rik.

All this information are collected in the text file UwbReading.txt.

Only for this positioning system are given a guess solution for the antenna positions in the environment,

collected in the text file UwbAntennaGuess.txt.

Page 7 of 10

Vehicle SLAM Assignment Assignment Scenario

pose id antenna id rik σrik

1 2 1.4 0.2

1 5 5.2 0.15

i k .. ..

n k .. ..

Table 4: UwbReading.txt file organization

antenna id x position y position

1 1.5 3

.. .. ..

k .. ..

.. .. ..

p .. ..

Table 5: UwbAntennaGuess.txt file organization

Problem 1

(Odometry Incremental Localization)

In this first problem is request to perform the vehicle localization using the raw encoder data in the en-

coderReading.txt file. The incremental pose has to be estimated using the Equation 1 while the uncertainty

can be estimated with the Equation 2. It is important to notice that the formulation of Equation 2 already

include the uncertainty propagation.

Problem 2

(Odometry and Aruco Incremental Localization)

In this second problem, it is introduced the Aruco measurement. In particular is required to perform an

Incremental Localization using the Aruco measurement (like the class exercise without encoder data).

All the necessary information for this step can be found in the ArucoReading.txt file.

N.B.: As in real measurement the measurement ratio of the Aruco markers is lower than the encoder.

This means that while using encoder there is a measurement for each vehicle pose, using the Aruco only

in some vehicle poses i there is a measurement.

Facultative: As facultative improvement try to fuse the information of the Odometry Incremental Local-

ization with the Aruco Incremental Localization. As hint, both the localization must be perform at same

time, and the uncertainty propagation of encoder Equation 2 has to start from the previous fused (with

Aruco) CXkwhen there is an Aruco measurement.

Page 8 of 10

Vehicle SLAM Assignment Problem 2

Problem 3

(Odometry and Aruco SLAM)

Use the information of odometry and Aruco markers to perform a SLAM (Simultaneous Localization and

Mapping) with the GTSAM library. The data to use in this case are:

• file encoderReading.txt

• file ArucoReading.txt

As initial solution for the vehicle poses graph you can use the odometry incremental localization. Regarding

the landmark initial pose for the graph, select the most appropriate way and justify the choice.

Problem 4

(Odometry, Aruco and UWB SLAM)

By using all the information given, develop a SLAM with the GTSAM library using in addition, with respect

to the Problem 3, the information of the UWB sensors. The data to use in this case are:

• file encoderReading.txt

• file ArucoReading.txt

• file UwbReading.txt

For the initial solution of the UWB Antenna positions use the guess in the file UwbAntennaGuess.txt.

Provide the map of the Aruco markers and UWB antennas.

N.B.1:: UWB antennas are identified with a position and not a pose

N.B.1:: UWB antennas and Aruco markers are different landmarks, placed in different place in the

environment.

Page 9 of 10

Vehicle SLAM Assignment Problem 4

Results

They are provided two datasets (a short and a long path).

For each problem (and dataset), provide (when it is possible) the following data:

• vehiclePose.mat

Structure with the reconstructed vehicle pose and covariance:

vehiclePose(..).pose = [x, y, δ]

vehiclePose(..).covari =< 3x3 > matrix

• ArucoMap.mat

Structure with the solved Aruco map:

ArucoMap(..).pose = [x, y, δ]

ArucoMap(..).covari =< 3x3 > matrix

• UwbMap.mat

Structure with the solved UWB map:

UwbMap(..).position = [x, y]

UwbMap(..).covariance =< 2x2 > matrix

References

[1] Mariolino De Cecco, Luca Baglivo, and Francesco Angrilli. Real-Time Uncertainty Estimation of Au-

tonomous Guided Vehicle Trajectory Taking Into Account Correlated and Uncorrelated Effects. IEEE

T. Instrumentation and Measurement, 56(3):696–703, 2007.

[2] Mariolino De Cecco. Sensor fusion of inertial-odometric navigation as a function of the actual manoeuvres

of autonomous guided vehicles. Measurement Science and Technology, 14(5):643, 2003.

Page 10 of 10