UNIVERSITY OF TRENTO, DEPT.

OF INDUSTRIAL ENGINEERING 1

Cooperative Localization of two UAVs in Indoor

Environment based on Distributed Kalman Filter
Francesco Miglioranzi

I. Problem Statement As a result, each agent will know the distance from

I N this paper, a simulated solution for the cooperative the other agent, the distance from the TAGs in its
localization of two UAVs that can fly inside a room is neighborhood and their position.
presented.
The main idea is that both agents have communication,
processing and measurement capability, as well as a II. Modeling
limited communication and sensing range.
A. Environment
Each agent i receives velocity inputs in order to perform
a specified trajectory. Thanks to the knowledge of these The environment is modeled as a 10x10x10 m room,
inputs, the drone can propagate its equation of motion with landmarks represented as points placed on roof, floor
and walls as shown in fig. 1 and tab. I.
xik+1 = f i (xik , uik , nik ) (1)
where
• f is the system function,
i

• xk is the position vector,

i

• uk is the input vector,

i

• nk is the process noise vector of agent i.

i

The position estimation obtained through the propaga-

tion of the equation of motion diverges from the actual
one due to the process noise. As a result, each UAV carries
exteroceptive sensors, that can use to measure its distance
from known landmarks located inside the environment and
from the other UAV j (if in range).
zik = hi (xik , xjk ) + vki (2)
where
• h is the measurement model,
i

• vk is the measurement noise of agent i.

i

The aim of the project is to present a method to use

those measurements so that the UAVs are capable to
estimate their position into the room with high accuracy.
The proposed solution adopts a distributed Kalman Filter
to perform this kind of cooperative localization.
A. Possible implementation
A possible implementation of this situation could be a
room with passive RFID tags on floor, roof and walls. The
UAVs are equipped with an active RFID reader, so they
can read the information stored in the passive tags when
they are close enough to them. The information stored
in the passive tags are their cartesian coordinates, with
respect to a fixed reference frame located in a corner of
the room.
Moreover, the two UAVs are equipped with ultrasonic
distance sensors that can use to measure their reciprocal
distance and their distances from the passive RFID tags
(provided with ultrasonic receivers).
Fig. 1. Graphical representation of the flying space with highlighted
measurements-communication links. Black lines represent the UAV-
landmarks distance measurements, red line represent the UAV-UAV
distance measurement. Blue circles are the landmarks out of UAV’s
range while red ones are in range. Each UAV is represented with its
covariance ellipsoid, with a green ∗ indicating the ”real” simulated
position and the red ∗ indicating the estimated one.
B. UAVs
UAVs are modeled as massless point objects that can
move in the 3D space of the room, so a realistic dynamic
model is neglected. The choice of a more accurate model
for the UAVs is of second importance with respect to the
goal of the project, yet it could be taken into account for
a deeper analysis in future developments.
UNIVERSITY OF TRENTO, DEPT. OF INDUSTRIAL ENGINEERING 2

TABLE I close enough, their relative distance is calculated with its

Locations of RFIDs tags measurement uncertainty. However, in order to simulate
the interference between the measurement systems, only
ID# x [m] y [m] z[m] ID# x [m] y [m] z[m]
one of the two distance measurements is kept, with
1 0 0 0 14 5 5 10
equal probability to be used/rejected. The drone that
2 0 0 5 15 5 10 0
succeeds in measuring the distance from the other is
3 0 0 10 16 5 10 5
4 0 5 0 17 5 10 10
named temporarily ”master”. Its role will be explained
5 0 5 5 18 10 0 0
later.
6 0 5 10 19 10 0 5 At each instant (according to UAV update frequency),
7 0 10 0 20 10 0 10 the two UAVs can thus acquire the following information
8 0 10 5 21 10 5 0 (if in range):
9 0 10 10 22 10 5 5 • set of UAV-landmarks distance measurements,
10 5 0 0 23 10 5 10 • locations of the landmarks in range,
11 5 0 5 24 10 10 0 • distance from the other UAV.
12 5 0 10 25 10 10 5
Assume that UAV ”i” is close enough to UAV j
13 5 5 0 26 10 10 10
and to N landmarks, then it will collect the following
measurements:  
di1 
The two agents are assumed to receive as inputs the 
 
 di2 
 
components of the velocity in the three cartesian direc-
 
i
zk = ... . (6)
tions:
d
  
iN
ẋ = ux

 

  
dij
 
ẏ = uy . (3)
The measurement model that correlates the distance

ż = uz

measurements zik to the position of the UAV xik , referring
The state transition model for the two UAVs is then
to eq. 2, is given by
described by
 p i
