Name 1:
Login: ee16a-
Name 2:
Login: ee16a-
In the past two weeks we introduced the signal processing part for our lab and obtained the TDOA's (Time Difference Of Arrivals) of different beacon signals. This week we are going to explore methods that help us determine the final location.
This lab will build upon the functions you wrote in weeks 1 and 2.
**
Copy the functions from previous labs into the cells below.
**
Alternatively if you have them saved in a .py file you can replace the cell below with the command:
%run [path_to_your_file].py.
This is the same syntax used to import the helper functions above.
%pylab inline
from __future__ import division
import random
import math
# Importing helper functions
%run wk3_code/virtual.py
%run wk3_code/signals.py
%run wk3_code/demod.py
# Week 1 Functions
def cross_correlation(signal1, signal2):
"""Compute the cross_correlation of two given signals
Args:
signal1 (np.array): input signal 1
signal2 (np.array): input signal 2
Returns:
cross_correlation (np.array): cross-correlation of signal1 and signal2
>>> cross_correlation([0, 1, 2, 3, 3, 2, 1, 0], [0, 2, 3, 0])
[0, 0, 3, 8, 13, 15, 12, 7, 2, 0, 0]
>>> cross_correlation([0, 2, 3, 0], [0, 1, 2, 3, 3, 2, 1, 0])
[0, 0, 2, 7, 12, 15, 13, 8, 3, 0, 0]
"""
# YOUR CODE HERE #
def identify_peak(signal):
"""Returns the index of the peak of the given signal.
Args:
signal (np.array): input signal
Returns:
index (int): index of the peak
>>> identify_peak([1, 2, 5, 7, 12, 4, 1, 0])
4
>>> identify_peak([1, 2, 2, 199, 23, 1])
3
"""
# YOUR CODE HERE #
# Week 2 functions
def separate_signal(raw_signal):
"""Separate the beacons by computing the cross correlation of the raw demodulated signal
with the known beacon signals.
Args:
raw_signal (np.array): raw demodulated signal from the microphone composed of multiple beacon signals
Returns (list): each entry should be the cross-correlation of the signal with one beacon
"""
# YOUR CODE HERE #
def average_signal(separated):
beacon_length = len(beacon0)
num_repeat = len(separated) // beacon_length
averaged = np.mean(np.abs(separated[0 : num_repeat * beacon_length]).reshape((num_repeat, beacon_length)), 0)
return averaged
def signal_generator(x, y, noise=0):
raw_signal = add_random_noise(simulate_by_location(x, y), noise)
return demodulate_signal(raw_signal)
def identify_offsets(averaged):
""" Identify the difference in samples between the samples.
The peaks of the signals are shifted to the center.
Args:
averaged (list): a python list in which each entry is a numpy array (e.g. output of average_signal)
Returns: (list) a list corresponding to the offset of each signal in the input
"""
# Reshaping (shifting) the input so that all of our peaks are centered about the peak of beacon0
peak = identify_peak(averaged[0])
shifted = [np.roll(avg, len(averaged[0]) // 2 - peak) for avg in averaged]
##### DO NOT CHANGE THE CODE ABOVE THIS LINE ####
# shifted represents all of the signals shifted so that they are centered about the peak of beacon0
# use shifted to determine the offsets
# consider what the offset for beacon0 should be
# YOUR CODE HERE #
# --Offsets to distances--
def offset_to_time(offsets, sampling_freq):
""" Convert a list of offsets to a list of TDOA's
Args:
offsets (list): list of offests in samples
sampling_freq (int): sampling frequency in Hz
Returns: (list) a list of TDOAs corresponding to the input offsets
"""
# YOUR CODE HERE #
def signal_to_offsets(raw_signal):
""" Compute a list of offsets from the microphone to each speaker.
Args:
raw_signal (np.array): raw demodulated signal from the microphone (e.g. no separation, averaging, etc).
Returns (list): offset for each beacon (beacon0, beacon1, etc). in samples
"""
# YOUR CODE HERE #
def signal_to_distances(raw_signal, t0):
""" Returns a list of distancs from the microphone to each speaker.
Args:
signal (np.array): recorded signal from the microphone
t0 (float): reference time for beacon0 in seconds
Returns (list): distances to each of the speakers (beacon0, beacon1, etc). in meters
"""
# YOUR CODE HERE #
Multilateration is a technique widely used in locationing systems to precisely locate an emitter by measuring the TDOAs from three or more synchronized emitters at one receiver location (for navigation applications) or time difference of arrivals (TDOAs) of the signal from the emitter at three or more synchronized receivers (for surveillance applications). As in the previous labs we will focus on the navigation application modeled after GPS with several speakers emitting beacon signals and a single microphone receiving them.
Suppose we have $n$ emitters $E_0$, $E_1$, ... $E_{n - 1}$, so the position of an emitter $E_m$ in the 2-D plane will be $E_m = (x_m, y_m)$. We also have a receiver $R$ with unknown position $(x, y)$ in the same plane. Let $R_m$ denote the distance of $E_m$ to $R$, $R_m = \sqrt{(x - x_m)^2 + (y - y_m)^2}$.
For simplification, in this lab we set the first emitter $E_0$ at position (0, 0), as a reference. We also let $\tau_1$, $\tau_2$ ... $\tau_{n - 1}$ denote the TDOA's.
Recall from last week that we find the distances from speakers to the microphone with the arrival time of the first beacon $t_0$. However in a real application like GPS finding $t_0$ is impossible. Thus we are unable to get the exact distances from the speakers to the microphone. Instead of obtaining circles as what we got last week, we are only able to get hyperbolas.
Luckily we can still find the relationship of position $R = (x, y)$ and $E_m = (x_m, y_m)$ with some calculations. Let $v_s$ be the speed of sound and $R_0$ be the distance between $R$ and the origin. Since the distance is the speed of sound multiplied by the time the sound travels,
$$R_m - R_0 = v_s \tau_m \qquad \text{and} \qquad R_m = R_0 + v_s \tau_m$$$$(R_m - R_0)(R_m + R_0) = v_s \tau_m (R_m + R_0) $$$${R_m}^2 - {R_0}^2 = v_s \tau_m (R_m + R_0) $$$$\frac{{R_m}^2 - {R_0}^2}{v_s \tau_m} = R_m + R_0 = R_0 + v_s \tau_m + R_0 = 2R_0 + v_s \tau_m $$$$v_s\tau_m = \frac{{R_m}^2 - {R_0}^2}{v_s\tau_m} - 2R_0 $$Since $R = \sqrt{x^2 + y^2}$ involves a square root we wish to get rid of it. The equation above is valid for all values of $m$, so sacrificing $E_1$ as a reference:
$$ v_s\tau_m - v_s\tau_1 = \frac{{R_m}^2 - {R_0}^2}{v_s\tau_m} - 2R_0 - \frac{{R_1}^2 - {R_0}^2}{v_s\tau_1} + 2R_0 = \frac{{R_m}^2 - {R_0}^2}{v_s\tau_m} - \frac{{R_1}^2 - {R_0}^2}{v_s\tau_1}$$Replace $R_m$ with $\sqrt{(x - x_m)^2 + (y - y_m)^2}$ and $R_0$ with $\sqrt{x^2 + y^2}$.
$$ v_s\tau_m - v_s\tau_1 = \frac{(x-x_m)^2 + (y-y_m)^2 - x^2 - y^2}{v_s\tau_m} - \frac{(x-x_1)^2 + (y-y_1)^2 - x^2 - y^2}{v_s\tau_1}$$$$ v_s\tau_m - v_s\tau_1 = \frac{-2x_mx + {x_m}^2 -2y_my + {y_m}^2}{v_s\tau_m} - \frac{-2x_1x + {x_1}^2 -2y_1y + {y_1}^2}{v_s\tau_1}$$$$ v_s\tau_m - v_s\tau_1 = x \left(\frac{-2x_m}{v_s \tau_m} - \frac{-2x_1}{v_s \tau_1}\right) + y\left(\frac{-2y_m}{v_s \tau_m} - \frac{-2y_1}{v_s \tau_1}\right) + \frac{{x_m}^2 + {y_m}^2}{v_s \tau_m} - \frac{{x_1}^2 + {y_1}^2}{v_s \tau_1}$$The final equation with respect to $E_m$ shall appear to be ($m$ ranges from 2 to $n-1$):
$$ (\frac{2 x_m}{v_s\tau_m}-\frac{2 x_1}{v_s\tau_1})x + (\frac{2 y_m}{v_s\tau_m}-\frac{2 y_1}{v_s\tau_1})y = (\frac{{x_m}^2 + {y_m}^2}{v_s\tau_m} - \frac{{x_1}^2 + {y_1}^2}{v_s\tau_1}) - (v_s\tau_m - v_s\tau_1)$$The result is a linear equation with our position as variables.
Note we need at least two equations to find the position of $R$, and we sacrificed 2 speakers to find the linear relationship. We must have no less than 4 speakers to keep the lab running.
Suppose we have four speakers located at (0, 0), (5, 0), (0, 5), (5, 5), respectively. We will simulate the case where the microphone is located at (1.2, 3.6). Run the following block.
# Assume we already know the time of arrival of the first beacon, which is 0.011151468430462413.
received_signal = get_signal_virtual(x=1.2, y=3.6)
demod = demodulate_signal(received_signal)
distances = signal_to_distances(demod, 0.011151468430462413)
distances = distances[:4]
print("The distances are: " + str(distances))
# Plot the speakers
plt.figure(figsize=(8,4))
plt.scatter([0, 5, 0], [0, 0, 5], marker='x', color='b', label='Speakers')
# Plot the circles to find the position as if we know t0.
d0 = plt.Circle((0, 0), distances[0],facecolor='none', ec='r')
d1 = plt.Circle((5, 0), distances[1],facecolor='none', ec='r')
d2 = plt.Circle((0, 5), distances[2],facecolor='none', ec='r')
d3 = plt.Circle((5, 5), distances[3],facecolor='none', ec='r')
fig = plt.gcf()
fig.gca().add_artist(d0)
fig.gca().add_artist(d1)
fig.gca().add_artist(d2)
fig.gca().add_artist(d3)
# Plot the linear relationship of the microphone and speakers.
TDOA = offset_to_time(signal_to_offsets(demod), sampling_rate)
helper = lambda i: float(speakers[i][0]**2+speakers[i][1]**2)/(v*TDOA[i])-float(speakers[1][0]**2+speakers[1][1]**2)/(v*TDOA[1])
helperx = lambda i: float(speakers[i][0]*2)/(v*TDOA[i])-float(speakers[1][0]*2)/(v*TDOA[1])
helpery = lambda i: float(speakers[i][1]*2)/(v*TDOA[i])-float(speakers[1][1]*2)/(v*TDOA[1])
x = np.linspace(-9, 9, 1000)
y2 = [((helper(2)-helper(1))-v*(TDOA[2]-TDOA[1])-helperx(2)*xi)/helpery(2) for xi in x]
y3 = [((helper(3)-helper(1))-v*(TDOA[3]-TDOA[1])-helperx(3)*xi)/helpery(3) for xi in x]
# You can calculate and plot the equations for the other 2 speakers here.
plt.plot(x, y2, label='Equation 2', color='g')
plt.plot(x, y3, label='Equation 3', color='c')
plt.xlim(-9, 18)
plt.ylim(-6, 6)
plt.legend()
plt.show()
The curves intersect at a single point which corresponds to the location of the microphone.
Reference (and more reading!): http://en.wikipedia.org/wiki/Multilateration
Once we find the equations for each speaker and the microphone, we are able to construct a linear equations systems.
** Is this system overdetermined or underdetermined? **
As we see in the above example, the microphone's position lies on the intersection of the curves. Finding the position of the microphone is equivalent to finding the solution for the linear system.
Write the function below that sets up the system of equations.
def construct_system(TDOA):
"""Construct the components of the system according to a list of TDOA's
Args:
TDOA (np.array): an array of TDOA's
Returns:
A (np.matrix): the matrix corresponding to the least squares system
b (np.array): the vector corresponding to the least squares system
Hint: see how the cell above uses the functions 'helperx', 'helpery' and 'helper'
"""
A, b = [], []
# YOUR CODE HERE
Take a look at your results and make sure it works correctly. How are we testing this function?
A, b = construct_system(TDOA)
for i in range(len(b)):
print "Row %d: %.f should equal %.f"%(i, A[i][0] * 1.2 + A[i][1] * 3.6, b[i])
Definition: If $A$ is an $m \times n$ matrix and $b$ is in $\mathbb{R}^m$, a least-squares solution of $Ax=b$ is an $\hat{x}$ in $\mathbb{R}^n$ such that for all $x$ in $\mathbb{R}^n$: $||b - A\hat{x}|| \leq ||b - Ax||$.
The solution for an overdetermined problem is given by solving the normal equations: $A^TAx=A^Tb$.
Why do we need least-squares here?
During the transmission of sound in air, some noise is added into the signal. Most of the time we don't receive the original signal perfectly; in other words, the linear system is no longer consistant due to the modified signal. Also in our locationing system, we have more than 2 linear equations to improve the accuracy. However with more equations, the linear system is more likely to be inconsistent. Least-squares solution ensures a best approximation we can get, even if there is technically no solution to the system.
# We added some noise in our virtual signal.
received_signal = get_signal_virtual(x=1.2, y=3.6, intensity=4)
demod = demodulate_signal(received_signal)
TDOA = offset_to_time(signal_to_offsets(demod), sampling_rate)
# ...then we plot the signal.
A, b = construct_system(TDOA)
x = np.linspace(-9, 9, 1000)
for i in range(len(b)):
y = [(b[i] - A[i][0]*xi) / A[i][1] for xi in x]
plt.plot(x, y, label="Equation" + str(i + 2))
plt.xlim(-9, 9)
plt.ylim(-6, 6)
plt.legend()
plt.show()
Implement the following function given arguments matrix A and vector b. You may implement your own function of solving least-squares or check the documentation of function numpy.linalg.lstsq() and understand its usage.
def least_squares(A, b):
"""Solve the least squares problem
Args:
A (np.matrix): the matrix in the least squares problem
b (np.array): the vector in the least squares problem
Returns:
pos (np.array): the result of the least squares problem (x)
"""
# YOUR CODE HERE
Run the following tests to make sure your least squares estimate works.
def get_signal_actual(mode, x, y, intensity):
"""Get the signal from the microphone"""
return mic.new_data()
# Define a dictionary to allow us to sqitch between the
get_signal = {'actual': get_signal_actual, 'virtual': get_signal_virtual}
# Define a helper function to use least squares to calculate location from just the TDOAs
def calculate_location(TDOA):
return least_squares(*construct_system(TDOA))
# Define a testing function
def test_loc(x_pos, y_pos, inten, src, debug=False):
raw_signal = get_signal[src](mode="Location", x=x_pos, y=y_pos, intensity=inten)
# Demodulate raw signal
demod = demodulate_signal(raw_signal)
# Separate the beacon signals
separated = separate_signal(demod)
# Perform our averaging function
avg = [average_signal(s) for s in separated]
# Calculate offsets and TDOAs
offsets = identify_offsets(avg)
TDOA = offset_to_time(offsets, sampling_rate)
# Contstruct system of equations
A, b = construct_system(TDOA)
# Calculate least squares solution
pos = calculate_location(TDOA)
if debug:
# Plot the averaged output for each beacon
plt.figure(figsize=(12,6))
for i in range(len(avg)):
plt.subplot(3,2,i+1)
plt.plot(avg[i])
plt.title("Beacon %d"%i)
plt.tight_layout()
# Plot the averaged output for each beacon centered about beacon0
plt.figure(figsize=(16,4))
peak = identify_peak(avg[0])
for i in range(len(avg)):
plt.plot(np.roll(avg[i], len(avg[0]) // 2 - peak), label="{0}".format(i))
plt.title("Beacons Detected")
plt.legend()
plt.show()
print "Offsets (samples): %s"%str(offsets)
print "Times (s): [%s]\n"%", ".join(["%0.6f" % t for t in TDOA])
print "Constructing system..."
print "Verifying system using known position..."
for i in range(len(b)):
print "Row %d: %.f should equal %.f"%(i, A[i][0] * x_pos + A[i][1] * y_pos, b[i])
print "\nCalculating least squares estimate..."
print("Expected: (%.3f, %.3f); got (%.3f, %.3f)\n"%(x_pos, y_pos, pos[0], pos[1]))
# Testing signals without noise.
test_loc(1.2, 3.6, 0, 'virtual', True)
Test your code with noisy inputs. Are all of the estimates in the cases with noise reasonble? Why or why not?
Note: The code below may give you "wrong" estimates that don't match the actual position. The point of this section is to figure out why that might be the case.
# Testing signals with noise
test_loc(1.2, 3.6, 100, 'virtual')
test_loc(1.2, 3.6, 111, 'virtual')
test_loc(1.2, 3.6, 120, 'virtual')
Save your notebook file onto the M or U drives and move to the testing station and load your code on the testing computer.
%run wk3_code/rec.py
def get_signal():
"""Get the signal from the microphone"""
return mic.new_data()
Begin with all of the speakers equidistant from the microphone (on the tape marks). Record the signal and make sure it looks reasonable, if not keep recording until it does.
raw_signal = get_signal()
plt.figure(figsize=(16,4))
plt.plot(raw_signal)
Make sure you see a well defined peak in the correlation of each beacon. If you don't see a well defined peak for one, remove it from the list of averages before moving on.
# Demodulate raw signal
demod = demodulate_signal(raw_signal)
# Separate the beacon signals
separated = separate_signal(demod)
# Perform our averaging function
avg = [average_signal(s) for s in separated]
# Plot the averaged output for each beacon
plt.figure(figsize=(12,6))
for i in range(len(avg)):
plt.subplot(3,2,i+1)
plt.plot(avg[i])
plt.title("Beacon %d"%i)
plt.tight_layout()
Run the code below to calculate the distance to the speakers. If speakers on the tape marks are exactly 127cm from the microphone, does this make sense?
# Plot the averaged output for each beacon centered about beacon0
plt.figure(figsize=(16,4))
peak = identify_peak(avg[0])
for i in range(len(avg)):
plt.plot(np.roll(avg[i], len(avg[0]) // 2 - peak), label="{0}".format(i))
plt.title("Beacons Detected")
plt.legend()
plt.show()
offsets = identify_offsets(avg)
TDOA = offset_to_time(offsets, sampling_rate)
print "Offsets (samples): %s"%str(offsets)
print "Times (s): [%s]\n"%", ".join(["%0.6f" % t for t in TDOA])
distances = signal_to_distances(demod, 0.00373211084)
print "Distances (m): [%s]\n"%", ".join(["%0.4f" % d for d in distances])
Move one of the speakers either closer to the microphone or farther away and repeat the same steps above. Does this match what you expect?
raw_signal = get_signal()
plt.figure(figsize=(16,4))
plt.plot(raw_signal)
# Demodulate raw signal
demod = demodulate_signal(raw_signal)
# Separate the beacon signals
separated = separate_signal(demod)
# Perform our averaging function
avg = [average_signal(s) for s in separated]
# Plot the averaged output for each beacon
plt.figure(figsize=(12,6))
for i in range(len(avg)):
plt.subplot(3,2,i+1)
plt.plot(avg[i])
plt.title("Beacon %d"%i)
plt.tight_layout()
# Plot the averaged output for each beacon centered about beacon0
plt.figure(figsize=(16,4))
peak = identify_peak(avg[0])
for i in range(len(avg)):
plt.plot(np.roll(avg[i], len(avg[0]) // 2 - peak), label="{0}".format(i))
plt.title("Beacons Detected")
plt.legend()
plt.show()
offsets = identify_offsets(avg)
TDOA = offset_to_time(offsets, sampling_rate)
print "Offsets (samples): %s"%str(offsets)
print "Times (s): [%s]\n"%", ".join(["%0.6f" % t for t in TDOA])
distances = signal_to_distances(demod, 0.00373211084)
print "Distances (m): [%s]\n"%", ".join(["%0.4f" % d for d in distances])