A Python Implementation of Chans TDoA algorithm

for Ultrasonic Positioning and Tracking

Stock, V2_Lab Rotterdam

July 24, 2008

Contents
1 Python 2
1.1 Objects . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Syntax . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.3 The SciPy package . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.3.1 SciPy Arrays & Matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2 The Tracking-System Hardware 6

2.1 The Hexamite System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.1.1 The System Components . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.1.2 Buffering & Polling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.1.3 Bits, Bytes and Baudrates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.1.4 Clock Synchronisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.2 The Tracking Server . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3 The High-Speed RS485 Serial-Port Card . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3.1 The setserial Utility . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3 Collecting ToAs and Making TDoAs 9

3.1 Gathering ToAs into Events . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3.1.1 The TDoA-Window . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.2 Poll-Cycles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.2.1 Calculating the Position with Minimum Latency . . . . . . . . . . . . . . . . . . . 10
3.2.2 Calculating the Position with Maximum Accuracy . . . . . . . . . . . . . . . . . . 12
3.2.3 Sync Correction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.3 Grouping ToAs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
3.3.1 Catching Reflections . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.3.2 The ToA-Grouping Algorithm in Python . . . . . . . . . . . . . . . . . . . . . . . 19
3.4 Calculating TDoAs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

1
4 Calculating the Position 22
4.1 Chans Algorithm with 4 Monitors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4.1.1 Dissecting Chans Formula . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
4.1.2 Finding R0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
4.2 Chans Alternative Algorithm with 5 or More Monitors . . . . . . . . . . . . . . . . . . . 27
4.3 Chans Alternative Algorithm for Monitors in a Plane . . . . . . . . . . . . . . . . . . . . 27
4.3.1 Moving the Plane; Coordinate Transformations . . . . . . . . . . . . . . . . . . . . 27
4.4 Choosing the right Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
4.5 Choosing the right Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.6 Calculating the Time . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.6.1 From TOA to System Time . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.6.2 System Latency . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
4.6.3 Synchronizing Clocks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

5 Simulating Tags 31

6 Multiple Threads 31

7 OpenSoundControl Communication 31

1 Python
In this document, bits of Python-code will be presented, inside framed boxes. Python is an interpreted
yet very powerful, flexible and extensible programming-language. See http://www.python.org/
For readers who are unfamiliar with Python and its somewhat unusual syntax, it might be useful to list
a few of its features, so that the code-fragments presented throughout this document can be more readily
understood.

1.1 Objects

Python is an Object-Oriented language. Severely so; almost everything in Python is an object. Even
a simple number like 4 is in fact an instance of the built-in class integer with the value 4. Objects
can have methods (i.e. functions) and member-variables, which can in turn be objects, etc. Python has
no concept of pointers as in C/C++/Java, but since almost everything is an object, assignments are
generally done by passing a referecnce to the object (i.e. a pointer). Which is what you want in most
cases anyway. If you explicitly do not want a reference to, but rather a copy of the object, simply use
the objects .copy() method.
Here is a list of the most common object-types, explaining their use & behaviour:

Numbers: Python has three standard numerical types; integer, floating-point and complex num-
bers. These all come in the form of objects with a certain value. The value of an integer (-object)
is not limited by the size of the CPU-registers.
Lists: Like most programming-languages Python knows arrays, except now they are called lists.
Lists can have elements of mixed type, and are always 1-dimensional. It is possible to have a list
of lists (of lists, etc.) and build a higher-dimensional array that way, but there are much more
convenient array-like-objects provided by the SciPy package (See below).

2
 Lists can be created explicitly by listing the comma-separated elements between square brack-
ets; l = [3, 7, 5, two, [3.14, 2.71]] (note how the last element of l is a list of two
floats), or lists can be returned by a function (like the built-in function list()). Lists may also
be created empty; e = [], or with one element; f = [foo], and then grown by repeated
appendage or insertion of items.
Elements of a list are indexed using square brackets and indexes start at 0; l[0] is the first
element of list l. Attempts to index an item beyond the end of the list will raise an IndexError.
An especially nice feature is that elements can also be indexed form the end of the list, by
using negative indexes; l[-1] is the last item, l[-2] the before-last, etc. Indexes into deeper
levels of lists are simply tacked on the end; l[-1][1] returns the 2nd item of the list that is
the last item of l.
Indicated elements of a list are assignable, as long as the indicated item exists; l[3] = twee,
and new elements can be appended; l.append(77.6), or inserted; l[3:3] = [een] (if that
last bit looks too cryptic, read about slice notation, below). Our list l should now look like
[3, 7, 5, een, twee, [3.14, 2.71], 77.6]
Lists can be sliced; a subsection of the list can be indexed, returning a new list containing the
elements of the indicated subsection. A slice is written as two integers (start & end index)
separated by a colon. The resulting list will not include the item indicated by the end-index;
l[3:6] will return a new list containing only items 3, 4, 5, but not 6, of list l. The start and
end indexes of a slice can be omitted, which is taken to mean from start or to end; l[1:]
returns all of l except the first item (l[0]), and l[:] returns a copy of l. And slicing works
with negative indexes too; l[:-1] returns all of l except the last item
Finally, lists can be glued together with the + operator, and even repeated any number of
times using the * operator; [1, 2] + [3, 4] gives [1, 2, 3, 4], and [1, 2] * 3 returns
[1, 2, 1, 2, 1, 2]

Tuples: Python tuples are like lists; 1-dimensional arrays, supporting elements of mixed type,
except that they are immutable; they cant be changed after creation.

Tuples are created explicitly by listing the comma-separated elements between round brackets;
t = (1, 2, 3.14), or with the built-in tuple() function.
Elements of a tuple are also indexed using square brackets and indexes start at 0. Attempts
to index an item beyond the end of the tuple will raise an IndexError. Negative indexing,
slicing and hierarchies (lists / tuples can be elements of a tuple) are supported.
Indicated elements of a tuple are not assignable. Tuples are immutable.
And, like lists, tuples can be glued together with the + operator, and repeated any number of
times using the * operator; (1, 2) + (3, 4) gives (1, 2, 3, 4), and (1, 2) * 3 returns
(1, 2, 1, 2, 1, 2)
Tuples of one element can exists, in which case the single element is followed by a comma, but
no other elements; (3,)

Strings: These are immutable sequences of characters, and come in two flavours; standard (i.e.
ASCII) stringa and Unicode strings.

Strings are represented as a sequene of characters enclosed in single (), double () or triple
( / ) quotes; foo and foo are equivalent. Triple-quoted strings are taken literally, and
will include all characters between the opening & closing sets of quotes, including newlines,
tabs etc.
Unicode strings are represented exatly like regular strings, but preceded by the letter u (i.e.
in front of the opening quote(s)); ubar
Like tuples, inividual characters in a string can be indexed using square brackets. Negative
indexing is supported.
Strings can be sliced, concatenated with +, multiplied. Since strings are immutable, any
operation on strings will return a new string.

3
 Single- & duble-quoted strings support printf-like formatting operations. see String Format-
ting Operations http://docs.python.org/lib/typesseq-strings.html for more informa-
tion

Dictionaries: A.k.a dicts are indexed arrays of any type, where the indexes are called keys and
can be of any immutable type. Key-Value pairs are separated by a colon : and multiple pairs are
separated by commas. Obviously, slicing and negative indexing are not supported.

Dicts are created using curly brackets; d = {one:een, two:twee, three:3}, or

created as an empty dict ({}), with entries subsequently created; d[four] = vier. If an
entry with the specified key already exists, it is overwritten; d[three] = drie.
Entries in a dict are also indexed using square brackets; print d[three] should print drie.
A KeyError is raised if the exact key specified as index does not exist in the dict.
Entries from one dict can be added to another, possibly overwriting entries with the same key,
using the .update() method; d.update({six:zes, five:vijf})
Dicts cannot be iterated over. They do have three useful methods for generating lists from
dicts, but be warned that the order of the items in the lists is totally unpredictable, but will
be the same for the different methods of one and the same dict:
d.keys() returns a list of the dicts keys; [six, three, four, five one,
two]
d.values() returns a list of the dicts values; [zes, drie, vier, vijf, een,
twee]
d.items() returns a list of (key, value) tuples; [(six, zes), (three, drie),
(four, vier), (five, vijf), (one, een), (two twee)]

1.2 Syntax

Python has a few syntactic peculiarities which make it a unique, easy-to-learn and powerful programming-
language. These same peculiarities do tend to confuse people who are used to other lauguages, however.

Blocks: Logical blocks of code, for instance the code inside a while-loop or the body of an
if clause, are not enclosed in curly braces {}. Instead this is always handled by a simple colon
after the statement owning the block in question, and proper indentation of the block. The block
ends one line before the next statement that is less indented than the block itself. Thats right,
indentation matters! This also happens to prevent code that is unreadable because of shoddy or
no indentation from being executed. However, this does not imply that any Python-code is always
easily readable! Proper comments do help, as is the case in any programming-language.