(xk − x1 )2 + (yki − y1 )2 + (zki − z1 )2 
 
xk+1 = xk + tk uxk
 
 
(4)

 p 

yk+1 = yk + tk uyk . 
(x i − x ) 2 + (y i − y )2 + (z i − z )2 
2 2 2
 

 k k k 

...
  
zk+1 = zk + tk uzk (7)

.
This state transition model does not estimate perfectly  p(xi − x )2 + (y i − y )2 + (z i − z )2 
 
 k N k N k N 
the ”real” (simulated) trajectory of the UAVs: the input

 

 q
 

j 2 j 2 j 2 
velocities could be different from the actual ones (due to
 i i i

(xk − xk ) + (yk − yk ) + (zk − zk )

errors of the actuators, environmental effects, electronic
malfunctioning,...). Moreover, the accuracy of the model This model is non linear in xik and is dependent from the
is affected by the discrete integration time step tk . Those position of the other agent, in case of relative measure.
errors are modeled as process noise nk , assumed to be zero
mean multivariate Gaussian noises with covariance Qk . III. Localization Algorithm
xk+1 = xk + tk uk + nk . (5) The localization algorithm that will be presented in the
following dissertation is based on the EKF-CL (Extended
C. Sensors Kalman Filter - Cooperative Localization) algorithm de-
UAVs and landmarks are modeled as points in the flying picted in [1].
space, so the landmark-UAV distance measure can be As long as there is no relative measurement among team
simulated as point-to-point distance calculation. When members, the model described in eq. 7 is independent from
this distance is included in the range of measurement of the position of the other agent, in this way each agent can
the UAV, then it can be used for the localization. However, propagate its portion of the cross-covariance term locally
a measurement uncertainty must be introduced. A good without a need of communication, and can use a standard
approximation to simulate the fact that the measurement EKF as localization algorithm, taking advantage from the
performances decrease with the measured distance d, is to landmarks measurements only.
model a measurement uncertainty of 0.1d. A zero mean However, if a relative measurement occurs, the EKF-CL
white Gaussian noise is then included, such that 99.7 % must be used: the agents need to communicate, and the
of the times, the measurement error is smaller than 1/10 one that manages to take the relative measure is elected
of the measured distance. The covariance matrix of the master, it puts together the split cross-covariance terms
measurement noise is denoted with R. and proceeds with the update stage of the distributed
The relative distance measurements between the two Kalman filter. This particular case will be analyzed in the
drones are obtained in the same way, so when the UAVs get following.
UNIVERSITY OF TRENTO, DEPT. OF INDUSTRIAL ENGINEERING 3

A. Initialization
x̂i− (k + 1) = Ax̂i+ (k) + Bui (k) (9)
Consider the two UAVs belonging to the set U = {a, b}.
At iteration k=0, the algorithm is initialized as Pi− (k + 1) = APi+ (k)AT + Q(k) (10)

x̂i+ (0) = 03x1 , Pi+ (0) = M3 , P+ P−

ij (k + 1) = AP+ T
ij (k)A . (11)
ij (0) = 03x3 , j ∈ U \ {i}

where x̂i+ is the state estimate, Pi+ is the state covariance
estimate and P+ ij is the cross covariance matrix between
agents i and j.
B. Prediction step
At iteration k, equation 5 can be written as
xi (k + 1) = Axi (k) + Bui (k) + ni (k)
where
• A = I3 is the state transition matrix,
• B = t(k)I3 is the input to state matrix.
The process noise ni (k) has covariance Q(k), this matrix
is estimated accordingly to the errors that are assumed to
be caused by he model.
The propagation stage of the Kalman Filter can then
be written as
C. Update step
Suppose that drone a performs distance measures from
the other drone b. So a becomes the temporary master
with b becoming the temporary slave. The measurement
vector for this stage of the algorithm becomes
(12)

zab (k + 1) = dab .
(8) Slave sends its predicted position, covariance and cross
covariance estimates to the master. Given those informa-
tion, the role of the master is to ensemble the vectors and
matrices necessary for the calculations, and to send them
to the slave so that it can perform the update step as well.
• rab is the residual of the relative measurement;
• Sab is the covariance of the residual;
• Ha and Hb are the state observation matrices;
• Ki is the Kalman gain.

hab (x̂a− (k + 1), x̂b− (k + 1)) = (13)

p
(xa− (k + 1) − xb− (k + 1))2 + (y a− (k + 1) − y b− (k + 1))2 + (z a− (k + 1) − z b− (k + 1))2

∂ x̂i+ (k + 1) = x̂i− (k + 1) + Ki (k + 1)ra (19)

