An indoor positioning system relying on time difference of arrival measurements of ultrasonic pings from fixed transmitters.
Code available at https://github.com/YingVictor/ultrasonic-positioning
\documentclass[12pt]{article}
% Use reasonably small margins
\usepackage{fullpage}
% for various math utilities
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{commath}
\usepackage{cancel}
\usepackage{breqn}
% provides non-italicized Greek letters in text
\usepackage[euler]{textgreek}
% provides the \includegraphics command
\usepackage{graphicx}
\usepackage{subfig}
% allows figure environments to be placed in exact locations
\usepackage{float}
% Bold caption labels, and give captions increased margins
\usepackage[labelfont=bf, margin=1cm]{caption}
% allows for configurable source code listings
\usepackage{listings}
\usepackage{color}
\definecolor{dkgreen}{rgb}{0,0.6,0}
\definecolor{gray}{rgb}{0.5,0.5,0.5}
\definecolor{red}{rgb}{0.8,0,0}
\lstset{frame=tb,
language=C,
aboveskip=3mm,
belowskip=3mm,
showstringspaces=false,
columns=flexible,
basicstyle={\small\ttfamily},
numbers=left, % where to put the line-numbers; possible values are (none, left, right)
numbersep=5pt, % how far the line-numbers are from the code
numberstyle=\tiny\color{gray}, % the style that is used for the line-numbers
stepnumber=1, % the step between two line-numbers. If it's 1, each line will be numbered
keywordstyle=\color{blue},
commentstyle=\color{dkgreen},
stringstyle=\color{red},
breaklines=true,
breakatwhitespace=true,
breakindent=50pt,
tabsize=4
}
% allows hyperlinks
\usepackage{hyperref}
\DeclareMathOperator*{\argmin}{arg\,min\:}
\begin{document}
\begin{titlepage}
\begin{center}
\textsc{\LARGE ELE 302: Building Real Systems} \\[0.5cm]
\emph{ Professors: Andrew A. Houck and Antoine Kahn } \\[5cm]
{\huge \bfseries Independent Project: \\ Ultrasonic Positioning System} \\[3cm]
{\LARGE Michael Danielczuk, Andrew Kim, Monica Lu, and Victor Ying}
\vfill
{\LARGE Due Friday, May 15, 2015}
\end{center}
\end{titlepage}
\newpage
\tableofcontents
\vfill
\newpage
%--------------------------------------------------
% INTRODUCTION TO POSITIONING
%--------------------------------------------------
\section{Introduction}
%TODO: Write initial part of introduction?
Our indoor positioning system works by the same principle as a satellite navigation system, with ultrasonic pings substituted for radio waves. Signals are transmitted from stations at known locations, and signals traveling different distances to a given receiver will take different amounts of time to get there. With enough information about distances to known locations, the receiver's position can be calculated. As the speed of sound at room temperature is a mere 1126 feet per second, it is easy for general-purpose microcontrollers to measure the timing of ultrasonic signals to a precision that would enable spatial resolution of better than one foot. Our 25 kHz ultrasound has a wavelength of about half an inch, so if our receive circuitry allows for timing of ultrasonic signals to within several cycles, we might expect to achieve spatial resolution of a few inches. In fact, this is exactly what we observe.
The next three sections of this report document the design of the positioning system at three levels of abstraction. First, at the highest level of abstraction, there is Section~\ref{multilateration}, which explains the principles and mathematics of positioning used, assuming the ability to measure differences in time of flight. The other two sections concern the hardware and software systems needed to obtain these measurements. Section~\ref{hardware} discusses the physical hardware systems we had to build, while Section~\ref{signals} discusses the way the various components of the system communicate and the signals and processing that are needed to get accurate measurements of differences in times of flight.
%--------------------------------------------------
% THEORY AND MATH OF POSITIONING
%--------------------------------------------------
\section{Multilateration}
\label{multilateration}
A positioning system generally either relies upon the ability to either measure different distances to known points (multilateration), or different angles to known points (multiangulation). Multiangulation could be done by use of cameras or other angle-resolved sensors, but using cameras located on mobile robots such as our cars would be difficult as each car would need to process probably several different video feeds covering a 360 field of view and correctly identify beacons at known locations, and transform the apparent position of beacons in the video frames to the angles in which the beacons are oriented. Additionally, putting a bunch of cameras on every robot would be somewhat expensive. If doing multiangulation, it may make more sense to have a fixed number of cameras at known locations processing the angles it sees cars at. Unfortunately, this requires the cameras to actively track and communicate with each and every robot, which does not scale well to many independent robots.
Multilateration can be done by measuring the varying time of flight of signals between receivers on the robots to be located and transmitter stations at known locations. The benefit of this approach is that each receiver does not have to communicate any information to any transmitter or any other receiver. If the transmitters are transmitting a known signal, then the receiver can simply time the signals it receives to get information about the time of flight of the signals.
\subsection{Time difference of arrival multilateration}
It might seem easy to measure distances simply by measuring the time from when a transmitter's microcontroller starts trying to transmit and the time the receiver circuitry sends a detectable signal to a microcontroller on the car. Unfortunately, there are sources of delay that would contribute to this time measurement other than the time it takes for the ultrasound to propagate through the air. The transmitting and receiving ultrasonic transducers act as mechanical band-pass filters, as they are built to physically resonate at 25 kHz. They, together with the more significant band-pass filter in the receiver circuitry, will add some delay to the signal. We could carry out experiments to measure and correct for this delay, but with the receive circuitry in particular, this would require either being careful with tolerances to ensure each receive amplification and filter circuit is similar enough to the next, or individually characterizing and calibrating the correction term for different receivers. It would also mean every receiver would need to be synchronized with every transmitter to measure the time of transmission and time of receiving each signal on a shared clock, and such synchronization generally requires two-way communication between the transmitters and receivers, or one-way communication with known (or very small) latencies, which would impact the ability of the system to scale as more receivers are added.
In the approach used in our system, as well as in satellite navigation systems, there is a small fixed number of transmitter stations that are synchronized with each other and send out signals at fixed known times relative to each other, but the receivers do not communicate with the transmitter stations to synchronize their clocks. Knowing that the transmitters are broadcasting certain signals at certain times, the receiver can simply observe the time difference at which it sees signals from different transmitters to get a difference in the lengths of the signals flights. Since all the transmitters have identical hardware and are sending out the same sort of signal, and these signals are received through the same receiver hardware for each receiver, any difference in the signals' times of flight must be due to differences in times of propagation of the ultrasound through the air. This has the advantage that the receiver does not need to communicate with the transmitter stations in any form to synchronize its clock with their clocks. In general, calculating position using a time difference of arrival measurements requires one more measurement (i.e., one more transmitter) than using absolute time of flight measurements. However, adding one more transmitter station that is identical to the rest requires no additional hardware design, so it is a small fixed cost to pay for making the positioning system more scalable, as, without the need to synchronize receivers, now there can be any number of receivers which do not need to interact with each other or the transmitters to calculate their individual positions.
\subsection{Position calculations}
The problem of multilateration based on time differences of arrival was explored and solved in the context of ``sound ranging'', a practice used beginning in World War I to locate enemy artillery by comparing the time at which the sound of the enemy guns firing reached sound recording devices placed at known locations along the front. %TODO: find citation for sound ranging?
In sound ranging, the enemy guns at an unknown position were the transmitters of the sound signal. For our case, the transmitter stations are at known locations and the receiver's position is what we wish to know, but the ideas in the math are the same.
We place the transmitters in a rectangle of known width $X$ and length $Y$. The transmitters are arbitrarily numbered zero through three going counterclockwise, starting in the third quadrant. The origin of our coordinate system is arbitrarily placed below the center of this rectangle along the $z$-axis, at the height of the car receiver. The receiver is at a point $(x,y,0)$. Let $Z$ be the height of the transmitter plane above the height of the receiver; this is known because the car travels only on the flat floor. $x$ and $y$ are the unknowns we wish to calculate.
\begin{figure}[ht!]
\centering
\includegraphics[width=0.8\textwidth]{positioning.png}
\caption{Our coordinate system.}
\label{fig:positioning}
\end{figure}
We can write the measurements the receiver takes as being the difference between each distance to transmitters one through three and the distance to transmitter zero:
\begin{align*}
\Delta_1 = D_1 - D_0 &= \sqrt{\left(x-\frac{X}{2}\right)^2 + \left(y+\frac{Y}{2}\right)^2 + z^2} - \sqrt{\left(x+\frac{X}{2}\right)^2 + \left(y+\frac{Y}{2}\right)^2 + z^2} \\
\Delta_2 = D_2 - D_0 &= \sqrt{\left(x-\frac{X}{2}\right)^2 + \left(y-\frac{Y}{2}\right)^2 + z^2} - \sqrt{\left(x+\frac{X}{2}\right)^2 + \left(y+\frac{Y}{2}\right)^2 + z^2} \\
\Delta_3 = D_3 - D_0 &= \sqrt{\left(x+\frac{X}{2}\right)^2 + \left(y-\frac{Y}{2}\right)^2 + z^2} - \sqrt{\left(x+\frac{X}{2}\right)^2 + \left(y+\frac{Y}{2}\right)^2 + z^2}
\end{align*}
Given that we can measure $\Delta_1$, $\Delta_2$, and $\Delta_3$, we wish to solve these three equations for the two unknowns $x$ and $y$. In the next two subsections, we will discuss two approaches to this calculation that we tried.
A Mathematica notebook file showing the derivations of all the formulas discussed below can be found at \url{https://github.com/YingVictor/ultrasonic-positioning/raw/master/MultilaterationDerivationMathematica.nb}. The exact implementation of these calculations in software can be found in the file position.c located in appendix section~\ref{position.c}.
\subsubsection{A closed form solution?}
The problem is overdetermined, but if we treat $Z$ as an additional unknown, it turns out this problem has a closed-form solution that can easily be found using computer algebra software such as Mathematica:
\begin{dgroup*}
\begin{dmath*}
x = \frac{\Delta_1(\Delta_2 - \Delta_3)(\Delta_1-\Delta_2-\Delta_3)}{2X(\Delta_1-\Delta_2+\Delta_3)}
\end{dmath*}
\begin{dmath*}
y = \frac{\Delta_3(\Delta_2 - \Delta_1)(\Delta_3-\Delta_2-\Delta_1)}{2Y(\Delta_1-\Delta_2+\Delta_3)}
\end{dmath*}
\begin{dmath*}
Z = \pm \frac{1}{2X Y(\Delta_1-\Delta_2+\Delta_3)} \left[
X^2Y^2 \left(\Delta_1^2+(\Delta_2-\Delta_3)^2\right) \left(\Delta_3^2 + (\Delta_2-\Delta_1)^2\right)
- X^2 \left( \Delta_3^2 (\Delta_1-\Delta_2)^2 (\Delta_1+\Delta_2-\Delta_3)^2 + X^2Y^2 (\Delta_1-\Delta_2+\Delta_3)^2 \right)
- Y^2\left(\Delta_1^2 (\Delta_3-\Delta_2)^2 (\Delta_3+\Delta_2-\Delta_1)^2 + X^2Y^2 (\Delta_1-\Delta_2+\Delta_3)^2 \right)
\right]^{\frac{1}{2}}
\end{dmath*}
\end{dgroup*}
This geometry gives fairly simple closed form solutions for $x$ and $y$, and we can throw away the calculation for $Z$. The solution can be calculated directly except for the locations where the denominator tends to zero. This only occurs when $\Delta_1-\Delta_2+\Delta_3 \to 0$. Substituting in the definitions of $\Delta_1$, $\Delta_2$, and $\Delta_3$ and simplifying, it can be demonstrated that this only occurs when $x \to 0$ or $y \to 0$, which, intuitively, is when the receiver is equidistant from some of the transmitters. When calculating our position we can first check for these two cases and change the calculation appropriately. Specifically, if $\Delta_1 \approx 0$, then we can estimate our position as
\begin{align*}
x =& 0 \\
y =& \frac{\Delta_3\Delta_2}{2Y} \\
Z =& \pm \frac{1}{2Y} \sqrt{\left(\Delta_3^2 - Y^2 \right)\left(Y^2 - \Delta_2^2\right) - X^2Y^2 }
\end{align*}
And if $\Delta_3 \approx 0$, then we can estimate our position as
\begin{align*}
x =& \frac{\Delta_1\Delta_2}{2X} \\
y =& 0 \\
Z =& \pm \frac{1}{2X} \sqrt{\left(\Delta_1^2 - X^2\right)\left(X^2- \Delta_2^2\right) - X^2Y^2}
\end{align*}
This calculation turns out to work decently. Unfortunately, by treating $Z$ as an unknown, we are throwing away information rather than making use of all the information we have to get a better approximation. We could actually calculate the value of $Z$ and compare it against the known value as a sort of sanity check to help throw out bad measurements, but the calculation for $Z$ is sufficiently complex that it seemed not worth the trouble. This particular solution tends to magnify errors in measurement, particularly near the $x$- and $y$-axes where certain factors in both the numerators and denominators become very small and measurement errors can change these factors substantially. Using the approach just described where near the $x$- and $y$-axes we switch to a different approximation, the positioning calculation becomes discontinuous, so the errors one sees are not always locally consistent as one moves around in a small area. The entire problem of position calculation could be approached in other ways that do not give closed-form solutions but require iterative numerical approximation methods.
\subsubsection{A least squares regression}
\label{regression}
In an overdetermined problem, a common approach to finding an approximate solution is to minimize some metric that is the sum of some squares of errors. Actual GPS receivers do this, and, with some encouragement from Prof. Houck, we ended up doing this as well. In particular, the metric we minimized was
\[
f(x,y) = \left(\Delta_1 - \hat\Delta_1(x,y)\right)^2 + \left(\Delta_2 - \hat\Delta_2(x,y)\right)^2 + \left(\Delta_3 - \hat\Delta_3(x,y)\right)^2
\]
and so the problem we were solving was
\[
\argmin_{x,y} f(x,y)
\]
where $\Delta_n$ indicates the actual measured differences described earlier, and $\hat\Delta_n(x,y)$ indicates the expected value for $\Delta_n$ given particular values of $x$ and $y$. If you review how we defined the $\Delta_n$, you may notice that all of these deltas are with respect to $D_0$, the distance to transmitter zero located at $(-\frac{X}{2}, -\frac{Y}{2}, Z)$. This may seem like we have arbitrarily chosen to give more weight in the errors to this first distance. In fact this choice seems sensible in the context of how the transmitters synchronize, which is potentially a significant source of errors. As discussed in Section~\ref{txsignals}, TX0 is the master that determines when ultrasonic signals should begin being sent out, and the other transmitters follow its lead.
The method of minimization that we use is essentially Newton's method. Given a guess $\mathbf r = (x, y)$, we calculate the next guess as
\[
\mathbf{r}_{\text{next}} = \mathbf r - \alpha \frac{ f(\mathbf r) \nabla f(\mathbf r)}{\left\|\nabla f(\mathbf r)\right\|^2}
\]
where the factor $\alpha = 0.1$ is used to keep the guess from moving too far too quickly, which can lead to overshoot and failure to converge. The values of $f$ and $\nabla f$ can be calculated exactly at each point $(x,y)$, as $f$ and its partial derivatives have perfectly cromulent closed-form expressions in terms of $x$ and $y$. The expressions are quite large to write out, and so have been left out here, but many terms and factors, such as the expected distances between the four transmitter stations and the point $(x, y)$, appear repeatedly, so they can be calculated once and reused in the calculations in software. The exact calculation can be seen in appendix section~\ref{position.c}.
If at any point $\nabla f(\mathbf r) = \mathbf 0$, that is, if both partial derivatives of $f$ are zero and we have reached a stationary point, we stop the iterative minimization method. Otherwise, we continue iterating to get better approximations for $x$ and $y$ until either 100 iterations have been performed, which is generally more than enough, or the value of the metric $f$ drops below $0.01$ ft$^2$, at which point it is sufficiently small that we don't care to keep performing the iterative minimization.
Minimizing this error metric has the useful benefit that, if we find the value of the metric is very close to zero, we can be quite confident that we are basing our position calculation off of good measurements. Conversely, if the metric is large after the minimization, then we may wish to throw this set of measurements and calculation away, as the measurements themselves may have been suspect. Sources of bad measurements from ultrasonic signals are discussed in Section \ref{performance}. In our experience, when the receiver is stationary, our position calculations always give a consistent position to within a third of a foot, and the value of the error metric at that minimum is generally less than 0.1 ft$^2$. (The value of the metric at the minimum is, in general, likely to be smaller than the square of the actual error because the metric at the minimum is in general appreciably smaller than the metric evaluated at the correct $(x,y)$ values.) In our code, we check that the minimized value of the error metric is less than 0.5 before recording and reporting the new calculated position. If this check fails, we simply throw the current measurement away.
%--------------------------------------------------
% EXTERNAL HARDWARE WE HAD TO PUT TOGETHER
%--------------------------------------------------
\section{Hardware systems}
\label{hardware}
\subsection{Ultrasonics}
We decided to use 25 kHz ultrasonic signals for this project. This frequency is far enough from the typical 40 kHz used by the ultrasonic rangefinders commonly used by hobbyists for us to be confident that we would avoid picking up the rangefinder pings from other robots in the room using ultrasonic rangefinding, which might otherwise interfere with our positioning. We decided to use a lower ultrasonic frequency rather than a higher one because lower frequencies experience less attenuation traveling through the air. A lower frequency and hence a longer wavelength might sacrifice spatial resolution, but it was felt early on in the design process that maximizing the chances that we would be able to receive ultrasonic signals from across the room was a much greater concern. We decided to buy simple ultrasonic transducers so we could design circuits ourselves to maximize the range of our system. To this end, we purchased Prowave 250ST180 and
250SR180 transducers, which are the versions of the 18 mm diameter ultrasonic transducers offered by Prowave, optimized for transmission and reception, respectively. These ultrasonic transducers cost less than \$8 each, but were still relatively expensive compared to other common ultrasonic transducers that might cost only a couple dollars. They were chosen for their relatively high advertised sensitivity and sound pressure level.
\subsubsection{Ultrasonic transmitter stations}
\begin{figure}[ht!]
\centering
\subfloat{\includegraphics[width=0.5\textwidth]{ArduinoXBeeShield.jpg}}
\quad \subfloat{\includegraphics[width=0.4\textwidth]{TransmitterSetup.jpg}}
\caption{XBee radio module mounted to an Arduino, which connects to an ultrasonic transmitter. Note the use of the book-based precision mounting system.}
\label{fig:ArduinoXbee}
\end{figure}
We used an Arduino Uno as a low-cost microcontroller for our transmitter stations, which need to communicate with each other for synchronization purposes and control the Prowave 250ST180 ultrasonic transducers. The Prowave 250ST180 is simply connected directly to two digital output pins of the Arduino, which are capable of outputting 0 or 5 V. The reason we use two output pins rather than, for example, one output pin and ground, is that we can drive the two output pins as the inverse of each other, effectively doubling our amplitude. This is discussed further in section~\ref{txpings}. We observed this improved our reception range a little bit in testing.
\subsubsection{Ultrasonic receivers for cars}
\label{rxcircuit}
\begin{figure}[ht!]
\centering
\includegraphics[width=\textwidth]{rxSchematic.png}
\caption{Schematic of ultrasonic receiver amplification and filtering circuitry.}
\label{fig:rxschematic}
\end{figure}
\begin{figure}[ht!]
\centering
\includegraphics[width=0.5\textwidth]{ReceiverBoard.jpg}
\caption{Photograph of ultrasonic receiver amplification and filtering circuitry.}
\label{fig:rxphoto}
\end{figure}
When receiving ultrasonic signals from across the room, we typically see the Prowave 250SR180 transducer produce signals on the order of a millivolt, and sometimes even lower. To be able to read this signal using a PSoC and to get rid of unwanted noise requires significant amplification and filtering. The circuit we ended up with for this purpose is shown in Figure~\ref{fig:rxschematic}.
We spent a while trying to build our own differential amplifiers with voltage gain on the order of hundreds, using general purpose op-amps. We repeatedly found that the noise was intolerably bad, it was easy to produce unstable circuits when trying to make the gain so large, and, ultimately, when we got past those problems, we rediscovered the limitations of gain-bandwidth products. After all, to give our 25 kHz signal a gain on the order of a thousand would require gain-bandwidth product on the order of 25 MHz! Finally understanding the amplifier chip specifications we needed to be looking for to provide large gain at ultrasonic frequencies, we went of in search of a solution, and found that the LM386 audio amplifier chip was readily available. This chip takes a differential input biased to ground, and produces and output referenced to half the power supply, with a gain that be set between 20 and 200. As it is designed for audio, it works just fine for our 25 kHz signal, which is just barely above the upper limit for human hearing. It works extremely well providing a large first stage of amplification with the gain set to 200.
After the initial amplification, we were seeing a decent amount of both 60 Hz noise and high frequency noise in the signal. we wished to put our signal through band-pass filtering to reduce those low and high frequency noises, as well as provide additional amplification to bring the signal up to the level of whole volts. To do this, we decided to build a Sallen--Key band-pass filter, which is a common topology for an active filter that in general provides non-unity gain, which is desirable in this case. This filter theoretically has a center frequency of 24.7 kHz, a Q factor of about 11, voltage gain of about 32 at the center frequency. We found some TLV2462 dual op-amp chips to use that have a gain-bandwidth of about 6 MHz, which is sufficient. In practice, due to poor tolerances in the components, the Q factor and the gain both tend to be lower than the theoretical value, but the gain in all the versions of this circuit that we built was above an order of magnitude at 25 kHz, and it showed a decent ability to reject of 40 kHz signals, which we might pick up from ultrasonic rangefinders used in other projects. Perhaps more importantly, it did quite a good job filtering out 60 Hz and high frequency noise, producing a fairly clean waveform. As the amplification is relative to a reference voltage of half the power supply, the maximum amplitude we can get in the output is half the power supply, or 2.5 V. The two amplification stages together have a voltage gain of several thousand which is usually enough to get clipping of the received signal. Clipping is not a problem in our context, so this is fine.
\begin{figure}[ht!]
\centering
\includegraphics[width=.5\textwidth]{envelope.jpg}
\caption{Ch1 (Yellow): Output of the Sallen--Key band-pass filter. Ch2 (Blue): Output of DC offset subtractor, after the envelope detector.}
\label{fig:envelope}
\end{figure}
After the amplification and filtering, we have a fairly straightforward envelope detector to demodulate the signal that uses an ordinary silicon diode. After the envelop detector, we subtract off the DC offset in order to get a signal that is referenced to ground. This is essentially achieved using a AC coupling capacitor in the signal line and a pull-down resistor to ground on the output. However, if the DC offset subtraction consisted of only those two components, it would pull the average level of the output to ground, and the output would range from positive to negative voltages, and the voltage of the top and bottom of pulses in the signal would vary depending on the timing and durations of the pulses. To prevent this, a 1N34A germanium diode was added between the output and ground to limit the output so that it cannot drop far below ground. In particular, it cannot drop below ground by more than the turn-on voltage of the diode. A germanium diode was chosen because of its low turn-on voltage. This has the effect of keeping the parts of the signal that are low close to ground, and the pulses from when ultrasonic pings are received are all to positive voltages. A captured waveform demonstrating the function of the envelope detector and DC offset subtractor is shown in Figure~\ref{fig:envelope}.
\subsection{Serial radio communication}
To synchronize all of the transmitters, we needed a system of communication between the transmitters with either very low or very stable latency. A logical wireless communication choice, due to its relative simplicity was the XBee 802.15.4 (series 1) radio module from Digi. In contrast, we could communicate with a transport protocol such as TCP or UDP over an internet connection using WiFi, but with such a heavy protocol stack, it becomes difficult to reason about the sources of latency and to know whether latency is symmetric, and in all likelyhood the latency would be highly variable depending on contention for the WiFi bandwidth available in the room. XBee modules are essentially just modems that translate between RS-232 on a physical wire and the 802.15.4 wireless communication standard. They can be configured for point-to-point communication, but even easier is just to have all the transmitter stations XBee modules on the same channel, so that each character sent by one transmitter station is broadcast and received by all the others. While the configuration tool for XBee modules can be strangly finicky, once the XBees are configured, using them to communicate could not be simpler: they merely need to be provided with 5 V power and ground, and connected with two wires for sending and receiving data. Since we were able to obtain a exclusive channel for use in the lab, and since we are not sending much data between the transmitter stations, there is little contention in our radio communication network, and we do not expect latencies to vary due to data being held in buffers, waiting to be transmitted. As shown in Figure~\ref{fig:ArduinoXbee}, each transmitter station has an Arduino with a shield connecting it to an XBee module. We were able to use the Arduino SoftwareSerial library to communicate between each Arduino via the XBees.
%--------------------------------------------------
% SOFTWARE, SIGNALS, SYNCHRONIZATION, TIMING, ETC.
%--------------------------------------------------
\section{Signals}
\label{signals}
Measuring varying time of flights of a transmitted signal could be done by measuring the phase of a periodic signal, or calculating and finding the maximum of a cross correlation function between the received signal and the signal the transmitter is known to have transmitted, for some arbitrary transmitted signal. GPS works using the latter approach with pseudorandom signals, as this maximizes the time resolution of the time of flight measurements. In our case, we felt it would not be feasible to encode much information in the signals we send out, as there is a fair amount of distortion and echoing as the sound travels through the air, as well as ringing in the transmitter and receiver that would distort the signal. Therefore, we took a much simpler approach: we measure the time of the first rising edge for each ultrasonic ping signal received. This time should correspond to the time it took for the ultrasonic ping to travel the line-of-sight distance between the transmitter station and the receiver. The exact duration of echos and ringing after the first rising edge then does not matter.
\subsection{Transmitted signals and synchronization}
\label{txsignals}
To allow the car to know where it was in the room, we developed a system of four transmitters stations, one in each corner of the room, that send ultrasonic pulses at well-defined intervals. Each of the transmitters is pointed towards the opposite side of the room. This is because the transmitter is directional, that is, the strength of the signal depends on the angle of transmission. In this configuration, the receiver will be able to receive the pings if they are directly underneath the transmitter simply because the signal is stronger close to the transmitter, and if they are further away, the angle will help improve reception. If the last three pulses are sent at known times with respect to the first pulse during each series of pulses, then the time differences of arrival of each pulse at the receiver can be used to determine the car's position. Thus, all of the transmitters need to be synchronized such that they always send an ultrasonic pulse at the same time with respect to the first pulse, sent by the “master” transmitter. All of the code for the transmitter stations is given in Appendix~\ref{speedcode}.
\subsubsection{Synchronization}
While radio waves are fast, XBee communication has latency much higher than the time for the propogation of radio waves. To ensure synchronization, the first step was to determine the latency in serial communication between the master transmitter and each of the slave transmitters. To find the latencies, the master transmitter sends a character (either `a', `b', or `c', depending on which slave transmitter it wishes to communicate with) to the radio channel, then waits to receive the same character in return from the correct slave transmitter. Each slave transmitter is programmed to immediately respond to its own lowercase character (again `a' for the first slave transmitter, `b' for the second, and `c' for the third) by transmitting the same character back to the channel. The master transmitter is then able to calculate the round-trip latency by keeping track of the time to send and receive the character. We found typical round-trip latencies to vary from about 17 ms to 20 ms. We suspect much of this time is due not necessarily to the XBees, but to the software and hardware stack on the Arduino that sends and receives RS-232. The master transmitter tests the round-trip latency for a slave transmitter five times, and divides the total of the round-trip latencies by ten to get an estimate for the one-way latency to each slave transmitter station. It then transmits another character, this time an uppercase letter corresponding to the slave transmitter it wishes to communicate with (`A' for the first slave transmitter, `B' for the second, and `C' for the third). Upon sending the uppercase character, the master transmitter then sends a series of four bytes encoding what it measured the one-way latency for the slave transmitter to be. The slave transmitter then sends back an acknowledgment character (the same uppercase character that it received from the master to start the transaction) to tell the master that it has received all of the bytes. We make the assumption here that the round-trip latency is symmetric --- that the time taken to transmit from master to slave is the same as to transmit from slave to master. Since all the transmitter stations have identical hardware, we find this assumption to be quite reasonable, and we did a few quick tests to confirm the assumption. When repeated for each of the three transmitters (five round-trip latency tests and transmission of the average one-way latency time), this process ensures near-perfect synchronization. Although latencies can occasionally change, we found that they usually stayed relatively constant when the transmitter stations are not being disturbed, meaning the average round-trip latency is not skewed and the one-way latencies are fairly accurate.
\subsubsection{Sending Pings}
\label{txpings}
With the latency measurement and communication process completed, the master transmitter sends out a lowercase `p' over its XBee to the other transmitters to tell them that it is about to send a pulse. It then sends out its ultrasonic pulse, using two pins connected to the actual ultrasonic transmitter via a twisted pair. To gain more control over the ultrasonic pulse output, we used low-level pin manipulation to turn the pins of the Arduino connected to the ultrasonic transmitter on and off. In this way, we were able to simultaneously turn on one pin while turning off another, which cannot be done using the standard digitalWrite() command. The two pins are turned on and off opposite to each other, allowing for twice the voltage range. One pin would be switched on to +5V while the other was switched to ground, and then vice versa. With this manipulation, the differential signal sent to the ultrasonic transducer is a square wave with peak to peak voltage of 10V, double what we would achieve if we tied one pin of the transmitter to ground and only switched the other one. We also have good control over timing using this method, as one very fast register write switches both pins, and can use this control to send the pings at exactly 25 kHz, the optimal rated frequency for the transmitters and receivers that we purchased.
When the master sends out the `p' character followed by its ping, each of the slave transmitters receive the `p' character a few milliseconds later according to their respective latencies. They can then start timing how long it has been since the master transmitter first began transmitting its ping. Once the required time has elapsed (100 ms intervals between each ping), the slave transmitter begins transmitting its ping through the same low-level pin manipulation discussed above. Thus, the first slave transmitter waits 100 ms after receiving the send pulse character from the master minus the known latency time that it has received from the master plus a small amount of processing time due to the fact that the master must first write the `p' character and then send the ping (the actions are not simultaneous on the master end). Each consecutive slave transmitter waits an extra 100 ms more before sending its pulse, again subtracting the known latency time and adding the processing delay. 100 ms was chosen as the delay time between each ping because while the transmission of the ping itself lasts only 5 ms, the echoes created from the signal bouncing around the room cause the received signal to stretch out much more, sometimes up to about 25 ms, and if the receiver is accross the room from one transmitter while very close to another, the signal from the transmitter far away could take as much as 30 ms longer to reach the receiver. We want enough time between pings received such that there is no doubt about the reception of each ping, so 100 ms is a safe period of time between transmissions. The overall result is four 25 kHz ultrasonic pings sent out 100 ms apart.
After one cycle of four pings sent out, the master then waits 300 ms before beginning the next cycle of latency tests and pings. This delay is necessary to allow the receiver to distinguish which ping is the ping sent from the master transmitter and which are the pings sent from each of the slave transmitters: TX0, the master transmitter, always transmits first after a quiet period of at least a few hundred milliseconds. Thus, the total cycle time between when the master sends one of its pings and the next must be greater than around 500 ms assuming the 100 ms between pings so that the receiver can distinguish the master ping based on the longer delay time between the last ping and the master ping as compared to the normal 100 ms time between pings. Since the latency tests also take time, in the final version of our system, the transmitters send out a round of pings every two thirds of a second or so. However, if improvements were to be made to speed up the transmission such as to get more position calculations per second on the receiving end, both the delay between pings and the delay between sets of pings could be reduced. Also important is that the slave transmitters sit idle after transmitting their ping. Thus, the master can begin doing latency testing on the first slave transmitter immediately after it sends its ping but before the second slave transmitter sends its ping, effectively interweaving pinging and latency testing so that latency tests do not impinge on the total timing of the system. In effect, then, the overall frequency of transmission cycles is only limited by the pulse-stretching experienced by the receiver and the time needed to distinguish the master ping, not by latency test times.
\subsubsection{Timeouts and Error Checking}
As a note, we found that bytes are sometimes lost over the course of communicating between devices on the radio channel. This loss can be an issue if it causes the system to hang while, for example, the master looks for an acknowledgment from one of the slaves or one of the bytes containing the latency time is never received by the slave. To combat this issue, we created a timeout structure; if the master is ever waiting for a response from one of the slaves (either an acknowledgment or as part of a latency test), it will automatically timeout after 50 ms. If the timeout occurs while testing for latency, then that test is thrown out and not counted as part of the averaging. If an acknowledgment is not received, the master will simply move on without it. On the slave side, a timeout can also occur when receiving the bytes corresponding the latency from the master. If one of these bytes is lost, the slave simply uses the last known latency time and ignores the corrupted new data. This error checking and timeout structure helps to make the system more robust so that if a transmitter turns off or bytes are lost, the entire system can recover and continue operating without manual resetting.
\subsection{Digital processing of received signals}
\begin{figure}[ht!]
\centering
\includegraphics[width=\textwidth]{timinglogic.png}
\caption{Programmable hardware used to capture timing information for positioning.}
\label{fig:timinglogic}
\end{figure}
\subsubsection{Digital filtering}
\begin{figure}[ht!]
\centering
\includegraphics[width=0.5\textwidth]{ReceivertoComparatorWaveform.jpg}
\caption{Ch1 (Yellow): Output from the receiver circuitry that is fed into the PSoC. Ch2 (Blue): Output of the comparator.}
\label{fig:RxCompareWaveform}
\end{figure}
\begin{figure}[ht!]
\centering
\includegraphics[width=0.5\textwidth]{ComparatortoFilterWaveform.jpg}
\caption{Ch1 (Yellow): Output of the comparator. Ch2 (Blue): Output of the filtering system that is fed into the timer.}
\label{fig:RxFilterWaveform}
\end{figure}
Once the signal has been received, amplified, and filtered using the circuit described in section~\ref{rxcircuit}, we are left with the signal shown as the yellow waveform in Figure \ref{fig:RxCompareWaveform}. To turn this waveform into a series of four clean square pulses so that timing can easily be done requires several pieces of programmable hardware. The schematic detailing the programmable hardware and connections referenced in this section can be found in Figure \ref{fig:timinglogic}.
The first step was to use a comparator to turn the analog input into a series of digital pulses so that the edge time was rigidly defined and voltage levels could be raised even further from about the peak 2V received to 5V. We used a comparator level of 0.7V, high enough to avoid the comparator staying high for various echoes of the pings that had been sent out, but low enough to give accurate timing, since we want to record when the ping first reaches the receiver. This level of 0.7V was generated with an 8-bit voltage digital-to-analog converter. The output after the comparator step is shown as the blue waveform in Figure \ref{fig:RxCompareWaveform}.
As is evident from the waveform representing the output of the comparator, due to the inconsistent nature of the analog waveform being received, further processing is necessary to generate a single pulse corresponding to the reception of each ping. Thus, we have a system of two glitch filters to further refine the signal so that it is suitable for timing. A very useful, but somewhat unknown component, the glitch filter can be used either to eliminate small dips in a high signal or short spikes in the midst of a low signal. The former can be done by setting the glitch filter to bypass high mode such that its output remains high for a set period after seeing a high value while the latter can be achieved in the normal mode, equivalent to bypass low.
The first glitch filter (CompGlitchFilter in Figure \ref{fig:timinglogic}), set to normal mode, directly takes the output of the comparator as input and has a very short period of only 40 \textmu s, which is the period of a 25 kHz signal. This eliminates possible spikes that do not correspond to pings, possibly generated from small amounts of noise. However, it will not eliminate any significant part of the pings, since they always last longer than 40 \textmu s for the initial spike when they are received.
The second glitch filter is made up of two components, an edge detector and a counter (labeled GlitchCounter in Figure \ref{fig:timinglogic}). This glitch filter removes the occasional times when the signal goes low in the middle of the ping signal, as seen in the comparator waveform (yellow) in Figure \ref{fig:RxFilterWaveform}. We made our own glitch filter out of two components here due to a limitation of the Glitch Filter component available in PSoC Creator: it can only maintain a moving window of length at most 256. Thus, with a clock at 1 MHz, its maximum filtering time would be only 0.256 ms, much shorter than a possible drop in the signal, which we have measured at up to 10 ms in length. To allow for a longer filtering time, the clock can be slowed, but this could cause a loss of precision in our timing measurements, since we ideally want a filtering time of about 25 ms (or about twice as long as possible drops in the signal), which would require a clock speed of about 10 kHz. With a 10 kHz clock speed, timing is only accurate to 0.1 ms, corresponding to about one tenth of a foot. That's not so bad at all, but it seems there's no reason that timing should be even close to a limiting factor in measurement precision. To get around this filtering length/measurement precision tradeoff, we created our own glitch filter using an edge detector and a counter. The edge detector outputs pulses on both edges of its input (essentially the output of the comparator). These are used to reset the counter, so the counter is always counting the number of cycles of the 1 MHz clock that have passed since the last edge. The counter outputs a high pulse if its count value is less than 25000 (corresponding to 25 ms with a 1 MHz clock). Thus, the output of the counter goes high as soon as it gets the initial edge and stays high even as the signal drops low for small periods of time. Since the high parts of the signal never last for more than 25 ms, this combination of components effectively imitates the function of a glitch filter. The output of this two-part filter can be seen in Figure \ref{fig:RxFilterWaveform}, where the yellow waveform represents the input to the filtering (the comparator output) and the blue waveform represents the output of the filtering system: four clean pulses.
\subsubsection{Timing}
Now all that is left is timing the positive edges of the filtered received signal. To do this, we use a Timer component, named UltraTimer in Figure~\ref{fig:timinglogic}. Very conveniently, it can hold a queue of four values before triggering an interrupt.
In case some bad input gets past the filter, to make sure the queue is empty when the next round of pings starts, we reset the timer during the intervening quiet period between each set of four pings. To identify that quiet period, we have a counter component named UltraCounter that counts the number of cycles of a 1 MHz clock since the last time an edge was seen in the received signal, much like GlitchCounter does. Unlike GlitchCounter, UltraCounter counts up to a maximum value of 200000, or 200 ms. We use the terminal count output of UltraCounter as a reset signal for UltraTimer, to ensure UltraTimer's queue is emptied at some point during the quiet period, which is more than 200 ms long. 200 ms is longer than the delay between two received pings within a set of four pings should be, so this reset only happens during the reset period before the master transmitter sends out its next ping. Since the UltraCounter is a UDB Counter component, it counts edges of its "count" input, and must be operating at a higher speed clock. To convert the terminal count signal pulse from the faster clock domain to the 1 MHz clock domain used by the timer, a pulse converter component is used.
When UltraTimer sees four positive edges on the filtered received signal and captures those four times, it triggers an interupt handler that reads those captured values and converts the time differences into differences in distances in feet. Before doing so, some sanity checks on the captured values are performed. First, since a timer reset should occur every cycle of four pings, we know something is wrong if the captured timer value indicates it has been a long time since the last reset of the timer. If any captured timer value indicates it was captured more than a second since the previous measurement, something has gone wrong and we throw out the entire measurement. Furthermore, we throw away the entire measurement if any of the differences in distances are much larger than the dimensions of the rectangle of transmitters. Only if the measurements make it through these sanity checks do we carry out the positioning math described in section~\ref{regression}.
\section{Testing and debugging scaffolds}
Before we were doing integration testing of our positioning system, we developed various testing tools to test one part of the system by simulating inputs as if from the rest of the system. Here we will briefly discuss a few such test setups.
First, to test ultrasonic reception and the basic ability to time the difference between two pings, we had tx_test_2. In this test, one Arduino Uno controls two transmitters, and alternately sends out pings from them. We put the transmitters on the ends of long twisted wire pairs, so that we could move them close and further away, and see whether we could measure the time difference between when the pings are received changed. Different receiver circuits could then be tested. The code for this test is included in appendix section \ref{txtest2}.
To test the positioning math given relatively clean reception, we had rx_test, whose code is included in appendix section~\ref{rxtest}. Here, the Arduino sends out a series of four square pulses spaced unevenly, looking a lot like the blue waveform in Figure~\ref{fig:RxFilterWaveform}. A wire would be connected from the Arduino output pin directly to the PSoC, and the Arduino is essentially pretending to be the receiver circuitry, so that there were no actual ultrasonic components involved in this test setup. We could calculate what the timings of the received pulses should be at arbitrarily chosen locations, program the Arduino to send pulses with those timings, and then check whether the PSoC was correctly calculating the position we chose.
Finally, we had a version of rx_test that actually uses ultrasonics, named tx_test_4. The code for tx_test_4, given in appendix section~\ref{txtest4}, would run on an Arduino connected to a single transmitter. The Arduino would send out a series of four pings with certain timings. The actual ultrasonic receiver circuit would be plugged in to the PSoC, and we would see whether the PSoC was correctly calculating the position those timings should indicate. Since all the pings are being sent out a single transmitter, they all experience the same time of flight to the receiver, so the time of flight does not introduce any differences to the relative timings of the received pings. This could be tested with the transmitter on the other side of the room, and essentially the one transmitter is pretending to be four synchronized transmitters at different locations.
We did do some initial testing of radio communication by hooking Arduinos with XBees to separate laptops and communicating accross the room. The XBees always just seemed to work, and are rated to have a range of at least a few hundred feet, which is more than the length of the room. Having seen this, testing of the synchronization of transmitter stations was done simply by bringing all the transmitter stations to a single lab bench and using an oscilloscope to look at the relative timing of the pings they emitted. This did not require any special code. As practically none of the latency we are concerned with arises from the time for propagation of radio signals, we assumed that if the transmitters can correctly synchronize when they are within a few feet of each other, as assume they can still more or less correctly synchronize when they are spread around the room. The fact that positioning worked fairly well when we had the whole system running indicates this assumption was good enough.
\section{Performance and sources of bad data}
\label{performance}
When the car is sitting stationary and has a clear line of sight to all four transmitter stations, the positions it will calculate from repeated measurements will generally vary by a few inches, and a new measurement occurs after each round of pings, which happens once every two thirds of a second or so. However, there are several ways the positioning system can be foiled and produce poor results.
If something is blocking the direct line of sight between the car and a transmitter station, generally the ultrasonic ping from the transmitter station will still reach the car, because ultrasound has no trouble traveling around obstacles. However, this will delay the first rising edge at the receiver for this ping, leading the car to believe it is further from that transmitter station and ruining the positioning calculation.
Sharp noises with appreciable high-frequency content, especially loud clapping, are readily observed to cause the ultrasonic transducers at the receivers to ring briefly as if receiving an ultrasonic ping. This does not appear to be a problem in our receiver circuitry, but rather has to do with the resonance of the physical transducer itself. Usually, such extra pings will fail the sanity checks or the final error metric check, and the whole measurement will be thrown out, so the problem is not that this introduces error into positioning calculations, but that it delays the next new position information. In our testing, one person clapping continuously a couple yards away from the receiver could indefinitely prevent the car from getting a position fix.
If the car is moving, because the signals from the four transmitter stations are received at different points in time, they will also be received when the car receiver is at different points in space. All of our positioning calculations described in Section~\ref{multilateration} assume all measurements pertain to a single unknown location of the receiver, $(x, y, 0)$. When the car is moving, this assumption is false, and as shown in Figure~\ref{fig:mapping}, this can lead to errors that are much larger than when stationary. Intuitively, one might expect errors on the same order as the distance the car travels during a round of pings, which last a few hundred milliseconds. Thus, if the car is traveling a a couple feet per second, one can expect errors on the order of a foot or more.
\section{Applications}
Having built a indoor ultrasonic positioning system, we set out to demonstrate its capabilities.
\subsection{Navigation}
One application of our positioning system would be to create an autonomous navigation system, whereby a car could travel to defined waypoints, either making stops at desired locations or tracing out a desired path. With radio communication, cars could interact and travel routes around each other, avoiding collisions, or following a path previously traveled by another car.
\subsubsection{H-Bridge}
\begin{figure}[ht!]
\centering
\includegraphics[width=0.5\textwidth]{HBridgeBoard.jpg}
\caption{The board of the H-bridge circuit.}
\label{fig:hbridgecircuit}
\end{figure}
To enable the car to navigate robustly around the room, the ability to drive the car in reverse would be helpful, so we added an H-bridge circuit (as seen in Figure \ref{fig:hbridgecircuit}) to our motor control. The H-bridge makes it possible for the PSoC to run the motor in both directions.
\begin{figure}[ht!]
\centering
\includegraphics[width=0.5\textwidth]{HBridgeSchematic.png}
\caption{Schematic of the H-bridge circuit..}
\label{fig:hbridgeschematic}
\end{figure}
The gate of each transistor corresponds to a control signal from the PSoC. Inputs A and B controlling the NMOS's can come directly from the PSoC. Because the PMOS transistors had to deliver 7.2V to the motor and the PSoC only outputs a maximum of 5V, we need to put some hardware between the PSoC output and the gate of the PMOS's to raise the signal voltage to 7.2V. For one car, we chose the LM311P comparator IC to output 7.2V when the PSoC signal is above a reasonable threshold of 3.6V and output 0V when the PSoC signal is under 3.6V. For the other car, we simply ran the PSoC output to simple single-transistor inverters, constructed out of a general-purpose 2N7000 n-type pull down transistor and a 1 k{\textOmega} pull-up resistor.
With such an H-bridge circuit, driving forward can be done by, for example, turning on the PMOS controlled by A' and sending a PWM signal to A. Backwards driving can be done by turning the PMOS controlled by B' and sending a PWM signal to B. Rheostatic braking can also be acheived, for example by simultaneously turning on both NMOS's while leaving the PMOS's off. In the software that controls all this, we ensure any time we're switching between any of these three modes, (forwards, backwards, and braking), all the transistors are first turned off for a millisecond, to guarantee there is no time at which a short circuit path exists. This can be seen in appendix section~\ref{speedcode}.
\subsubsection{General autonomous navigation}
Unfortunately, figuring out how to get the car to navigate to an arbitrary $(x, y)$ position using only the positioning system proved more difficult than initially expected. If we knew the orientation of the car, getting to an arbitrary position seems fairly straightforward. At any given time, take the desired orientation to be the direction between the position of the car and the desired position. If the difference between the current orientation and desired orientation is small, we could simply perform PID or any other form of feedback control to point the car towards the desired destination. If the difference is large, we could code special turning maneuvers for the car to rotate itself around.
Unfortunately, the only way we had to figure out the orientation of the car in general is to make some movement larger than the effective spatial resolution of our positioning system, and look at the change in position. After identifying an initial orientation, in principle one could keep updating an estimate of orientation by a combination of odometry and continuing to look at the direction in which the most recent positions have moved. Unfortunately, we started investigating this relatively late, and ran out of time before coming up with a working solution. The noise in our positioning is such that merely taking our orientation to be the direction of the change between the two most recent positions is far too noisy. In general, it seems estimating orientation based on a fixed-length history of recent positions might work well for some speeds, but would do poorly if the speed of the car varied, and a special approach would have to be taken when the car comes to a stop. It seems possible in principle to come up with a good algorithm to look at a dynamically adjusting number of previous points and determining the recent trend in change direction, but we have not found this algorithm. As for odometry, we have the Hall effect sensor to tell us about our speed, and we always know something about the current steering angle as we have control over that. Unfortunately, it seems that steering is sufficiently nonlinear and/or the Hall effect sensor doesn't provide enough resolution that orientation could be tracked accurately through turns. Using inertial sensing (i.e., a gyroscope) would probably help a lot.
\subsubsection{Simpler navigation demonstration}
Of course, to demonstrate the ability to use the positioning system to navigate, it is not truly necessary to have the car autonomously navigate itself to an arbitrary $(x,y)$ position from any arbitrary position and orientation. It is quite easy to start the car in a known orientation in a known area, and have it move forward or backward until it sees its $x$- or $y$-coordinate reach a certain threshold, before it takes some other action, such as changing direction. Having the car oscillate between certain $y$-coordinates, or stop and go at certain locations, turns out to be quite simple.
\subsection{Mapping}
For the mapping application of our positioning system, we again decided to use the XBee radio modules to communicate between the PSoC board on the car and our computer. The goal was to have the positioning information collected by the car sent to the computer so that it could be plotted in real time. To accomplish this goal, we used an XBee module on the car with a shield similar to those used for the Arduino transmitters to connect to the PSoC and a XBIB-U-DEV board to connect the XBee module to the computer via USB. These XBees were configured such that they were using a different channel to communicate than the XBees used in the transmitter stations to avoid possible interference that could derail both systems of communication.
On the car side, all communication was coordinated by the PSoC through the provided UART module. Using this module, we can set TX and RX pins to send and receive information via the XBee connected to those designated pins. Whenever data is generated from the receiver, it is written to the UART module and sent over the XBee to the XBee attached to the computer. The position string sent to the computer is always the same: `X' then current x-coordinate, then `Y' then current y-coordinate. An example string, if the car were located at (2.41, -5.34), would be ``X2.41Y-5.34".
\begin{figure}[ht!]
\centering
\includegraphics[width=\textwidth]{Map3.jpg}
\caption{A partial map plotted in Matlab using coordinates received from the car as it drove around the racetrack. At about $(-5, 0)$ you can see where the car veered off the track, and was manually returned to the track, before it continued along the track. On the other side of the curve, at around $(-7, 7)$, there is one noticeable bad data point, which appeared to be caused by a human blocking line of sight between the car and one of the transmitter stations.}
\label{fig:mapping}
\end{figure}
On the computer side, we used Matlab to communicate via serial with the XBee due to its relatively simple serial port I/O commands and ability to easily turn points received into a plot. To create a real-time connection between the car, we first connect to the correct COM port and open it using the correct Baud Rate. Then we set up the plot according to the parameters of our grid such that the plot will be to the same scale of the room. We used the animatedline object in Matlab to integrate new information into the plot as we received it from the car. The animatedline is very efficient for this application as it does not require the user to store each individual point in an array and update it each time a new point is received. Instead, the animatedline object has an addPoint() function that allows the user to add new points to the line that are then stored in the object dynamically, requiring only a refresh of the plot and no vector separately storing points. Then, as each string containing the data points is received via the attached XBee, we can parse the string according to the known formatting and extract the x and y coordinates and feed them into the animatedline for a dynamic plot of real-time position. The Matlab script used to generate these plots can be found in Appendix~\ref{mappingcode}.
Just as the Arduino code for the transmitters required timeouts and other checks to ensure a robust system, due to occasional data loss, this system also needs similar checks to be sure that it does not quit or produce undesirable results when it receives corrupted data. Thus, we have implemented various checks on the input, such as whether the string contains all four elements in the correct order (character, float, character, float), whether the string contains any information, and a timeout to determine if the connection to the car has been lost. Using the fgetl() function, we can take one line of input at a time to process, since we look for a newline character to terminate each string sent from the car. However, one problem that arose with this strategy was that sometime either in the transmission or receiving process, extra characters could be appended onto the front of the string that were either random or whitespace, throwing off our parsing. To avoid these characters being included in the parsing process, we do an initial paring of the string down to just the necessary information before the string format check; specifically, we iterate through the string until the `X' character is found, signifying the beginning of the relevant coordinate string. With these error checks, the script runs smoothly, plotting the points received effectively and exiting gracefully by closing the serial port if an error occurs.
\newpage
\appendix
\section{C code compiled in PSoC Creator for car}
Cypress's APIs use capitalized letters at the start of words. Therefore, whenever we define our own functions, we use only lowercase letters, which helps distinguish functions we write from the provided APIs, and reduces the chance of a name collision.
\subsection{main.c}
\lstinputlisting[language=C]{main.c}
\newpage
\subsection{position.c}
\label{position.c}
\lstinputlisting[language=C]{position.c}
\subsection*{position.h}
\lstinputlisting[language=C]{position.h}
\newpage
\subsection{drive.c}
\lstinputlisting[language=C]{drive.c}
\subsection*{drive.h}
\lstinputlisting[language=C]{drive.h}
\newpage
\subsection{speed.c}
\label{speedcode}
\lstinputlisting[language=C]{speed.c}
\subsection*{speed.h}
\lstinputlisting[language=C]{speed.h}
\newpage
\subsection{steer.c}
\lstinputlisting[language=C]{steer.c}
\subsection*{steer.h}
\lstinputlisting[language=C]{steer.h}
\newpage
\subsection{shell.c}
\lstinputlisting[language=C]{shell.c}
\subsection*{shell.h}
\lstinputlisting[language=C]{shell.h}
\newpage
\subsection{usb\_uart.c} \label{usb_uart}
\lstinputlisting[language=C]{usb_uart.c}
\subsection*{usb\_uart.h}
\lstinputlisting[language=C]{usb_uart.h}
\newpage
\section{Arduino code for transmitter stations}
\label{txcode}
\subsection{Master transmitter station}
\lstinputlisting[language=C++]{master_transmitter.c}
\newpage
\subsection{Slave transmitter stations}
\lstinputlisting[language=C++]{slave_transmitter.c}
\newpage
\section{Arduino code for testing tools}
\subsection{tx\_test\_2}
\label{txtest2}
\lstinputlisting[language=C++]{\detokenize{tx_test_2.c}}
\newpage
\subsection{tx\_test\_4}
\label{txtest4}
\lstinputlisting[language=C++]{\detokenize{tx_test_4.c}}
\newpage
\subsection{rx\_test}
\label{rxtest}
\lstinputlisting[language=C++]{\detokenize{rx_test.c}}
\newpage
\section{MATLAB code for mapping}
\label{mappingcode}
\lstinputlisting[language=MATLAB]{XBeePlot.m}
\end{document}