Marco Polo

You walk into a cafe, looking for your friend. Seems like an easy task, until you see it’s so packed that you can’t see through the crowd at all, and everyone’s talking so loud that you can barely hear anything. The only things you know are your movements, and how far you are from your friend (through the special psychic bond you two share). How will you find each other?I wanted to solve the exact same problem, but with devices instead of people (so no psychic connection for me), existing in a space of hundreds of other devices. Working the problem taught me a lot of really interesting science relating to robotics and state estimation, and I wrote this post so you can learn, too.The Formal Problem: Range-Only Relative LocalizationI have two microcontrollers (the large blue boards). Each has an inertial measurement unit (IMU, small board on top right), which gives me data including acceleration and compass heading. They also have an ultra-wideband unit (UWB, small board in slot on left), which gives the distance to the other unit in the pair. The Beacons in question. Each of these packages will be contained within a wearable device, and can therefore prompt its wearer to move around. This is an important freedom, because it means we can use the change in distance over time to determine location during the localization process, rather than purely statically.Potential SolutionsTo get us in the mindset of doing a technical implementation, let’s understand some options we have and see why they are or aren’t a good fit for what we’re trying to do.Candidate 1: Multi-Antenna Ultra-WidebandUltra-wideband localization, found in location-aware products including AirTags, uses a simple call-and-response system known as time of flight. The UWB measures the time it takes for a roundtrip exchange of information, which is then turned into a distance d=c⋅(Tloop−Treply)/2d = c \cdot (T_{loop} - T_{reply})/2.The initiator sends out a timestamped pulse to the responder, which then responds in TreplyT_{reply} time (reported by the responder) with its own timestamp packet.

The whole exchange takes TloopT_{loop} time, calculated at the initiator:Distance 15.00 M TreplyT_{\text{reply}} 120.00 NS TloopT_{\text{loop}} 220.07 NS TreplyT_{\text{reply}} 120.00 NS Derived ToF\text{Derived ToF} 50.03 NS Recovered Distance 15.00 MToF=Tloop−Treply2=220.07−120.002=50.03 ns\text{ToF} = \frac{T_{\text{loop}} - T_{\text{reply}}}{2} = \frac{220.07 - 120.00}{2} = 50.03 \text{ ns}d=c⋅ToF=15.00 md = c \cdot \text{ToF} = 15.00 \text{ m}Release to updateThe time of flight system works fine for getting the distance to the other device, but alone is insufficient for getting the bearing to the other device. This limitation is where phase difference of arrival (PDoA) ultra-wideband comes in.PDoA uses a property of radio waves called the phase. Light and radio waves oscillate up and down as they travel; where they are in that cycle is the phase. As the wave passes any fixed point in space, its phase at that point cycles:Wavelength λ\lambda 80.00 φ=3.14 rad\varphi = 3.14 \text{ rad}We use this property to determine the heading relative to the other device. By positioning two antennas a known distance dad_a apart, we can derive the angle of arrival of the other device’s messages by comparing the phase at the two antennas at each timestep:Drag the point to move the wave source.λ\lambda 80.00 Δφ\Delta\varphi 0.00 rad θ\theta 0.00 rad (0.00°)θ=arcsin⁡

⁣(Δφ⋅λ2π⋅da)=arcsin⁡ ⁣(0.00⋅80.002π⋅100)=0.00 rad\theta = \arcsin\!\left(\frac{\Delta\varphi \cdot \lambda}{2\pi \cdot d_a}\right) = \arcsin\!\left(\frac{0.00 \cdot 80.00}{2\pi \cdot 100}\right) = 0.00 \text{ rad}Release to updateFailure mode: PDoA is a great system, but it has a caveat: it requires two antennas. This design restriction makes it very difficult to integrate into a wearable, where you have to deal with skin substantially attenuating signals. One antenna (what I chose for Beacons) can maybe get through with clever placement, but two is much more difficult.Candidate 2: External Trilateration SystemAnother potential way to do things is to involve external devices with known fixed positions, usually known as anchors. With at least three of them, a Beacon can measure its distance to each and solve for its location. This strategy is the same as how GPS works, using multiple circles (or spheres in 3D) of distance to derive an exact location for the device:Drag the beacon.r1r_1 130.00 r2r_2 200.00 r3r_3 200.00r12=(x−x1)2+(y−y1)2r22=(x−x2)2+(y−y2)2r32=(x−x3)2+(y−y3)2\begin{aligned} r_1^2 &= (x - x_1)^2 + (y - y_1)^2 \\ r_2^2 &= (x - x_2)^2 + (y - y_2)^2 \\ r_3^2 &= (x - x_3)^2 + (y - y_3)^2

\end{aligned}[x2−x1y2−y1x3−x1y3−y1][xy]=12[k2−k1k3−k1],ki=xi2+yi2−ri2\begin{bmatrix} x_2 - x_1 & y_2 - y_1 \\ x_3 - x_1 & y_3 - y_1 \end{bmatrix} \begin{bmatrix} x \\ y \end{bmatrix} = \tfrac{1}{2}\begin{bmatrix} k_2 - k_1 \\ k_3 - k_1 \end{bmatrix},\quad k_i = x_i^2 + y_i^2 - r_i^2(x,y)=(240.00, 190.00)(x, y) = (240.00,\ 190.00)Release to updateFailure mode: Trilateration is easy, once you have those three anchors. In practice, this means permanently installing devices to always broadcast their locations within a space. The localization needs to work without specially-installed equipment, so I don’t see trilateration as a valid solution. However, if I were to deploy these in a known location that has a lot of traffic (like a conference venue), this strategy could act as a refining system to keep the main system grounded.Candidate 3: Kalman FiltersKalman filters are the gold standard for state-space estimation in physical spaces, and are exactly what we need to solve our problem. They operate on the principle of having an internal estimation of the unobservable true state — in this case, the relative position — which is updated with new information we get from measurements in a looping process. This loop eventually converges on a reasonable estimate that we can use to guide the user. Our true state is unobservable because we can’t directly measure bearing from the sensors alone; the bearing comes from the combined information from all of our sensors.I specifically chose an extended Kalman filter (EKF), which gives support for the nonlinear functions needed to calculate the expected bearing. The reason an EKF is our best solution is that it only requires one UWB antenna (unlike PDoA) and only two devices (unlike trilateration).Implementing an EKFAn EKF is a loop composed of two parts: a prediction and a measurement step.