Ha (k + 1) = hab (x̂a− (k + 1), x̂b− (k + 1)) (14)
∂xa Pi+ (k + 1) = Pi− (k + 1) − Ki (k + 1)Sab Ki (k + 1)T
∂
Hb (k + 1) = hab (x̂a− (k + 1), x̂b− (k + 1)) (15) P+ −
ij (k + 1) = Pij (k + 1) − Ki (k + 1)Sab Kj (k + 1)
T
∂xb
If at the same time landmark (absolute) measurements
are available, another update is performed independently
rab = zab (k + 1) − hab (x̂ a−
(k + 1), x̂ b−
(k + 1)) (16)
by the intrested UAV(s) using a standard version of EKF.

Sab = R(k + 1) + Ha (k + 1)Pa− (k + 1)Ha (k + 1)T + IV. Simulation Results

+ Hb (k + 1)Pb− (k + 1)Hb (k + 1)T + In order to show the potential of this localization
algorithm, a particular case is presented.
+ Hb (k + 1)P− T
ba (k + 1)Ha (k + 1) +
The simulation is set in a way that agent 1 receives input
+ Ha (k + 1)P−
ab (k + 1)Hb (k + 1)
T
velocities so as to perform a parabolic trajectory across the
room. However, in order to simulate a malfunctioning of
(17) the actuation system, the actual velocities that are used to
compute its trajectory are 1.5 times the nominal ones on
Ki (k + 1) = (P− T
ib (k + 1)Hb (k + 1) + the x-y directions, and are also affected by white Gaussian
+ P− T −1
ia (k + 1)Ha (k + 1) )Sab (18) noise (fig. 2).
Meanwhile, agent 2 receives input velocities in order to
perform a spiral trajectory, from the floor to the top of
Finally, it sends the results of the calculations to the the room. Actual velocities of agent 2 are only affected by
slave, so that both agents can perform their own state white Gaussian noise (fig. 3).
update step. UAVs are set so that their update frequency and sensing
range are respectively 20 Hz and 5 m. The simulation lasts
for 200 s.
UNIVERSITY OF TRENTO, DEPT. OF INDUSTRIAL ENGINEERING
Fig. 2. This plot shows the difference of the component of velocity if
UAV 1 from the input velocities. To simulate a malfunctioning, real
x and y velocities are set 1.5 times the input velocities.
Fig. 3. This plot shows the difference of the component of velocity
of UAV 2 from the input velocities. Real velocities are only affected
by a noise.
Starting from this scenario, different situations are
analyzed:
A. After a few seconds, agent 1 has a malfunctioning; it
can neither perform distance measures of any kind or
communicate, while agent 2 can rely on landmarks
measurements only;
B. Agent 1 and agent 2 measurement systems are work-
ing properly, they can perform distance measurements
with respect to each other and landmarks;
C. agent 1 cannot perform distance measures, but it can
be helped by agent 2 when they are close enough;
D. both UAVs can perform landmark distance measures
only. Relative measurements are not available.
To evaluate the performances of the algorithm in the three
4
situations, the following index is used for both UAVs:
pi =
q
N
X (x̂i+ i 2 1+ 1 2 1+ 1 2
k − xk ) + (ŷk − yk ) + (ẑk − zk ) (20)
N
k=1
Index pi is the root mean square error relative to the
deviation of the estimated position of UAV i from its real
position at each iteration. The lower pi , the more accurate
the localization of agent i.
A.
After a few seconds, agent 1 has a malfunctioning,
it cannot perform distance measures of any kind and
cannot communicate. Agent 2 can rely on landmarks
measurements only.
In this way, agent 1 propagates its position according
to its model, on the basis of wrong information on
the velocities. Its covariance increases over time and its
position estimates is affected by an error (see fig. 4). On
the other side, agent 2 can localize itself thanks to the
landmark measurements and the Kalman Filter ((see fig.
5)).
As a consequence, the two root mean square errors used
as performance indexes take the following values:
p1 = 3.433m
p2 = 0.064m.
We see that p1 is much higher than p2, which means that
UAV 1 is performing a bad localization.
Fig. 4. Estimated VS real trajectory for agent 1 in situation ”A”:
the position estimation is wrong due to the malfunctioning and the
inability to communicate with agent 2.
UNIVERSITY OF TRENTO, DEPT. OF INDUSTRIAL ENGINEERING
Fig. 5. Estimated VS real trajectory for agent 2 in situation ”A”:
after an initial error (due to the fact that the UAV does not know
its initial position) agent 2 can localize itself with a small error.
B.
After a few seconds, agent 1 has a malfunctioning, it
cannot perform distance measures but it can communicate
with agent 2, which can rely both on relative and land-
marks measurements. In this way, when the two agents
are close enough, drone 2 can ”help” drone 1 with its own
measurements.
This results in a strongly improved localization for drone
1 with respect to situation ”A”, as can be seen from fig. 6.
In particular, it is possible to notice that the estimated
position is periodically brought near the real one. This is
due to the fact that UAV 2 gets close to UAV 1 periodically
becouse of its helicoidal motion.
Fig. 6. Estimated VS real trajectory for agent 1 in situation ”B”:
the position estimation is better than the previous case, thanks to
the ”help” from agent 2.
5
Fig. 7. Estimated VS real trajectory for agent 2 in situation ”B”:
localization performances are similar to the previous case.
Moreover, the root mean square error relative to UAV
1 decreases drastically:
p1 = 0.936m
p2 = 0.062m.
C.
Agent 1 and agent 2 measurement systems are active,
they can perform distance measurements with respect to
each other and to landmarks. In these conditions, the
localization algorithm has the highest performances.
UAV 1 performs the best localization among the three
different situations, while UAV 2 obtains results similar to
the previous ones. This is supported by the values assumed
by the indexes p1 and p2 :
p1 = 0.336m
p2 = 0.062m.
Although UAV 1 has a wrong knowledge of its velocities,
the localization algorithm can stabilize its root mean
square error around 0.3m, ten times lower than situation
A.
From fig. 8 and 9 is possible to qualitatively evaluate
the effectiveness of the localization by checking the fitness
of the estimated trajectory with the actual one for both
UAVs.
D.
In this scenario, both UAVs can perform landmark
distance measures only. The algorithm then becomes a
standard Kalman Filter, executed independently by the
agents. With respect to situation C, only a small loss
in performances is noticeable. The highest effect can be
qualitatively seen for UAV 1 in fig. 10, when it travels in
the central part of the room, where it is too far from the
landmarks. This is confirmed by the values of the indexes:
UNIVERSITY OF TRENTO, DEPT. OF INDUSTRIAL ENGINEERING
Fig. 8. Estimated VS real trajectory for agent 1 in situation ”C”:
the position estimation is better than the previous cases ”A” and
”B”. In this scenario, in fact, drone 1 is capable to perform both
landmark and relative measures.
Fig. 9. Estimated VS real trajectory for agent 2 in situation ”C”:
localization performances are similar to the previous cases.
p1 = 0.339m
p2 = 0.062m.
This means that the distributed localization algorithm has
its best application when one of the two agents is not able
to acquire landmarks measurement.
V. Conclusions
From the simulation results presented in sec. IV, it is
possible to formulate the following conclusions:
• If one of the two agents is not able to localize itself due
to the inability to perform distance measurements of
any kind, the cooperative localization algorithm al-
lows a very good correction of its position estimation
(see section IV-B).
6
Fig. 10. Estimated VS real trajectory for agent 2 in situation ”D”:
there is a loss of accuracy while traveling in the central part of the
room, where it is too far from the landmarks.
Fig. 11. Estimated VS real trajectory for agent 2 in situation ”D”:
localization performances are similar to the other cases.
• If agents have a wrong knowledge of the inputs,
which results in an erratic propagation of the position
estimation, should the affected agent has access to
at least landmarks measurement, the localization
algorithm can compensate this effect (see section IV-C
and IV-D).
• The usefulness of the relative distance measurements
and the distributed Kalman Filter arises when one of
the two agents has not access to landmarks measure-
ments (see section IV-B and IV-D).
A. Future developments
This simulation is focused on the investigation about
the performances of the cooperative localization algorithm
proposed by [1].
However, to obtain a more realistic simulation, the
following aspects should be implemented:
UNIVERSITY OF TRENTO, DEPT. OF INDUSTRIAL ENGINEERING 7

• detailed dynamic model of the UAVs,

• realistic communication system,
• application of the algorithm to a swarm of UAVs
(scalability).

Appendix A
Matlab Implementation
The full simulation code can be found at the following
URL.
https://drive.google.com/drive/folders/
11tDjJEaCuZCsXcBKZz5XxSpf1_usRxJB?usp=sharing
Figure 12 represents a flow chart where the most impor-
tant functions have been highlighted, so that it can be
used as a reference while reading the code.

References
[1] S. Kia Solmaz , Rounds Stephen , Martinez Sonia, ”Coopera-
tive Localization for Mobile Agents: A Recursive Decentralized
Algorithm Based on Kalman-Filter Decoupling”, 2015.
[2] Fontanelli Daniele, ”Distributed System for Measurement and
Automation class notes”, 2016-2017.

Fig. 12. Flow chart of the Matlab code.