The for Loop: The for statement comes in many shapes an sizes, and is often confusing. In
Python, there is only one for, and it can only do one thing; iterate over the elements of a list.
This might sound limiting, but it is in fact extremely powerful in practice (the elements of a list
can be any type of object!). For simple counting tasks, there is a built-in function, range(), that
returns a list of consecutive integers. The C-like statement for(i=2; i<5; i++) { . . . } becomes
for i in range(2, 5):
The if / while . . . in . . . Statement: The keyword in can also appear in conditional statements.
In this case, the item following in must be a sequence-type (i.e. list, tuple, string, etc.) The
statement is True if the (value of) the item before in occurs in the sequence following in. If the
item following in is a dict, the statement checks for occurrence in the list of the dicts keys (i.e.
if bla in d: is equivalent to if bla in d.keys():), to check for occurence in the dicts
values, use if . . . in d.values():
Unpacking: The individual elements of a list or tuple can be assigned to a set of variables in one op-
eration. This is called unpacking the list / tuple; (a, b, c) = (1, 2, three) will assign to a
the value 1, b the value 2 and c the value three. Of course, the number of variables to assign to must
be the same as the number of elements to be unpacked, otherwise a ValueError is raised. Unpacking
also works for the iterating variable in a for-loop; for (a, b) in ((1, 3), (2, 5), (4, 7)):

4
 The else Clause in for & while Loops: In Python, for- and while-loops can have an
else: clause. The body of this else-clause is only executed if the loop runs out (i.e. when for
has reached the end of the list it iterates over, or when the condition in the while-statement has
become False), but not if the loop is terminated by a break-statement. Very useful, that. . .

1.3 The SciPy package

Nearly all of the presented code uses the array and matrix objects provided by the SciPy package.
See http://www.scipy.org/ The SciPy package is actually a whole set of packages, each providing one
or more libraries of objects and functions that are very useful for many kinds of number-crunching tasks.
After the Scipy package has been installed, it has to be imported by our script before we can use it.

import scipy

Now, the SciPy package is huge, and contains a bunch of sub-packages which are not automatically
imported, even if you did import the SciPy core (with import scipy). Two sub-packages in particular
well be needing later, so we might as well import them now:

import scipy.linalg
import scipy.stats

1.3.1 SciPy Arrays & Matrices

The array and matrix objects that SciPy provides function similar to Pythons lists, but not exactly
the same:

Array-objects can be 0-dimensional (i.e a scalar), 1-dimensional (a vector) or higher-dimensional.

Arrays are always of uniform type, and array-elements cant be sequence types (lists, strings, etc.).
Arrays have a .shape() method which returns the length of each dimension (as a tuple)
Arrays can be created from lists using scipy.array(). Higher-dimensional arrays can be cre-
ated from hierarchies of lists, provided all the sub-lists have the same length. Arrays can be
turned back into (hierarchical) lists with the arrays .tolist() method. scipy.array([range(3),
range(2, 5), range(4, 7)]) will yield:

[[0, 1, 2],
[2, 3, 4],
[4, 5, 6]]

Array-elements are indexed just like lists, except that the indexes for multiple dimensions are now
comma-separated between one set of []. 0-Dimensional arrays cannot be indexed. Instead, use the
arrays .item() method to retrieve its value. (.item() works for any array with a single element,
regardless of its number of dimensions)
Arrays can be glued together with scipy.concatenate(), which allows specifying along which
dimension the arrays are to be glued, and sub-arrays can be taken using slice-notation or with
scipy.take().
Unary and binary operations with arrays tend to work on a per-element basis. In binary operations
with two arrays of different size or dimensions, attempts are made to broadcast the smaller array
to the larger. For example, when multiplying row-vector a (a.shape() == (3,)) with array b
(b.shape() == (3, 3)) we get:

  b0,0 b0,1 b0,2 a0 b0,0 a1 b0,1 a2 b0,2
a0 a1 a2 b1,0 b1,1 b1,2 = a0 b1,0 a1 b1,1 a2 b1,2
b2,0 b2,1 b2,2 a0 b2,0 a1 b2,1 a2 b2,2

5
 But when multiplying column-vector a (a.shape() == (3, 1)) with array b (b.shape() == (3, 3))
we get:

a0 b0,0 b0,1 b0,2 a0 b0,0 a0 b0,1 a0 b0,2
a1 b1,0 b1,1 b1,2 = a1 b1,0 a1 b1,1 a1 b1,2
a2 b2,0 b2,1 b2,2 a2 b2,0 a2 b2,1 a2 b2,2

If the broadcasting fails, a ValueError (shape mismatch) is raised.

Matrix-objects are a special variety of 2-dimensional arrays. Matrix-operations are generally not
per-element operations, but follow the rules of Linear Algebra. Now, when multiplying row-vector
a (of shape (3,)) with matrix b (of shape (3, 3)) we get:
T
  b0,0 b0,1 b0,2 a0 b0,0 + a1 b1,0 + a2 b2,0
a0 a1 a2 b1,0 b1,1 b1,2 = a0 b0,1 + a1 b1,1 + a2 b2,1
b2,0 b2,1 b2,2 a0 b0,2 + a1 b1,2 + a2 b2,2

(The result is actually another row-vector of shape (3,), but its represented here as a transposed
column-vector, to make it fit on the page.) But when multiplying column-vector a (of shape (3, 1))
with matrix b (of shape (3, 3)) we get:

b0,0 b0,1 b0,2 a0 a0 b0,0 + a1 b0,1 + a2 b0,2
b1,0 b1,1 b1,2 a1 = a0 b1,0 + a1 b1,1 + a2 b1,2
b2,0 b2,1 b2,2 a2 a0 b2,0 + a1 b2,1 + a2 b2,2

another column-vector of shape (3, 1).

Matrices can be created from 2-or-lower dimensional arrays with scipy.matrix(). Matrices can
be cast back to (2D) arrays with their .asarray() method, and even have a member-variable
.A which contains the array-representation of the matrix values. This is useful for escaping the
default Linear Algebra behaviour and doing per-element operations on matrices, as if it were a
regular array.

I have barely scratched the surface here. Im sure there are many more useful things one can do with
SciPy-arrays which i havent even discovered yet. SciPy is a large and extensive set of extensions to
Python, most of it written in C/C++ or Fortran and compiled as shared libraries that Python can use.
Operations on arrays and matrices will generally execute much faster than operating on the elements of
a list in a for-loop.
For more info on SciPy, see its on-line documentation: http://scipy.org/Documentation which is,
unfortunately, rather chaotic, sparse, and/or under construction. This is mainly due to the fact that
SciPy has unified a whole group of older Python-packages (its predecessors), and inherited whatever
documentation existed for those packages at the time. Apparently there is also a book on NumPy (one
of those predecessors mentioned above, NumPy provides the array- and matrix-objects, among other
things): http://www.tramy.us/

2 The Tracking-System Hardware

The goal of the implementation presented here is to calculate the position in space of the source of an
ultrasonic transmission, based on the difference in the time of arrival of the ultrasonic signal at different
receiving-stations placed in known locations. Ideally we would like the calculated position to be as
accurate as possible and to get a position-estimate for each transmission, preferably along with some
indication of how reliable the estimate is.

2.1 The Hexamite System

The Ultrasonic Positioning System we are using for this project is made by Hexamite (http://www.
hexamite.com/), in Australia. Their HX11 system consists of a (potentially large) number of receivers
on one multi-drop RS485 serial network, and some hand-held (or body-mountable) transmitters. http:
//www.hexamite.com/hx11.htm

6
2.1.1 The System Components

While working with this system, we have tended to stick with Hexamites naming-conventions. Through-
out the text and the presented code, the transmitters are called Tags, the receiving-stations are Moni-
tors and a serial-bus with a set of Monitors connected is a Network
The Tags work autonomously; theyre battery-powered, with no link to the outside world except the
ultrasound signal it transmits at regular intervals and a red LED that flashes when it does so. Each Tag
has a unique ID-number which is encoded in its ultrasonic transmissions. The interval between the Tags
transmissions can be adjusted (in 7 exponential steps) and it can be randomized (in fact, it randomly
chooses to wait either 1x or 1.5x the set interval between consecutive transmissions)
Since the Monitors, or indeed the whole Tracking-System, has no way of knowing exactly when a Tag
transmits, all the Monitors can do when they receive a Tags transmission is to refer to their internal
clocks, decode the Tag-ID from the transmission, and store the Tag-ID with the Time-of-Arrival (ToA)
in a buffer.

2.1.2 Buffering & Polling

