Localization with time of arrival measurements
Contents
2.6. Localization with time of arrival measurements¶
2.6.1. Introduction¶
Acoustic sensors can be used as a mean to track a moving object. Typically, acoustic measurements are characteristic of the distance between the receiver and an acoustic source. Hence, they convey information on the position of the latter, which can be leveraged for localization. In the next, we will see how gtsam can be used in such a scenario.
The following example considers an omnidirectional sound source that periodically emits an acoustic signal while moving. The signals are received by a fixed array of microphones that can synchronously record the signals from which Time of Arrival (ToA) measurements are extracted. The objective is to recover, from the ToA measurements, the trajectory of the moving agent. In this simple example, no odometry information is used, but odometry factors can seamlessly be integrated.
2.6.2. Example trajectory¶
We start by defining the microphones position and create the ground truth trajectory along with sound events. We first define useful units.
MS = 1e-3 # 1 millisecond in s
CM = 1e-2 # 1 centimeter in m
Next, we istantiate a functor with a value \(v = 330 m.s^{-1}\) for the speed of sound.
TIME_OF_ARRIVAL = TimeOfArrival(330)
We define the positions of four microphones that we constrain to lie in a horizontal plane (i.e with constant \(z\) coordinate).
height = 0.5
microphones = []
microphones.append(Point3(0, 0, height))
microphones.append(Point3(403 * CM, 0, height))
microphones.append(Point3(403 * CM, 403 * CM, height))
microphones.append(Point3(0, 403 * CM, 2 * height))
K = len(microphones)
for i in range(K):
print("mic {} = {}".format(i, microphones[i]))
mic 0 = [0. 0. 0.5]
mic 1 = [4.03 0. 0.5 ]
mic 2 = [4.03 4.03 0.5 ]
mic 3 = [0. 4.03 1. ]
We subsequently create a ground truth trajectory for the moving sound emitter which follows a straight line, and specify the times at which a sound is emitted.
n = 5
groundTruth = []
timeOfEvent = 10
# simulate emitting a sound every second while moving on straight line
for key in range(n):
groundTruth.append(
Event(timeOfEvent, 245 * CM + key * 1.0, 201.5 * CM, (212 - 45) * CM))
timeOfEvent += 1
#for event in groundTruth:
# event.print()
For visualization, we display the agent positions corresponding to an excitation event, along with the microphone array.
fig = plt.figure()
plt.plot([mic[0] for mic in microphones], [mic[1] for mic in microphones], "b.", markersize=10, label="Microphones")
plt.plot([ev.location()[0] for ev in groundTruth], [ev.location()[1] for ev in groundTruth], "s", color="r", markersize=5, linestyle="-", lw=0.5, label="Source positions")
plt.xlabel("x [m]"); plt.ylabel("y [m]")
plt.legend(); fig.show()
We can now simulate the time of arrival measurements based on the distance between each microphone and the moving sound source. To determine the (noise-free) ToA between microphone \(j\) for the \(i\)-th source position, one may simply use the formula:
\[ z_{i, j} = \frac{d_{i, j}}{v}, \]
where \(d_{i, j}\) is the distance between them. Otherwise, the function TIME_OF_ARRIVAL.measure can take care of it.
def simulate_one_toa(microphones, event):
"""Simulate time-of-arrival measurements for a single event."""
return [TIME_OF_ARRIVAL.measure(event, microphones[i])
for i in range(len(microphones))]
simulatedTOA = [simulate_one_toa(microphones, event)
for event in groundTruth]
for key in range(n):
for i in range(K):
print("z_{}{} = {} ms".format(key, i, simulatedTOA[key][i] / MS))
z_00 = 10010.245662478956 ms
z_01 = 10008.531002716338 ms
z_02 = 10008.531002716338 ms
z_03 = 10009.824738271 ms
z_10 = 11012.615535847513 ms
z_11 = 11007.276214441754 ms
z_12 = 11007.276214441754 ms
z_13 = 11012.276140565378 ms
z_20 = 12015.22154271807 ms
z_21 = 12007.174542408371 ms
z_22 = 12007.174542408371 ms
z_23 = 12014.941460610633 ms
z_30 = 13017.961192990088 ms
z_31 = 13008.268633130547 ms
z_32 = 13008.268633130547 ms
z_33 = 13017.724455875657 ms
z_40 = 14020.78169909914 ms
z_41 = 14010.17998044382 ms
z_42 = 14010.17998044382 ms
z_43 = 14020.577436670008 ms
2.6.3. Creation of the nonlinear factor graph¶
As always, we create a factor graph to solve the problem at hand.
graph = NonlinearFactorGraph()
# Create a noise model for the TOA error
model = noiseModel.Isotropic.Sigma(1, 0.5 * MS)
K = len(microphones)
key = 0
for toa in simulatedTOA:
for i in range(K):
factor = TOAFactor(key, microphones[i], toa[i], model)
graph.push_back(factor)
key += 1
print(graph.at(0))
keys = { 0 }
isotropic dim=1 sigma=0.0005
ExpressionFactor with measurement: 10.0102
2.6.4. Optimization and results¶
The optimizer needs to be provided an initial trajectory estimate.
initial_estimate = Values()
zero = Event()
for key in range(n):
TOAFactor.InsertEvent(key, zero, initial_estimate)
print(initial_estimate)
Values with 5 values:
Value 0: (gtsam::Event)
{'time':0, 'location': 0 0 0}
Value 1: (gtsam::Event)
{'time':0, 'location': 0 0 0}
Value 2: (gtsam::Event)
{'time':0, 'location': 0 0 0}
Value 3: (gtsam::Event)
{'time':0, 'location': 0 0 0}
Value 4: (gtsam::Event)
{'time':0, 'location': 0 0 0}
The graph is optimized using the Levenberg-Marquardt optimizer.
params = LevenbergMarquardtParams()
params.setAbsoluteErrorTol(1e-10)
params.setVerbosityLM("SUMMARY")
optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)
result = optimizer.optimize()
print("Final Result:\n", result)
Initial error: 5.84103e+09, values: 5
Final Result:
Values with 5 values:
Value 0: (gtsam::Event)
{'time':10, 'location': 2.45 2.015 1.67}
Value 1: (gtsam::Event)
{'time':11, 'location': 3.45 2.015 1.67}
Value 2: (gtsam::Event)
{'time':12, 'location': 4.45 2.015 1.67}
Value 3: (gtsam::Event)
{'time':13, 'location': 5.45 2.015 1.67}
Value 4: (gtsam::Event)
{'time':14, 'location': 6.45 2.015 1.67}
iter cost cost_change lambda success iter_time
0 5.164106e+04 5.84e+09 1.00e-05 1 1.60e-04
1 9.877895e+04 -4.71e+04 1.00e-06 1 4.97e-05
1 9.877004e+04 -4.71e+04 1.00e-05 1 4.53e-05
1 9.868104e+04 -4.70e+04 1.00e-04 1 4.37e-05
1 9.780158e+04 -4.62e+04 1.00e-03 1 6.07e-05
1 8.987462e+04 -3.82e+04 1.00e-02 1 4.46e-05
1 4.707280e+04 4.57e+03 1.00e-01 1 4.30e-05
2 2.678423e+02 4.68e+04 1.00e-02 1 4.27e-05
3 2.184513e+03 -1.92e+03 1.00e-03 1 4.24e-05
3 2.754496e+00 2.65e+02 1.00e-02 1 4.40e-05
4 3.952703e+03 -3.95e+03 1.00e-03 1 4.45e-05
4 2.626570e+00 1.28e-01 1.00e-02 1 4.28e-05
5 9.504650e+03 -9.50e+03 1.00e-03 1 4.22e-05
5 2.999944e+00 -3.73e-01 1.00e-02 1 4.37e-05
5 2.590684e+00 3.59e-02 1.00e-01 1 4.36e-05
6 3.343468e+00 -7.53e-01 1.00e-02 1 4.24e-05
6 2.575458e+00 1.52e-02 1.00e-01 1 4.27e-05
7 4.061605e+00 -1.49e+00 1.00e-02 1 4.23e-05
7 2.557734e+00 1.77e-02 1.00e-01 1 4.34e-05
8 5.529765e+00 -2.97e+00 1.00e-02 1 4.38e-05
8 2.536729e+00 2.10e-02 1.00e-01 1 4.25e-05
9 8.525429e+00 -5.99e+00 1.00e-02 1 4.27e-05
9 2.511344e+00 2.54e-02 1.00e-01 1 4.25e-05
10 1.420894e+01 -1.17e+01 1.00e-02 1 4.39e-05
10 2.480107e+00 3.12e-02 1.00e-01 1 4.36e-05
11 2.262388e+01 -2.01e+01 1.00e-02 1 4.25e-05
11 2.441486e+00 3.86e-02 1.00e-01 1 4.21e-05
12 2.819573e+01 -2.58e+01 1.00e-02 1 4.35e-05
12 2.395791e+00 4.57e-02 1.00e-01 1 4.40e-05
13 2.161321e+01 -1.92e+01 1.00e-02 1 4.30e-05
13 2.350357e+00 4.54e-02 1.00e-01 1 4.23e-05
14 8.893437e+00 -6.54e+00 1.00e-02 1 4.36e-05
14 2.310615e+00 3.97e-02 1.00e-01 1 4.34e-05
15 3.335472e+00 -1.02e+00 1.00e-02 1 4.39e-05
15 2.245055e+00 6.56e-02 1.00e-01 1 4.27e-05
16 2.705415e+00 -4.60e-01 1.00e-02 1 4.22e-05
16 2.192382e+00 5.27e-02 1.00e-01 1 4.23e-05
17 2.973750e+00 -7.81e-01 1.00e-02 1 4.33e-05
17 2.171846e+00 2.05e-02 1.00e-01 1 4.29e-05
18 3.481307e+00 -1.31e+00 1.00e-02 1 4.25e-05
18 2.155508e+00 1.63e-02 1.00e-01 1 4.20e-05
19 4.232390e+00 -2.08e+00 1.00e-02 1 4.21e-05
19 2.137254e+00 1.83e-02 1.00e-01 1 4.29e-05
20 5.145027e+00 -3.01e+00 1.00e-02 1 4.34e-05
20 2.116200e+00 2.11e-02 1.00e-01 1 4.24e-05
21 5.827907e+00 -3.71e+00 1.00e-02 1 4.22e-05
21 2.092165e+00 2.40e-02 1.00e-01 1 4.35e-05
22 5.640956e+00 -3.55e+00 1.00e-02 1 4.38e-05
22 2.065760e+00 2.64e-02 1.00e-01 1 4.21e-05
23 4.437973e+00 -2.37e+00 1.00e-02 1 4.33e-05
23 2.038763e+00 2.70e-02 1.00e-01 1 4.32e-05
24 3.085438e+00 -1.05e+00 1.00e-02 1 4.38e-05
24 2.013275e+00 2.55e-02 1.00e-01 1 4.24e-05
25 2.379401e+00 -3.66e-01 1.00e-02 1 4.21e-05
25 1.990791e+00 2.25e-02 1.00e-01 1 4.20e-05
26 2.206055e+00 -2.15e-01 1.00e-02 1 4.19e-05
26 1.973857e+00 1.69e-02 1.00e-01 1 4.30e-05
27 2.203392e+00 -2.30e-01 1.00e-02 1 4.43e-05
27 1.962458e+00 1.14e-02 1.00e-01 1 4.25e-05
28 2.213889e+00 -2.51e-01 1.00e-02 1 4.18e-05
28 1.953351e+00 9.11e-03 1.00e-01 1 4.36e-05
29 2.197574e+00 -2.44e-01 1.00e-02 1 4.34e-05
29 1.944720e+00 8.63e-03 1.00e-01 1 4.48e-05
30 2.147265e+00 -2.03e-01 1.00e-02 1 4.22e-05
30 1.936277e+00 8.44e-03 1.00e-01 1 4.22e-05
31 2.074180e+00 -1.38e-01 1.00e-02 1 4.34e-05
31 1.928275e+00 8.00e-03 1.00e-01 1 4.30e-05
32 2.001147e+00 -7.29e-02 1.00e-02 1 4.24e-05
32 1.921109e+00 7.17e-03 1.00e-01 1 4.29e-05
33 1.947307e+00 -2.62e-02 1.00e-02 1 4.35e-05
33 1.915115e+00 5.99e-03 1.00e-01 1 4.24e-05
34 1.917141e+00 -2.03e-03 1.00e-02 1 5.11e-05
34 1.910425e+00 4.69e-03 1.00e-01 1 4.27e-05
35 1.903793e+00 6.63e-03 1.00e-02 1 4.34e-05
36 4.515029e+00 -2.61e+00 1.00e-03 1 4.35e-05
36 1.894638e+00 9.15e-03 1.00e-02 1 4.34e-05
37 2.096397e+00 -2.02e-01 1.00e-03 1 4.37e-05
37 1.883310e+00 1.13e-02 1.00e-02 1 4.29e-05
38 1.869517e+00 1.38e-02 1.00e-03 1 4.26e-05
39 1.865248e+00 4.27e-03 1.00e-04 1 4.33e-05
40 3.422928e+01 -3.24e+01 1.00e-05 1 4.36e-05
40 1.864472e+00 7.75e-04 1.00e-04 1 4.25e-05
41 5.549677e+04 -5.55e+04 1.00e-05 1 4.35e-05
41 1.863038e+00 1.43e-03 1.00e-04 1 4.35e-05
42 1.078480e+06 -1.08e+06 1.00e-05 1 4.27e-05
42 1.944951e+00 -8.19e-02 1.00e-04 1 4.21e-05
42 1.861094e+00 1.94e-03 1.00e-03 1 4.34e-05
43 2.140040e+00 -2.79e-01 1.00e-04 1 4.30e-05
43 1.859993e+00 1.10e-03 1.00e-03 1 4.36e-05
44 3.141813e+00 -1.28e+00 1.00e-04 1 4.35e-05
44 1.858369e+00 1.62e-03 1.00e-03 1 4.26e-05
45 1.473321e+01 -1.29e+01 1.00e-04 1 4.23e-05
45 1.855753e+00 2.62e-03 1.00e-03 1 4.19e-05
46 1.716507e+03 -1.71e+03 1.00e-04 1 4.21e-05
46 1.851031e+00 4.72e-03 1.00e-03 1 4.21e-05
47 5.817719e+04 -5.82e+04 1.00e-04 1 4.37e-05
47 1.844308e+00 6.72e-03 1.00e-03 1 4.35e-05
48 5.798557e+05 -
5.80e+05 1.00e-04 1 4.39e-05
48 2.868843e+00 -1.02e+00 1.00e-03 1 4.29e-05
48 1.831366e+00 1.29e-02 1.00e-02 1 4.24e-05
49 6.112849e+00 -4.28e+00 1.00e-03 1 4.22e-05
49 1.824859e+00 6.51e-03 1.00e-02 1 4.25e-05
50 3.347084e+01 -3.16e+01 1.00e-03 1 4.22e-05
50 1.814935e+00 9.92e-03 1.00e-02 1 4.34e-05
51 7.356522e+02 -7.34e+02 1.00e-03 1 4.40e-05
51 1.798165e+00 1.68e-02 1.00e-02 1 4.24e-05
52 1.195310e+04 -1.20e+04 1.00e-03 1 4.25e-05
52 1.770356e+00 2.78e-02 1.00e-02 1 4.24e-05
53 7.431515e+04 -7.43e+04 1.00e-03 1 4.34e-05
53 2.282773e+00 -5.12e-01 1.00e-02 1 4.39e-05
53 1.743914e+00 2.64e-02 1.00e-01 1 4.33e-05
54 3.079642e+00 -1.34e+00 1.00e-02 1 4.32e-05
54 1.730431e+00 1.35e-02 1.00e-01 1 4.22e-05
55 5.462330e+00 -3.73e+00 1.00e-02 1 4.30e-05
55 1.712739e+00 1.77e-02 1.00e-01 1 4.29e-05
56 1.419804e+01 -1.25e+01 1.00e-02 1 4.21e-05
56 1.688403e+00 2.43e-02 1.00e-01 1 4.22e-05
57 5.623867e+01 -5.46e+01 1.00e-02 1 4.18e-05
57 1.652772e+00 3.56e-02 1.00e-01 1 4.32e-05
58 3.089692e+02 -3.07e+02 1.00e-02 1 4.34e-05
58 1.596572e+00 5.62e-02 1.00e-01 1 4.25e-05
59 1.519448e+03 -1.52e+03 1.00e-02 1 4.54e-05
59 1.508797e+00 8.78e-02 1.00e-01 1 4.21e-05
60 3.865269e+03 -3.86e+03 1.00e-02 1 4.31e-05
60 1.683688e+00 -1.75e-01 1.00e-01 1 4.33e-05
60 1.449402e+00 5.94e-02 1.00e+00 1 4.22e-05
61 1.884185e+00 -4.35e-01 1.00e-01 1 4.25e-05
61 1.421809e+00 2.76e-02 1.00e+00 1 4.24e-05
62 2.243353e+00 -8.22e-01 1.00e-01 1 4.33e-05
62 1.389656e+00 3.22e-02 1.00e+00 1 4.18e-05
63 2.883280e+00 -1.49e+00 1.00e-01 1 4.20e-05
63 1.351767e+00 3.79e-02 1.00e+00 1 4.25e-05
64 4.014127e+00 -2.66e+00 1.00e-01 1 4.28e-05
64 1.306576e+00 4.52e-02 1.00e+00 1 4.46e-05
65 5.976171e+00 -4.67e+00 1.00e-01 1 4.50e-05
65 1.252004e+00 5.46e-02 1.00e+00 1 4.18e-05
66 9.237920e+00 -7.99e+00 1.00e-01 1 4.23e-05
66 1.185334e+00 6.67e-02 1.00e+00 1 4.14e-05
67 1.419267e+01 -1.30e+01 1.00e-01 1 4.23e-05
67 1.103185e+00 8.21e-02 1.00e+00 1 4.39e-05
68 2.048249e+01 -1.94e+01 1.00e-01 1 4.36e-05
68 1.001871e+00 1.01e-01 1.00e+00 1 4.29e-05
69 2.594909e+01 -2.49e+01 1.00e-01 1 4.40e-05
69 8.787148e-01 1.23e-01 1.00e+00 1 4.14e-05
70 2.683782e+01 -2.60e+01 1.00e-01 1 4.32e-05
70 7.348708e-01 1.44e-01 1.00e+00 1 4.32e-05
71 2.132277e+01 -2.06e+01 1.00e-01 1 4.26e-05
71 5.782224e-01 1.57e-01 1.00e+00 1 4.13e-05
72 1.273288e+01 -1.22e+01 1.00e-01 1 4.22e-05
72 4.221483e-01 1.56e-01 1.00e+00 1 4.13e-05
73 5.940392e+00 -5.52e+00 1.00e-01 1 4.35e-05
73 2.824307e-01 1.40e-01 1.00e+00 1 4.29e-05
74 2.362726e+00 -2.08e+00 1.00e-01 1 4.51e-05
74 1.750511e-01 1.07e-01 1.00e+00 1 4.26e-05
75 8.786118e-01 -7.04e-01 1.00e-01 1 4.22e-05
75 1.048692e-01 7.02e-02 1.00e+00 1 4.24e-05
76 3.256411e-01 -2.21e-01 1.00e-01 1 4.35e-05
76 6.314866e-02 4.17e-02 1.00e+00 1 4.12e-05
77 1.243516e-01 -6.12e-02 1.00e-01 1 4.33e-05
77 3.883707e-02 2.43e-02 1.00e+00 1 4.21e-05
78 4.962699e-02 -1.08e-02 1.00e-01 1 4.22e-05
78 2.442829e-02 1.44e-02 1.00e+00 1 4.24e-05
79 2.081541e-02 3.61e-03 1.00e-01 1 4.31e-05
80 6.679197e-04 2.01e-02 1.00e-02 1 4.22e-05
81 1.545700e-08 6.68e-04 1.00e-03 1 4.34e-05
82 1.499863e-16 1.55e-08 1.00e-04 1 4.35e-05
83 1.262177e-23 1.50e-16 1.00e-05 1 4.24e-05
The visual representation of the result shows the excelent matching between estimated and ground truth positions.
# parsing the result to retrieve the estimated locations
L = result.__repr__().split('\n'); L = [L[ind] for ind in range(2, len(L), 2)]
L = [l.split('location')[1].split()[1:] for l in L]
estimated_trajectory = [ [float(coord.split('}')[0]) for coord in l] for l in L]
# Display
fig = plt.figure()
plt.plot([mic[0] for mic in microphones], [mic[1] for mic in microphones], "b.", markersize=10, label="Microphones")
plt.plot([ev.location()[0] for ev in groundTruth], [ev.location()[1] for ev in groundTruth], "s", color="r", markersize=5, linestyle="-", lw=0.5, label="Source positions")
plt.plot([x[0] for x in estimated_trajectory], [x[1] for x in estimated_trajectory], "s", color="g", markersize=5, linestyle="-", lw=0.5, label="Estimated positions")
plt.xlabel("x [m]"); plt.ylabel("y [m]")
plt.legend(); fig.show()