You have a state vector x^\hat{\textbf{x}} and covariance matrix PP you want to keep as consistent as possible with the real unobservable state x\textbf{x} (for example, the xx and yy distance to a target). We use the “hat” ⋅^\hat{\cdot} to say that this state vector is an estimate of the true state. PP doesn’t get a hat since it isn’t an estimate of anything, it just helps inform the estimate x^\hat{\textbf{x}}. To update x^\hat{\textbf{x}} and PP, you will ask two questions: “What do I think my next state will be?”: Answered by the state transition function f(x,u)f(\textbf{x}, \textbf{u}) (also known as the prediction function), this takes the current state along with the control input and projects it to the next timestep. In the case of Beacons, it asks: “Given how I’m moving and how I’m accelerating (represented as [ax,ay][a_x, a_y]), what does physics think about where I’ll end up next?” Acceleration is a control input and not a measurement because it’s driving the state transition, not a result of it. “What measurements do I expect to receive?”: Answered by the measurement function h(x)h(\textbf{x}), this also takes the current state, but it instead asks what the probable sensor readings are. For Beacons: “Given how I’m moving, what do I expect the UWB reading to be at the next timestep?” The state x\textbf{x} represents what the system is keeping track of, but it’s also very important to know the relations of each of the state values to each other. This is where the covariance matrix P∈R∣x∣×∣x∣P \in \mathbb{R}^{|\textbf{x}|\times|\textbf{x}|} comes in. It’s a square matrix with row/column counts equal to the size of x\textbf{x}, denoted ∣x∣|\textbf{x}|. Each entry Pi,jP_{i,j} in that matrix answers the question “how do xi\textbf{x}_i and xj\textbf{x}_j impact each other’s distributions?” For entries along the diagonal, this reduces to the variance of xi\textbf{x}_i.

We use the entries in PP to shape our belief of the distributions of the variables in x\textbf{x} during the prediction and measurement steps.Sidenote: LinearizationLinearization is the process of taking a nonlinear function (like a parabola) and “zooming in” really close, so close that the curved line actually looks straight, or in other words, linear. It’s an approximation so it’s not guaranteed to be optimal, but it’s good enough for our use case (and others, including GPS).Zoom 1× f(x)=x2f(x) = x^2 (solid line)L(x)=2x−1L(x) = 2x - 1 (dashed line, linearization at x0=1x_0 = 1)EKFs have an additional layer of complexity in return for supporting nonlinear functions like square root, which you will see are necessary for our task: they require differentiability of f(x,u)f(\textbf{x}, \textbf{u}) and h(x)h(\textbf{x}) to linearize around x^k+1∣k\hat{\textbf{x}}_{k+1|k} using the Jacobians of ff and hh, FF and HH respectively. Doing so lets us reuse the regular Kalman update equations, making calculations much easier. The notation k+1∣kk+1|k expands to “we condition our guess of k+1k+1 on the information we have at timestep kk”.The Flow of DataWe now have all of the pieces to describe how data flows through the EKF loop. We start with the current state x^k∣k\hat{\textbf{x}}_{k|k} and covariance matrix Pk∣kP_{k|k}, along with our control input for the timestep, uk\textbf{u}_k.

Our first step is to predict what the next state and covariance will be.x^k+1∣k=fk(x^k∣k,uk)Pk+1∣k=FkPk∣kFkT+Qk\begin{align*}\hat{\textbf{x}}_{k+1|k} &= f_k(\hat{\textbf{x}}_{k|k}, \textbf{u}_k)\\P_{k+1|k}&=F_k P_{k|k} F_k^T + Q_k\end{align*}Notice FkPk∣kFkTF_k P_{k|k} F_k^T: this is the linearization I was talking about earlier. The QkQ_k term is the prediction noise covariance for timestep kk. The noisier the prediction is, the less the filter trusts it. Next, we update x^k+1∣k\hat{\textbf{x}}_{k+1|k} and Pk+1∣kP_{k+1|k} with our measurements:Kk+1=Pk+1∣kHk+1T(Hk+1Pk+1∣kHk+1T+Rk+1)−1x^k+1∣k+1=x^k+1∣k+Kk+1(zk+1−hk+1(x^k+1∣k))Pk+1∣k+1=(I−Kk+1Hk+1)Pk+1∣k\begin{align*} K_{k+1}&=P_{k+1|k}H_{k+1}^T(H_{k+1}P_{k+1|k}H_{k+1}^T + R_{k+1})^{-1}\\ \hat{\textbf{x}}_{k+1|k+1} &= \hat{\textbf{x}}_{k+1|k} + K_{k+1}(z_{k+1} - h_{k+1}(\hat{\textbf{x}}_{k+1|k}))\\ P_{k+1|k+1} &= (I - K_{k+1}H_{k+1})P_{k+1|k} \end{align*}zk+1z_{k+1} is our measurement for the timestep.