The reason for buffering the Tags ToAs and not transmitting the data to the Tracking-Server as soon
as it is decoded is that the multi-drop, half-duplex serial-bus used does not allow multiple transmitters
(a.k.a. bus-masters or simply masters). If each Monitor could spontaneously start transmitting on
the bus after receiving a Tags signal, chaos would ensue. Especially since the Tags signal will arrive at
some Monitors almost simultaneously, and can even arrive exactly simultaneously, in case of equidistance.
There exist handshaking protocols and bus-collision avoidance schemes to deal with this issue, but all of
these require extra hardware (more wires), extra CPU-cycles (more processing) and/or cost bandwidth
because of protocol overhead.
The alternative solution is to have only one master on the bus, and initiate all communication on the
bus from this master. The Monitors must keep their data until the master ask each of them in turn
what they have in their buffers. This polling scheme (the master polls the Monitors for data) obviously
introduces some latency; the Tracking-System cannot calculate a position until (at least) 4 Monitors have
been polled and responded with valid data. Also, the master cannot know which Monitors have data
available until they have been polled. However, if the Monitors can be polled fast enough, so that the
average expected delay between data being available and data being received by the Server is equal to
(or even less than) the expected overhead of the handshaking-protocol that would otherwise be required,
it is an acceptable solution.
To query a Monitor for data, the Server must transmit (broadcast, in effect) the Monitors data-address
on the Network. Each Monitor has a data-address (which is always an even number between 0 and
65534) and a config-address (which is always its data-address + 1). These addresses are transmitted on
the Network as two raw (i.e. unencoded) bytes. When a Monitor receives its own data-address, it will
respond immediately with #<CR>, if its buffer is empty, or with <data> [<data>[. . .]]#<CR> (thats
one or more data-packets, separated by spaces and followed by a # and a Carriage-Return). The data-
packets are 8-digit hexadecimal numbers, where the two leftmost digits encode the Tag-ID, and the other
6 encode the ToA. All-in-all this seems to be a reasonably efficient, low-overhead protocol; to get a no
data response from a Monitor requires the transmission of only 4 bytes in total (a 2-byte address from
Server, and #<CR> from the Monitor), and a ToA-measurement can be acquired in 12 bytes, with an
additional 9 bytes (8 digits plus a space) for each additional Tag-ID/ToA-pair in the Monitors buffer.

2.1.3 Bits, Bytes and Baudrates

Of course, the effective speed with which all these transactions happen depends largely on the serial-port
speed (or bitrate / baudrate) used, and herein lies a major catch. The HX11 Monitors can communicate
on their common RS485 bus at two possible speeds. The default speed, 19200b/s, is a standard baudrate
which can be attained by any serial-port hardware made in the last 15 years. However this bitrate has
proved far to low to be usable in any serious deployment of this system!
In serial transmissions, it takes 10 bits to transmit a byte, not 8, so at 19200b/s it would take 1000/1920 =
0.52 msec to transmit one byte. At this rate, getting a no data response takes > 2 msec, and a single

7
data-packet > 6 msec. When polling 12 Monitors, say, a full poll-cycle takes at least 24 msec with no
data available, and around 50 msec if half of them have received one Tag-transmission.
These calculations only take into account the time used for serial-bus data-transmission. In addition,
there will a small turnaround delay at each Monitor; the time it takes for the Monitor to check if the
last two bytes received are indeed its address and to start transmitting its reply. At the other end, there
is another delay while the server-side software decodes the reply, stores the Tag-ID/ToA-pair for further
processing and prepares to transmit the next Monitors address. These delays are arguably very short,
but they are incurred for each Monitor on the Network. For our 12-Monitor example, these last delays
might add another few msec to the duration of a full poll-cycle.
At this point, the Network poll rate for a realistic Tracking-Network (12 Monitors is still quite a small
Network, in terms of floor-area covered.) has fallen well below 20Hz, and with that it cannot realistically
be called a real-time Tracking-System.
That leaves the other option; the HX11-units can be configured to operate with a serial-bus speed of
250kb/s. Thats over 13 times faster than 19200b/s, and would seriously reduce the time taken up by
serial transmissions. At this speed, the full polling-time for each Monitor, including serial-transmission
and turnaround delays, stays well under 1 msec (for replies containing upto 2 data-packets). However,
250kb/s is not a standard baudrate and is not supported by most serial-ports (nor by the standard
serial-port drivers!).
One important task, then, before the implementation can even begin, is to find a special serial-port card
that can operate at 250kb/s (half-duplex), is supported by our OS-of-choice (Linux), and preferably comes
with very detailed documentation (or even better; source-code for the driver) so that we may customize
the driver-code to the point where it lets us set the serial-port speed to 250kb/s. See section 2.3 below.

2.1.4 Clock Synchronisation

One clear advantage of the single-master bus topology is that the master can broadcast messages on the
Network that will be received by all Monitors at the same time. For example, the master can send a
sync message which will be received by all Monitors on the Network and cause all Monitors to reset
their internal clocks. After this, the Monitors clocks will run in sync and the reported ToAs from all
monitors will specify the time of arrival relative to a common point in time, namely the last time a sync
message was sent.
Care should be taken that the synchronisation of the Monitors happens often enough, so that inter-
Monitor clock-drift due to small variations in temperature or component tolerances does not become a
significant factor. The caveat here is that synchronisation should not happen too often either: If the
Monitors clocks are reset while a signal from a Tag is still in flight, some Monitors may have received the
signal before the clock-reset happened, reporting their ToAs relative to the previous sync-event, while
other Monitors receive the signal after sync, and report their ToAs relative to the sync that just happened.
Thus we have a set of measurements, all from the same Tag-transmission, but with two disparate frames-
of-reference. Such a set is not directly usable for a position-calculation, depending on the number of
Monitors involved; if neither subset has 4 Monitors, we cant calculate a position. It may be possible for
the tracking-software to keep track of exactly how much time passed since the last sync, and subtract
that amount of time from the ToAs reported by the pre-sync Monitors, but this has not (yet) been
tested.
Another effect that can cause similar problems is clock rollover: The HX11-units use a 24-bit clock to
keep track of time. Their time frame starts at 0 and is incremented every tick until it reaches 224 1.
On the next tick of the clock, however, the counter will jump to 0 again, causing the same problem as a
clock-reset through synchronisation from the Server, if this occurs while transmission from a Tag is still in
flight. Again, it may be possible for the tracking-software to detect when this has occurred, and subtract
an amount of time equivalent to 224 clock-ticks from the ToAs reported by pre-rollover Monitors. Always
assuming that inter-Monitor clock-drift is indeed negligible and that all Monitors clocks rollover at the
same time! The HX11 clocks run at 16MHz, and so rollover occurs once every 224 /(16 106 ) = 1.048576
seconds.
Since there is no fixed temporal relationship between the actual time the Tag transmits and when the
Monitors receive the ultrasound signal, nor between the time a Monitor has data ready and the time

8
the Server polls the Monitor, the temporal accuracy of the system is not very good. By the time you
finally have a position calculated, there is no way of knowing exactly how much time passed since the tag
actually was in that position. We can only hope that the spatial accuracy of the system is good enough.

2.2 The Tracking Server

For the position-tracking application, we have installed Linux on a 2.4GHz Intel Core2Duo machine. It
is running a Linux 2.6.15 kernel which has been customized and compiled with the scheduler frequency
set to 1000 Hz instead of the default 100 Hz. This has been done to allow sleep() calls of milisecond
duration.
The dual-core architecture was chosen to allow for great performance benefits when running multi-
threaded applications. But apparently, in Python, this is not so straightforward. In fact, creating
multi-threaded applications in Python is relatively easy, but these multiple threads are guaranteed not
to execute in parallel, even when multiple CPUs or cores are available. Multiple processes, however, do
execute in parallel on the available cores.
There are many on-line discussions going on about the not-really-multi-thread capability of Python and
threads vs. processes. It is well beyond the scope of this document to discuss these subjects here (although
I will discuss the latter to some extent in section 6). If you are interested, do a web-search on Python
GIL, and youll see what i mean.

2.3 The High-Speed RS485 Serial-Port Card

In order to connect the RS485 multi-drop bus which forms the HX11 Network to our Tracking Server,
we needed to find a RS485-compatible serial-port card. Preferably one thats capable of communicating
at 250kbps, which is certainly not a standard speed for serial ports in general.
A PCI-card which fits these requirements is made by BrainBoxes; the CC-525 http://www.brainboxes.
com/product/PCI_RS422485/CC-525.aspx card.

2.3.1 The setserial Utility

3 Collecting ToAs and Making TDoAs

The proposed algorithm will return a 3D position-estimate calculated from 4 (or more) ultrasonic TDoA
measurements. A Hexamite-Network polling and ToA-collecting application has been written. This
application polls all Monitors on the Network at a suitably high rate (1000 Monitor-polls/sec). If a
Monitor has heard a Tags signal since the last time it was polled, it will return the signals Time of
Arrival (ToA) together with the associated Tag-id. The received ToA/Tag-id pairs are grouped by Tag-
id, and re-associated with the Monitor that reported the Tag. This process builds a buffer of ToA/Mon-id
pairs for each Tag seen. Along with the ToA and the Monitors ID, the Monitors position in 3D space
is also stored in the Tags ToA-buffer.

3.1 Gathering ToAs into Events

Since the Tags ToA buffer grows with each poll-reply from each Monitor that received the Tags signal,
it becomes important to figure out which ToAs in the buffer belong to the most recent transmission from
that Tag, and delete any left-over ToAs from earlier transmissions. In order to do this, we must first
define a time-window within which we consider ToAs to belong together. That is; all ToAs falling within
this time-window form one event, for which a position should be calculated.

9
3.1.1 The TDoA-Window

This time-window should be wide enough to allow large ToA-differences to be accepted, but small enough
not to get ToAs from consecutive events grouped together. If we know the maximum distance that a
Tag-transmission could travel through the air and still be reliably detected, we can calculate the max
expected Time-of-Flight of the transmissions, and that gives us a lower limit for the TDoA-window:

TmaxT oF = Rmax /csnd (1)

where Rmax is the Tags max expected transmission-range, in m, and csnd is the speed of sound. The
maximum range of the Tags should be specified by the systems manufacturer. In our case, with the
HX11 system, the Rmax given is 8 m.
In Python, this becomes:

tdoa_window = tag_max_range / c

3.2 Poll-Cycles

The Network can only poll the Monitors for data one after another. Because each transaction with a
Monitor takes time (about a millisecond, in high-speed mode) the polling progresses linearly over time.
However, the Time of Arrival of the signal from the Tag at each Monitor is dependant only on the Tags
transmission-time and its distance to the Monitor, and the distribution in time of the ToAs from various
Monitors is highly non-linear. Moreover, the poll-interval per Monitor and the TDoAs are more-or-less
in the same order of magnitude; milliseconds.
I found it very insightful to visualise the progress of the poll-cycle and the ToAs in a set of timing-
diagrams for various cases. In these graphs Ive drawn a timeline for the Tag (at top) and one for each of
the Monitors involved. The consecutive Poll-Cycles are depicted as a row of cells at the bottom, and the
poll-cycles progress through the Network, from the top Monitor to the bottom one, is drawn as slanted
dashed lines. Bear in mind that each Monitors datapoint (ToA) is only available to the Server after the
Monitor has been polled, i.e. when the slanted dashed line first crosses the Monitors timeline behind
(i.e. after ) the datapoint.
The dashed rectangles in the diagrams represent the TDoA-window (see section 3.1.1). The start of this
TDoA-window is determined by the earliest ToA, and the duration (or width) of this window is dictated
by the maximum time an ultrasonic signal from a Tag can be expected to travel before it becomes too
weak to be reliably detected.

3.2.1 Calculating the Position with Minimum Latency

The goal is to implement a real-time 3D tracking-system. Ideally, we would like to be given the position
of the Tag in space at the very instant it transmits a signal. Unfortunately, the signal has to travel
through the air a bit before it reaches a number of Monitors, and this takes time. Then some more time
passes as the ToAs measured by the Monitors are collected by the Server, before the position is finally
calculated. It is this latency, the amount of time that passes between the Tag transmitting from a certain
position in space and the Server reporting this position to other (client-) applications, that we would like
to minimize.
Real real-time-ness is out of the question, given the ultrasonic nature of the system. But it might be
possible to minimise the latency of the system. The first delay depends only on the positions of the Tag
and Monitors in space and cannot be reduced or minimized. The latter bit of the total latency depends on
the serial-bus speed and the specific implementation of the data-collecting part of the tracking software
described here, and therefore merits some detailed discussion.
We have a high-speed RS-485 serial-port card operating at 250kb/s (see section 2.3), so that part is
already optimal. Next we investigate the interaction between and, in particular, the timing of the arrival
of the Tags signal at the Monitors and the Servers poll-cycles for various possible cases.

10
 t=0
Tag TX
Monitor RX
TDoA Window
Poll Cycle Empty

Sufficient data

Polling Progress

Pos Calculation
Total Latency

Figure 1:
The ideal case where all data is available at the end of one poll-cycle.

In this case, all Monitors data is collected in one poll-cycle, and the Tags position can be calculated
(using all 7 datapoints) at the point in time marked with the vertical arrow .
As stated before, a significant portion of the total latency is simply the Tags signals ToF, and this cannot
be minimised. The remainder of the total latency is determined by the duration of the poll-cycle(s) that
gather(s) the datapoints used in the position-calculation. The simplest scheme, where a position is
calculated as soon as enough (4 or more) datapoints have been collected, will certainly minimise this
portion of the latency, but it has a few drawbacks in cases where the arrival of the datapoints is timed
less ideally as shown in figure 1.

t=0
Tag TX
Monitor RX Used in position-calculation

Unused in position-calculation

Buffered, then used

TDoA Window
Poll Cycle Empty

Sufficient data

Insufficient data

Polling Progress
Pos Calculation
Total Latency

Figure 2:
Data arrives spread across 3 poll-cycles.

Here, the 3 datapoints collected during the first (light grey) poll-cycle are insufficient to calculate a
position from, and they are simply stored in the buffer in the hope that the next poll cycle will yield at
least one more datapoint. And so it does. Three more datapoints arrive, so by the end of the second
(dark grey) poll-cycle we have 6 datapoints, and calculate a position from those.
The total latency in this situation is one full poll-cycle longer than in the case of figure 1
The 7th datapoint, arriving during the third poll-cycle, is initially buffered as well, but since no further
datapoints are forthcoming, it is eventually removed from the buffer as a stale ToA. This suboptimal use
of the available data causes a loss of precision, and the situation could be worse if 4 datapoints happen
to be available in the first poll-cycle.

11
 t=0
Tag TX
Monitor RX Used in position-calculation

Unused in position-calculation

TDoA Window
Poll Cycle Empty

Sufficient data

Insufficient data

Polling Progress

Pos Calculation
Total Latency

Figure 3:
Data arrives spread across 3 poll-cycles, only 4 datapoints are used

Now only the first 4 datapoints are used, the rest ignored. The latency is again minimal, but clearly we
are minimising latency at the expense of accuracy. That is probably not the desired behaviour for this
application.
Another problem arising from the calculate as soon as you can approach is that, with 8 or more Monitors
involved, we might get double position-calculations, each with minimal precision and one poll-cycle apart,
the for the same Tag-transmission:

t=0
Tag TX
Monitor RX
TDoA Window
Poll Cycle Empty

Sufficient data

Polling Progress

Pos Calculation
Total Latency

Figure 4:
Data arrives in sets of 4 per poll-cycle.

Note that the poll-cycles in figure 4 are slightly longer ( 8 ms) than in figures 1 - 3 ( 7 ms), because
8 Monitors take longer to poll than 7 Monitors.
In this situation, 4 datapoints are available after the first poll-cycle, and a position is calculated (with
minimal latency and minimal precision). In the next poll-cycle, 4 more datapoints arrive and the position
is again calculated, again with minimal precision. Since both calculated positions are of the same Tag-
transmission, one could detect this double-banger grouping of ToAs and then take the mean of the two
calculated positions as the final result, but it can be shown that doing one position-calculation based on
all 8 datapoints is slightly more efficient and far more accurate.

3.2.2 Calculating the Position with Maximum Accuracy

It becomes clear that a compromise has to be found between minimising latency and maximising the
accuracy (make optimal use of data). To optimise the accuracy of the position-calculations, we should
wait until we are certain that all available data has been gathered. One easy way to do this would be to
wait with calculating the position until the end of the first empty poll-cycle after one or more poll-cycles
yielding datapoints.

12
 t=0
Tag TX
Monitor RX Used in position-calculation

Buffered, then used

TDoA Window
Poll Cycle Empty

Sufficient data

Polling Progress

Pos Calculation
Total Latency

Figure 5:
Data is buffered until an empty poll-cycle occurs.

It is evident that this approach has an increased latency in comparison to the approach outlined in the
previous section. In fact, the total latency will be one poll-cycle longer in all cases, even the ideal case
of figure 1.

t=0
Tag TX
Monitor RX
TDoA Window
Poll Cycle Empty

Sufficient data

Insufficient data

Polling Progress

Pos Calculation
Total Latency

Figure 6:

The double-banger problem illustrated in figure 4 is also avoided by this scheme:

t=0
Tag TX
Monitor RX
Buffered, then used

TDoA Window
Poll Cycle Empty

Sufficient data

Polling Progress

Pos Calculation
Total Latency

Figure 7:

With a small number of Monitors, and the Network operating in high-speed mode, the additional latency
introduced by this scheme is still acceptable, but with larger numbers of Monitors, the poll-cycles get
longer, and the additional latency can quickly become unacceptable. We should look for some other
point in time at which we are certain that all data has arrived. In principle, the end (i.e. the right-hand
edge) of the TDoA-window provides just that: When the TDoA-window expires, we can be certain that
no other Monitor can still receive the signal from the Tag, because the signal has traveled too far to be
detected by Monitors at that range or even further away.
If we calculate the Tags position at the end of the first empty poll-cycle after one or more poll-cycles
yielding datapoints, or when the TDoA-window expires (whichever comes first), we are guaranteed to

13
make optimal use of the data, and still keep the total latency within acceptable limits. In fact, the
worst-case total latency can now be calculated exactly:
 
R0
TmaxLatency = + TmaxT oF (2)
csnd

where R0 is the distance between the Tag and the first Monitor to receive the Tags signal (see sec-
tions 4 & 4.1.2), csnd is the speed of sound, and TmaxT oF is the width of the TDoA-window, as given by
equation 1.
With Networks containing larger numbers of Monitors, the poll-cycles last longer too. In these cases,
calculating the position at the end of the TDoA-window results in much lower latencies than simply
waiting for an empty poll-cycle would.

t=0
Tag TX
Monitor RX Used in position-calculation

Buffered, then used

Unused in position-calculation

TDoA Window
Poll Cycle Empty

Sufficient data

Insufficient data

Polling Progress
Pos Calculation
Total Latency

Figure 8:
A Network with 24 Monitors. Not all Monitors are within range of the Tag.

However, when the TDoA-window lasts approximately as long as, or shorter than, a poll-cycle, a new
problem arises; in order to calculate the position when the TDoA-window expires, the poll-cycle will
likely be interrupted somewhere along the line, and certain ToAs that lie inside the TDoA-window might
not be available to the Server when the position is calculated. The Tags signal arrives at the Monitor
on time (within the TDoA-window) but the Monitor is polled too late (outside the TDoA-window).
In figure 8 the ToA from Monitor 13 (counting Monitors from the top of the diagram) is polled by the
server at the point in time indicated by the vertical grey dashed line, just after the position has been
calculated. The ToA from Monitor 1 is only available to the Server at the beginning of the next poll-cycle,
well after the position has been calculated.
This problem could be averted by first estimating the Tags position (or using the Tags previous known
position, if the Tag transmits often enough), then calculating the distance from each Monitor to the Tags
estimated position, and then polling the Monitors in order of proximity to the Tag.

14
 t=0
Tag TX
Monitor RX Used in position-calculation

TDoA Window
Poll Cycle Empty

Sufficient data

Polling Progress
Pos Calculation
Total Latency

Figure 9:
A Network with 24 Monitors, polled in order of proximity.

The ToAs in this case are exactly the same as in figure 8 but now the Monitors are arranged top-to-bottom
in order of proximity to the Tags expected position. Now all ToAs fall neatly within one poll-cycle, and
by the time the TDoA-window expires, we are guaranteed to have polled all Monitors that lie within the
Tags range (because the Tags range defines the TDoA-windows width). Even if not all the Monitors
expected to be in range actually receive a signal from the Tag.

3.2.3 Sync Correction

As explained in section 2.1.4 earlier, the Monitors clocks need to be synchronised ever so often. If this
synchronisation happens less often than once every 1.048 second, clock-rollover will occur, and regular
synchronisation is still necessary. Either way, because of clocks being reset or rolling-over, there will be
large jumps in the timestamps recorded by the Monitors just before, and just after the clock-reset or
-rollover. In fact, the post-sync or -rollover ToAs will appear to come from a much earlier time, when in
reality they arrived after the pre-sync or -rollover ToAs.
If the jumps in time are much larger than the TDoA-window, they might be detectable by the ToA-
grouping algorithm. If the clock-jumps due to sync or rollover can be reliably detected, and if we know
the exact amount of time that was jumped, we could potentially correct any post-sync or -rollover ToAs
by adding the exact amount of time that was lost in the jump.
In the case of clock-rollover, the jump is known to be exactly 224 /(16 106 ) = 1.048576 seconds. In
the case of clock-sync, it is exactly the amount of time that passed between the last and before-last
sync-messages.

15
 t=0 t=0
Tag TX
Monitor RX Used in position-calculation

Buffered, then used

Sync-corrected, used

TDoA Window
Poll Cycle Empty

Sufficient data

Insufficient data

Polling Progress
Pos Calculation
Total Latency
Clock Sync
Sync Interval

Figure 10:
Sync-correction in action. The four post-sync ToAs (at left) get the sync-interval added to them.

Note that the sync-interval in figure 10 is approximately 100 msec. This is an unrealistically short
sync-interval, but it serves the purpose of illustrating the described principle.
Of course, after sync-correcting any ToA, we still need to verify weather the corrected ToA falls within
the TDoA-window. Since this rather complicates matters for the algorithm that groups the ToAs into
sets belonging to a single Tag-transmission.

3.3 Grouping ToAs

In order to keep track of which ToAs belong to one Event, the Tag object keeps track of what it thinks
are the earliest and latest ToA of the current event and checks if new ToAs fall within the TDoA-window.
If the new ToA falls in-between the current Event Start Time (EST) and Event End Time (EET), it is
only tested to see if the ToA represents an indirect path (i.e. a reflection) (see section 3.3.1) If the new
ToA falls outside the current limits of the Event, this could imply that a new Event has occurred, and
ToAs from that event are beginning to arrive. However, it could also simply mean that the current EST
or EET need to be updated to include the new ToA.
If the new ToA is earlier than EST, but later than (EET - TDoA-window), the EST is adjusted (the
current ToA is the earliest one), and the ToA is stored in the buffer. Checking for reflections is unnecessary
in this case, because the earliest ToA must always represent a direct path.
If the new ToA is later than EET, but earlier than (EST + TDoA-window), The ToA is first checked
against reflections, and if it passes the EET is adjusted (the current ToA is the latest one), and the ToA
is stored. In adjusting the EST or EET, the interval between them can never exceed the TDoA-window.
If the new ToA falls outside the TDoA-window entirely, this could mean that ToAs from a new Event
have started to arrive, or possibly a clock-synchronisation has occurred while the Tags signal was in
transit. (see sections 2.1.4 & 3.2.3) To verify whether the latter is the case, we can add or subtract the
sync-interval to/from the ToA, and check again if the sync-corrected ToA falls within the TDoA-window
(after correction), adjusting EST or EET if required, as described above.
If the new ToA still falls outside the TDoA-window, even after sync-correction, this could have 2 possible
causes; either a new Event has occurred and this is its first (though possibly not its earliest) ToA, or the
ToA under consideration is an outlier. (i.e. a glitch in the system, or a measurement resulting from an
exceptionally long reflected path). Unfortunately we have no way to distinguish between these two cases,
based on the new ToA and the ToAs already collected in the buffer.
However, looking at the buffers contents allows us to make some sort of decision; if the buffer contains
less than 4 datapoints (which are all related, i.e. belonging to the same Event), these might just have
been leftovers from an earlier poll-cycle with insufficient data, and should be cleared from the buffer.
After this, the current ToA is stored (because it is the first ToA of the new event) and EST & EET are
set equal to the ToA.
On the other hand, if the buffer contains 4 or more datapoints, we can calculate a position from the ToAs
already collected, and we can choose to ignore any additional ToAs that fall outside the TDoA-window.

16
The sequence of ifs above and the various possible cases for the new ToA may seem a bit convoluted,
but they do have a certain symmetry which is best illustrated with a flow-chart:

Mon / ToA
pair

Mon: Monitor ID
ToA: Time of Arrival
EST: Event Start Time
EET: Event End Time
buflen > 0 TDoAw: Time Difference of Arrival Window
n ? Dsync: Interval between last 2 sync-msgs
buflen: number of datapoints in the buffer
y

ToA >= EST

? n
1 3
y

ToA <= EET

5
n ?

y
ToA <= (EST ToA >= (EET
6 4
+TDoAw) ? n 2 n -TDoAw) ?

y y
ToA -= Dsync ToA += Dsync

5 First
8 Correction
n
?

y
buflen < 4
? n

y
Reject Reflections Reject Reflections
9

Clear ToA-buffer Ignore ToA

Set EST = ToA

5 2 3
Set EET = ToA

Set EET = ToA Set EST = ToA

Store ToA @ Mon

Figure 11:
Flow-chart of the ToA-Grouping algorithm

The various cases, or paths by which a new ToA might be stored in the buffer, are numbered in figure 11:

1. The buffer is empty (maybe because a position-calculation was just completed); the new ToA sets
EST & EET and is stored.

17
 2. The new ToA lies between the current EST & EET, is checked for reflections and stored.

3. The new ToA is earlier than EST and later than (EET - TDoA-window), it sets a new (earlier)
EST, and is stored.
4. The new ToA is earlier than EST and outside the TDoA-window. Add the sync-interval to the ToA
and retry (see 7)
5. The new ToA is later than EET and earlier than (EST + TDoA-window), it is checked for reflections,
sets a new (later) EET, and is stored.
6. The new ToA is later than EET and outside the TDoA-window. Subtract the sync-interval from
the ToA and retry (see 7)
7. If the current ToA has just been sync-corrected for the first time (in 4 or 6), retry the process from
step 2.
8. Previously sync-corrected ToAs that still fall outside the TDoA-Window will flow through the
opposite branch on the second pass (because the sync-interval will always be much greater than
the TDoA-window) , and get the sync-correction removed from them (via 4, then 6 or via 6, then
4). Then we look at the current number of datapoints in the buffer to decide weather to ignore the
new ToA, or to delete the ToAs already collected (via 9)
9. Stored ToAs are cleared from the buffer. The new ToA sets EST & EET and is stored.

What happens inside the Reject Reflections boxes is described below, in section 3.3.1
There is a risk that this scheme, specifically the rejection of outliers or already collected data based
on the number of collected datapoints, throws away three valid datapoints when the fourth incoming
ToA happens to be an outlier. Even worse, taking the outlier as the new Events reference will cause
subsequent rejection of any following ToAs which are correct. But this only happens if the fourth ToA is
an outlier, and thus has a chance of pO
N of occurring, where pO is the expected probability of any outliers
and N is the number of Monitors involved, with N = 4.
Alternative approaches for handling outliers or new Events could involve counting the number of rejected
outliers, and only clearing the buffer (with < 4 datapoints) if the reject-count would otherwise become
equal to the number of datapoints already in the buffer.
Another, interesting approach would be to temporarily store any rejected ToAs (i.e. those that made it
to step 8) in a second buffer, and moving the contents of the second buffer into the first buffer (observing
steps 1 - 7), after a position has been calculated and the first buffer cleared. After all, the rejects in
the second buffer might be outliers, but could also be ToAs from the next Event. Any ToAs that get
delegated to the second buffer again during the transfer to the first buffer truly are outliers and can be
deleted. This could be combined with the reject-counting idea; if the length of the second buffer would
become equal to the length of the first buffer, clear the first buffer and transfer the contents of the second
buffer (again observing steps 1 - 7)

3.3.1 Catching Reflections

Given the nature of the ultrasonic measurement-system were using, it is not impossible for a single
transmission from a Tag to be received by a specific Monitor more than once. The signal could be
reflected off walls or pillars in the space, in which case the direct (i.e. unreflected) path has the shortest
Time-of-Flight, and therefore the earliest ToA.
We can check if the ID of the Monitor reporting the current ToA already exists in the buffer. If so, we
should select the earliest ToA of the two, or rather; overwrite the ToA in the buffer with the new ToA
only if the new ToA is earlier than the stored ToA.

18
 Mon / ToA
pair

Mon exists in
n buff ?

ToA <
buff[Mon]
n
?

Mon / ToA
Ignore ToA
pair

Reject Reflections
Figure 12:
The Reject Reflections routine from figure 11

3.3.2 The ToA-Grouping Algorithm in Python

Because all the ToAs stored in the Tag-objects ToA-buffer have unique Monitor-IDs (any duplicate ToAs
are filtered-out by the Reflection Rejection routine described above), we can store the Monitor-ID / ToA
pairs in a dict. This has the advantage that checking for duplicates becomes really easy, and overwriting
a later ToA (from a reflected path) with an earlier ToA (from the direct path) becomes automatic.

# in class Tag(object):
def catchReflections(self, mon_id, toa):
"""Check if the given Monitor already exists in the buffer.
If so, check if the given ToA is less than the one already stored,
and overwrite it if this is the case.
Returns True if the Monitor existed.
"""
ret = False
if mon_id in self.toa:
if toa >= self.toa[mon_id]:
return True # not earlier, ignore toa but report rejection

ret = True

self.toa[mon_id] = toa

return ret

Additionally, we can easily find EST & EET by using the built-in functions min(. . .) and max(. . .) on
the buffers .values()-list (i.e. the list of ToAs in the buffer), and dont have to keep these in separate
member-variables.

19
 # in class Tag(object):
def storeToa(self, mon, toa):
"""Store the given HX11Timestamp from the given Monitor for this Tag.
Clears all other toas from the buffer if new toa is not within the
max expected tdoa-window.
Returns the length of the buffer.
"""
if len(self.toa) == 0:
self.toa[mon.addr] = toa
return 1

est = min(self.toa.values())
eet = max(self.toa.values())

cnt = 0 # count the number of sync-correction retries

while cnt < 2:
if (toa >= est) and (toa <= eet):
self.catchReflections(mon.addr, toa)
break

elif (toa < est):

if (toa >= (eet - self.net.tdoa_window)):
self.toa[mon.addr] = toa
break

else:
toa += self.net.sync_delta
cnt += 1

elif (toa > eet):

if (toa <= (est + self.net.tdoa_window)):
self.catchReflections(mon.addr, toa)
break

else:
toa -= self.net.sync_delta
cnt += 1

else: # this is the else of while, only executed if cnt == 2

if len(self.toa) < 4:
self.toa = {mon.addr:toa}

return len(self.toa)

At this point, we have a ToA-buffer with unique Monitor-ID/ToA-pairs. However, as described in section
3.2.2, we want the further processing of the collected ToAs (i.e. the actual position-calculation) to wait
until an empty poll-cycle occurs, or until the TDoA-window expires, whichever comes first.

20
3.4 Calculating TDoAs

As the next step in the process, the Monitor-IDs are used to look-up the Monitors positions in space,
and the Monitors coordinates and measured ToAs are packed into a 4xN array, the ToA array:

X0 Y0 Z0 T oA0

X1 Y1 Z1 T oA1

T oA =
X2 Y2 Z2 T oA2
(3)
X3 Y3 Z3 T oA3
.. .. .. ..

. . . .

the contents of this array are sorted by ToA and the earliest ToA is subtracted from all ToAs in the
array. This yields a set of TDoA data, where the TDoA for the reference Monitor (the one that received
the signal first) equal to 0.

0 T oA0 T oA0

T DoA1,0
T oA1 T oA0


T DoA2,0 =
T oA2 T oA0
(4)
T DoA3,0 T oA3 T oA0
.. ..

. .

Additionally, the time-differences are converted to distance-differences:

Rn,m = T DoAn,m csnd (5)

where Rn,m is the distance-difference in meters (i.e. the Tag is Rn,m meters closer to Monitor m than to
Monitor n). T DoAn,m the time-difference of arrival in seconds between Monitor m and Monitor n, and
csnd is the speed of sound in m/s.
Finally the Monitors coordinates and measured distance-differences are packed into a 4xN array, the
DDoA array:

X0 Y0 Z0 0

X1 Y1 Z1 R1,0

DDoA =
X2 Y2 Z2 R2,0
(6)
X3 Y3 Z3 R3,0
.. .. .. ..

. . . .

where Xn , Yn , Zn are the coordinates of Monitor n, and Rn,0 is the difference in the distance measured
by Monitors n and 0, with R0,0 always equal to 0, of course.
The array-indexes of equations 4 - 6 do not correspond to the indices in eqation 3, because the ToA-array
is first sorted in order of increasing ToA before the DDoA-array is constructed.
Note especially that the actual distance between the Tag and the reference Monitor (R0 ) is unknown, all
we know is the additional distances the signal traveled to the other three Monitors!
In Python, the sorting of the buffer contents is done indirectly; first the function a_idx = scipy.argsort(a)
returns an array with the indexes into the original array in sort-order. That is, if we were to take el-
ements from the original array, in the order dictated by the index-array, wed get a sorted copy of the
original. Next, the function b = scipy.take(a, a_idx, 0) does exactly that, it builds a new array,
taking elements from the original array indicated by the index-array, accumulating along the specified
axis.

21
 # in class Tag(object):
def groupToas(self):
# look-up monitor positions
tmp = []
for (addr, toa) in self.toa.items():
tmp.append(self.net.monitors[addr].pos.tolist() + [toa])

# build array of [X Y Z ToA] rows

toa = scipy.array(tmp)

# clear toa-buffer
self.toa = {}

# get indexes of the toas in buf, in sort-order!

toa_idx = scipy.argsort(toa[:,3])
# get earliest toa
first = toa[toa_idx[0],3]
# get subarray with monitor positions, sorted by toa
mon = scipy.take(toa[:,:3], toa_idx, 0)
# calculate distance-differences, sorted by toa
dd = (scipy.take(toa[:,3], toa_idx, 0) - first) * self.net.c
# assemble mon & dd into one array
ddoa = scipy.concatenate((mon, dd.reshape(len(dd),1)), 1)

return ddoa

4 Calculating the Position

When all the available measurments have been grouped (or rejected), we have a DDoA-array where each
row of the aray represents one measurement, or datapoint. The array will have at least 4 rows, but
possibly more, depending on the number of Monitors in range of the Tag. As it turns out, we will have to
treat different DDoA-arrays of different sizes differntly. Not only that, but the geometry of the positions
in space of the Monitors involved is also relevant. But well get to that later, lets first look at the simplest
case; when we have exactly 4 measurements from 4 Monitors.

4.1 Chans Algorithm with 4 Monitors

To find the Tags position, we can solve Chans original equation;

1 2
Tx X1,0 Y1,0 Z1,0 R1,0 R1,0 K1 + K0
1 2
Ty = X2,0 Y2,0 Z2,0 R2,0 R0 + R2,0 K2 + K0 (7)
2 2
Tz X3,0 Y3,0 Z3,0 R3,0 R3,0 K3 + K0

but only if R0 is first known. All other variables in this equation are known; Xn,0 , Yn,0 , Zn,0 are the
coordinates of Monitor n relative to the reference Monitor (Monitor 0), and Kn is the squared distance
of Monitor n to the Networks origin:

Kn = Xn2 + Yn2 + Zn2 (8)

At first glance, it looks like we have three equations in four unknowns, but this is not the case, because
the distance-differences Rn,0 are not independent of the reference-distance R0 , in fact Rn = R0 + Rn,0 .
Also, given four distance-difference measurements (with R0,0 = 0) we should be able to solve all four
unknowns.

22


Writing down an equation to find Rn given the Tag position T and the position of Monitor n;
q
Rn = (Xn Tx )2 + (Yn Ty )2 + (Zn Tz )2 (9)

if n = 0, substituting equation 7 for T produces a quadratic equation for R0 in terms of the Monitor
positions and the measured distance-differences. To solve this equation, we need to separate-out the
terms in Chans equation. As stated before, we can calculate all of these terms (except for R0 ) from the
data in the DDoA-array from equation 6.
First, however, we convert the DDoA-array to a matrix D:

4.1.1 Dissecting Chans Formula

First we need to calculate the positions of Monitors 1, 2 & 3 relative to Monitor 0, because the measured
distance-differences are also relative to Monitor 0. We need to find the Position-Difference matrix P :

X1,0 Y1,0 Z1,0 X1 X0 Y1 Y0 Z1 Z0
P = X2,0 Y2,0 Z2,0 = X2 X0 Y2 Y0 Z2 Z0 (10)
X3,0 Y3,0 Z3,0 X3 X0 Y3 Y0 Z3 Z0

Or, in Python:

# in class Tag(object):
def doChanForFour(self, D):
P = D[1:4,0:3] - D[0,0:3]

where D is the DDoA scipy.matrix returned by the groupToas() function from section 3.4. In Python,
subtracting a 1x3 matrix from a 3x3 matrix will do the right thing. That is, the vector D[0,0:3] is
subtracted from each of the rows of sub-matrix D[1:4,0:3].
Next we define matrix A being the negative inverse of P:

A = P 1 (11)

A = -P.I

The .I member of a scipy.matrix contains the matrix inverse. This gives us the first term of Chans
equation.
Note that, if all four Monitors lie in one plane, matrix P will be singular, and will not have an inverse! In
this case, Python will raise a scipy.linalg.LinAlgError when trying to access P.I. We can trap this
error, abort the calculation and take action accordingly (maybe trying a different set of Monitors). This
is the main reason for importing scipy.linalg

 T
From matrix D we can also easily extract the column-vector R = R1,0 R2,0 R3,0 which appears
in the middle of equation 7.

R = D[1:4,3]

2  2 2 2
T
For the final term of equation 7, well need a column-vector R = R1,0 R2,0 R3,0 . The easiest


way to do this, in Python, would be to multiply R with itself using element-by-element multiplication,
not matrix-multiplication. Python allows for escaping the default matrix-multiplication behavior by
using the matrices array-representation (the .A member of a scipy.matrix). The resulting scipy.array
can simply be cast back to type scipy.matrix

23
 R_squared = scipy.matrix(R.A * R.A)

And finally we need the squared distance from the origin to each Monitor (the Kn terms, see eq. 8). To
find the squared length of a vector, one can matrix-multiply the vector with its transpose.

T
Kn = Xn2 + Yn2 + Zn2 = M n M n (12)

 
where M n = Xn Yn Zn is the position of Monitor n.
Lets store the Kn values in an array, for convenience:

K = scipy.zeros((4,1)) # creates a 4x1 array filled with zeros

M = D[:,0:3] # M is a sub-matrix of D; Ds first 3 columns
for n in range(4):
K[n] = M[n] * M[n].T

The .T member of a scipy.matrix holds the matrix transpose.

Now we can define vector B , the last term of equation 7:
2
R K1 + K0 K1

1 1,0
2 1
2
B = R2,0 K2 + K0 = R K2 + K0 (13)
2 2 2
R3,0 K3 + K0 K3

B = (R_squared - K[1:4] + K[0]) / 2

With these elements, Chans equation (equation 7) can now be written as:

   
T = A R R0 + B = A R R0 + A B (14)

By defining two more vectors;

E = A R (15)



F = A B (16)

E = A * R
F = A * B

equation 14 can be written as:

T = E R0 + F (17)

4.1.2 Finding R0


Substituting equation 17 for T in equation 9, and squaring both sides gives:

2 2 2
R02 = (X0 (Ex R0 + Fx )) + (Y0 (Ey R0 + Fy )) + (Z0 (Ez R0 + Fz )) (18)

24
Writing out the squared terms;
 
2
R02 = X02 2X0 (Ex R0 + Fx ) + (Ex R0 + Fx ) +
 
2
Y02 2Y0 (Ey R0 + Fy ) + (Ey R0 + Fy ) +
 
2
Z02 2Z0 (Ez R0 + Fz ) + (Ez R0 + Fz ) (19)
R02 X02 2X0 Ex R0 2X0 Fx + Ex2 R02 + 2Ex Fx R0 + Fx + 2


=
Y02 2Y0 EY R0 2Y0 Fy + Ey2 R02 + 2Ey Fy R0 + Fy2 +

Z02 2Z0 Ez R0 2Z0 Fz + Ez2 R02 + 2Ez Fz R0 + Fz2

(20)

then re-shuffling terms and regrouping;

R02 Ex2 + Ey2 + Ez2 R02 +

=
2 (X0 Ex + Y0 Ey + Z0 Ez ) R0 2 (Ex Fx + Ey Fy + Ez Fz ) R0 +
2 (X0 Fx + Y0 Fy + Z0 Fz ) Fx2 + Fy2 + Fz2 X02 + Y02 + Z02



(21)

leads to a quadratic equation of the form aR02 + bR0 + c = 0, where

1 Ex2 + Ey2 + Ez2

a = (22)
b = 2 (X0 Ex + Y0 Ey + Z0 Ez ) 2 (Ex Fx + Ey Fy + Ez Fz ) (23)
c = 2 (X0 Fx + Y0 Fy + Z0 Fz ) Fx2 + Fy2 + Fz2 X02 + Y02 + Z02



(24)

Note that the last term of c is equal to K0

In vector form and using matrix-multiplication, these become:

T
a = 1 E E (25)



T
b = 2M 0 E 2 E F (26)


T
c = 2M 0 F F F K0 (27)

a = 1 - (E.T * E)
b = 2 * (M[0] * E - F.T * E)
c = 2 * (M[0] * F) - F.T * F - K[0]

At last we can find R0 by solving

b b2 4ac
R0 = (28)
2a

which has 2 possible solutions, at least, if the term b2 4ac (the discriminant) is positive.
Since R0 is supposedly the distance from Tag to Monitor 0, only positive solutions for R0 make sense.


If two positive solutions exist, both values should be used to calculate the Tag position T using Chans
equation (equation 17).

25
 # first test if the term under the sqrt is positive.
# scipy.sqrt() would return a complex number for a negative argument!
discr = b * b - 4 * a * c
poslist = []
if discr >= 0:
h = scipy.sqrt(discr)
for i in (h, -h): # try (h-b)/(2a) and (-h-b)/(2a)
R0 = (i - b) / (2 * a)
if R0 >= 0:
T = E * R0 + F
poslist.append((T.A.squeeze(), R0)) # convert T to a
# row-vector of dim 3
return poslist

At this point, poslist can be an empty list, or a list containing one or two possible Tag-positions T
relative to the Networks origin, together with the associated R0 for that position. When two possible

T s exist, a priori information is usually used to determine which T is most likely to be correct. We


could, for instance, calculate the distance from each possible T to the last known position of the Tag


(i.e. T 1 ).
Note again that the Monitors must not all lie on a plane, or equation 11 will be invalid, and no position
can be calculated. To make optimal use of this algorithm, one of every conceivable group of four Monitors
must lie outside the plane defined by the 3 others. An interesting geometric puzzle.
This clearly puts constraints on the placement of the Monitors; it is immediately obvious that one of the
easiest ways to deploy a Network of Monitors in a space would be to attach them to a convenient ceiling,
lighting-grid or other, similar stuctures, which is bound to place most, if not all, Monitors in a plane. It
would be highly desirable to find an alternative algorithm which could calculate the Tags position from
a planar array of Monitors, and preferably also from more than 4 Monitors.

26
4.2 Chans Alternative Algorithm with 5 or More Monitors

# in class Tag(object):
def doChanForMore(self, D):
M = D[:,:3]
Ga = D[1:] - D[0]
num = len(M)
Q = scipy.matrix((0.5 * scipy.identity(num - 1)) + 0.5)

E = -Ga.T * Q.I
Fi = (E * -Ga).I

R = Ga[:,3]
R_squared = scipy.matrix(R.A * R.A)

K = scipy.zeros((num,1))
for n in range(num):
K[n] = M[n] * M[n].T

h = (R_squared - K[1:] + K[0]) / 2

first_est = (Fi * E * h).A.squeeze()

R0 = first_est[3]

B = scipy.matrix(scipy.identity(num - 1) * (R.A + R0))

Y = B * Q * B

E = -Ga.T * (Y * (self.net.c ** 2)).I

Fi = (E * -Ga).I

second_est = (Fi * E * h).A.squeeze()

return [(second_est[:3], R0)]

4.3 Chans Alternative Algorithm for Monitors in a Plane

# in class Tag(object):
def doChanFlat(self, D, N): # N is the planes normal, calculated earlier
M = D[:,:3]
P = M[1:] - M[0]
P0 = P[0].A.squeeze()

4.3.1 Moving the Plane; Coordinate Transformations

Rot = scipy.array((P0, scipy.cross(P0, N), N)).T

lengths = veclen(Rot)
Rot = scipy.matrix(Rot / lengths)

for m in P:
m *= Rot

27
 R = D[1:,3]
R_squared = scipy.matrix(R.A * R.A)

Gl = scipy.concatenate((P[:,:2], R), 1)
num = len(P)
Q = scipy.matrix((0.5 * scipy.identity(num)) + 0.5)

E = -Gl.T * Q.I
F = E * -Gl

K = scipy.zeros((num,1))
for n in range(num):
K[n] = P[n] * P[n].T

h = (R_squared - K) / 2

XYR0 = (F.I * E * h).A.squeeze()

XY = scipy.matrix(XYR0[:2])
R0 = XYR0[2]

c = (XY * XY.T) - (R0 ** 2)

if (c > 0):
return []

h = scipy.sqrt(-c).item()
pos_est = XY.A.squeeze()

poslist = []
for est in (h, -h):
if est > 0:
continue

T = (scipy.array(pos_est.tolist() + [est]) * Rot.I) + M[0]

poslist.append((T.A.squeeze(), R0))

return poslist

4.4 Choosing the right Algorithm

Between the three algorithms presented in sections 4.1 - 4.3, we can now handle any configuration of 4
or more Monitors. All three algorithms get given the DDoA-data in its matrix form (i.e. the matrix D),


and all three return a list containing 0, 1 or 2 T s. What remains to be done is, given the matrix D, to
determine which of the three algorithms to call.
Since we have three algorithms to choose from, and two relevant selection-criteria (the number of Monitors,
and the geometry of the Monitors), it might be useful to put the various possibilities in a truth table:

Monitor Geometry Volume Plane

Number of Monitors 4 >4 4
Chan for Four
Chan for More
Chan for Plane

Here, an means that that particular algorithm can be used for that particular configuration of Monitors,
and a means the algorithm cannot be used.

28
From this table, it is easy to see that the Monitor geometry should be our first selection-criterion; if
all Monitors lie in a plane, the number of Monitors is not important. Selecting either of the other two
algorithms based on the number of Monitors only needs to happen if the Monitors turn out not to lie in
a plane.

# in class Tag(object):
def checkPlane(self, M):
P = M[1:] - M[0]

i = 1
P1 = P[0].A.squeeze()
while i < len(P):
P2 = P[i].A.squeeze()
N = scipy.cross(P1, P2)
i += 1

if veclen(N).item() > 0:
break

plane_check = ((N * P.T) == 0).A.squeeze()

return (plane_check.all(), N)

# in class Tag(object):
def chooseAlg(self, D, trgpos):
n = len(D)
try:
(plane, normal) = self.checkPlane(D[:,:3])
if plane:
out = self.doChanFlat(D, normal)
else:
if n == 4:
out = self.doChanForFour(D)
else:
out = self.doChanForMore(D)

except scipy.linalg.LinAlgError:
self.printErr("Tag [%d]: Singular Matrix!" % self.id)
return None

29
4.5 Choosing the right Position

# continued inside chooseAlg

if len(out) == 1:
return out[0]

if len(out) > 1:
mindist = 1e100
for i in range(2):
dist = veclen(out[i][0] - trgpos).item()
if dist < mindist:
closest = i
mindist = dist

return out[closest]

self.printErr("Tag [%d]: Position-calculation not possible." % self.id)

return None

4.6 Calculating the Time

Since the temporal resolution of the system presented here is not very good (see section 2.1.4), it is
more than likely that the position-information obtained from this tracking-system will be combined and
correlated with measurements from other sensor-systems. Inertial navigation systems, for instance, can
have very high spatial and temporal resolutions, but lack an absolute frame of reference. Combining the
system presented here with an accelerometer-based system could yield position-data at high speeds and
with a high degree of precision.
In order to correlate the position-measurements from this system with any other system, we need to
determine, as exactly as possible, the point in time for which a position is calculated. Not only do we
want to know where a Tag is located, but also when the Tag was there. In other words; we need to
timetag the calculated positions.

4.6.1 From TOA to System Time

Because of the polling architecture of the system, there is no fixed relationship between the Tags time
of transmission and the time when the Tracking-server receives the TOAs from the polled Monitors.
Fortunately the received TOAs themselves are accurate indications of when each Monitor received the
Tags signal. In fact, each TOA represents the time (in floating-point seconds) elapsed between the last
time the Network was synchronized (all Monitors clocks were reset) and the time the Tags signal arrived
at each monitor. In order to convert a given TOA to a system-time (in floating-point seconds since the
Epoch) all we need to do is add the TOA to the system-time of the last synchronisation:

tarrival = tsync + T OA (29)

(with all times in floating-point seconds)

This gives us the time when the Tags signal arrived, however, we want to know when the Tags signal
was transmitted. In order to find this, we can simply subtract the signals Time-of-Flight, which can be
calculated once R0 is known (see section 4.1.2)

R0
T OF0 = (30)
csnd

30
Combining the two equations above gives:

R0
ttransmission = tsync + T OA0 (31)
csnd

Note that we use T OA0 , the earliest TOA, which is the TOA of the reference-Monitor. Likewise, R0 is
the distance between the Tag and the reference-Monitor. The ttransmission here is the time when the Tag
transmitted its ultrasound packet, expressed in floating-point seconds since the epoch (i.e. in system
time format)

4.6.2 System Latency

By subtracting the time of the Tags transmission from the time when the position has been calculated,
we can calculate the system latency, i.e. the time that elapsed between the Tags transmission, and the
Tags calculated position becoming available:

4t = tpos ttransmission (32)

4.6.3 Synchronizing Clocks

In order to correlate the position-measurements with measurements from any other system, the two
systems to be correlated must share a common frame of reference (all positions must be expressed in
floating-point metres, for example) and also a common time-frame. This means that the system clock of
the Tracking-server must be accurately synchronized with the system clock(s) of whatever other sensor-
system(s) are to be correlated.
The de-facto standard for synchronising the system clocks of distinct systems is NTP, the Network Time
Protocol (see http://www.ntp.org/). At the time of writing, we do not yet know if NTP-synchronization
is accurate enough, especially if one of the systems to be synchronized is mobile and communicates over
wireless-lan. However, the NTP-daemon process does provide the capability of synchronizing a systems
clock to an external reference-clock. The reference-clock in question could for example be a GPS-receiver
(which will not usually work indoors) or a receiver for the DCF77 atomic reference-clock radio-signal
which is broadcast all over Europe. Synchronizing multiple disparate systems to the same reference-clock
source will certainly be accurate enough, but it does require additional hardware.

5 Simulating Tags

6 Multiple Threads

7 OpenSoundControl Communication

31