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 Localization

I 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.

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 Solutions

To 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-Wideband

Ultra-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 \cdot (T_{loop} - T_{reply})/2$.

The initiator sends out a timestamped pulse to the responder, which then responds in $T_{reply}$ time (reported by the responder) with its own timestamp packet. The whole exchange takes $T_{loop}$ time, calculated at the initiator:

❚❚ Pause

Distance 15.00 M $T_{\text{reply}}$ 120.00 NS

$T_{\text{loop}}$ 220.07 NS $T_{\text{reply}}$ 120.00 NS $\text{Derived ToF}$ 50.03 NS Recovered Distance 15.00 M

$$\text{ToF} = \frac{T_{\text{loop}} - T_{\text{reply}}}{2} = \frac{220.07 - 120.00}{2} = 50.03 \text{ ns}$$

$$d = c \cdot \text{ToF} = 15.00 \text{ m}$$

Release to update

The 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:

❚❚ Pause

Wavelength $\lambda$ 80.00

$$\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 $d_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.

❚❚ Pause

$\lambda$ 80.00

$\Delta\varphi$ 0.00 rad $\theta$ 0.00 rad (0.00°)

$$\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 update

Failure 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 System

Another 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.

❚❚ Pause

$r_1$ 130.00 $r_2$ 200.00 $r_3$ 200.00

$$\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}$$

$$\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)$$

Release to update

Failure 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 Filters

Kalman 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 EKF

An EKF is a loop composed of two parts: a prediction and a measurement step. You have a state vector $\hat{\textbf{x}}$ and covariance matrix $P$ you want to keep as consistent as possible with the real unobservable state $\textbf{x}$ (for example, the $x$ and $y$ distance to a target). We use the “hat” $\hat{\cdot}$ to say that this state vector is an estimate of the true state. $P$ doesn’t get a hat since it isn’t an estimate of anything, it just helps inform the estimate $\hat{\textbf{x}}$. To update $\hat{\textbf{x}}$ and $P$, you will ask two questions:

• “What do I think my next state will be?”: Answered by the state transition function $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 $[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(\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 $\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 \in \mathbb{R}^{|\textbf{x}|\times|\textbf{x}|}$ comes in. It’s a square matrix with row/column counts equal to the size of $\textbf{x}$, denoted $|\textbf{x}|$. Each entry $P_{i,j}$ in that matrix answers the question “how do $\textbf{x}_i$ and $\textbf{x}_j$ impact each other’s distributions?” For entries along the diagonal, this reduces to the variance of $\textbf{x}_i$. We use the entries in $P$ to shape our belief of the distributions of the variables in $\textbf{x}$ during the prediction and measurement steps.

Sidenote: Linearization

Linearization 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) = x^2$ (solid line)

$L(x) = 2x - 1$ (dashed line, linearization at $x_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(\textbf{x}, \textbf{u})$ and $h(\textbf{x})$ to linearize around $\hat{\textbf{x}}_{k+1|k}$ using the Jacobians of $f$ and $h$, $F$ and $H$ respectively. Doing so lets us reuse the regular Kalman update equations, making calculations much easier. The notation $k+1|k$ expands to “we condition our guess of $k+1$ on the information we have at timestep $k$”.

The Flow of Data

We now have all of the pieces to describe how data flows through the EKF loop. We start with the current state $\hat{\textbf{x}}_{k|k}$ and covariance matrix $P_{k|k}$, along with our control input for the timestep, $\textbf{u}_k$.

Our first step is to predict what the next state and covariance will be.

$$\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 $F_k P_{k|k} F_k^T$: this is the linearization I was talking about earlier. The $Q_k$ term is the prediction noise covariance for timestep $k$. The noisier the prediction is, the less the filter trusts it. Next, we update $\hat{\textbf{x}}_{k+1|k}$ and $P_{k+1|k}$ with our measurements:

$$\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*}$$

$z_{k+1}$ is our measurement for the timestep. $K_{k+1}$ is the Kalman gain, which weights the measurement against the prediction by their relative variances. A higher gain means we trust the measurement more; lower means we trust our prediction more. The linearization $H_{k+1}P_{k+1|k}H_{k+1}^T$ comes back again, this time for the measurement step. $R_{k+1}$ plays the same role as $Q_k$ but for measurement noise covariance.

And that is a single step of an EKF! Not too bad once you understand how everything fits together.

Now that we have an understanding of the underlying logic of an EKF, it’s time to define our variables.

Step 1: The State Vector $\textbf{x}$ and Covariance Matrix $P$

Arguably the most important part of designing a filter is what state to keep track of. There are a million things going on in any nonlinear system, especially in the case of Beacons, so being selective with what gets stored saves both processing time and later headaches when things inevitably don’t work first try. For Beacons, we will store four values:

• $\Delta x$ and $\Delta y$: The estimated difference in $x$ and $y$ (measured in meters) from the current device to the other in world frame. Can be turned into a triangle with Pythagoras’ theorem to calculate the bearing and distance.
• $\Delta \dot{x}$ and $\Delta \dot{y}$: The estimated velocities in each component direction, important in the physics calculations.

These four values (packed as $[\Delta x, \Delta y, \Delta \dot{x}, \Delta \dot{y}]^T$) allow us to approximate the physical system of two devices moving in a 2D plane. We initialize with the UWB distance $d_0$, assuming the initial relative position lies entirely along $x$:

$$\hat{\textbf{x}}_0 = [d_0, 0, 0, 0]^T$$

It’s important to note that it’s impossible to tell the direction to the other device at this point, since at $k=0$ we only have one distance measurement. The filter will accumulate a belief of the direction as the user moves the device around. We initialize the covariance matrix $P_0$ using tuned values:

$$\textrm{diag}(P_0) = [0.04, d_0^2, 1, 1]$$

The $0.04\ \textrm{m}^2$ (20 cm standard deviation) informs the EKF that the radial covariance is small and constrained by the UWB’s relatively low error, with the starting assumption that we are directly on the $x$ axis. The $d_0^2$ variance informs the EKF that the larger the distance, the more unsure it should be about the $y$ axis (since we assume we are only on the $x$ axis at $k=0$). We initialize the variances for $\Delta\dot{x}$ and $\Delta\dot{y}$ to $1\ \textrm{(m/s)}^2$ as a tuned starting value. The rest of $P_0$ is $0$. The filter will start changing all of its values as it learns more about the system.

Now that we know what we’ll be storing, we need to think about how we project that state prediction forward.

Step 2: The State Transition Function $f(\textbf{x}, \textbf{u})$

We have our state vector layout, now we need to project it forward each timestep using information from the control vector. Luckily this is pretty easy, using physics equations you likely learned in high school:

$$\begin{align*} \Delta\dot{x}_{k+1|k}&=\Delta\dot{x}_{k|k} + a_{x,k} \cdot \Delta t\\ \Delta\dot{y}_{k+1|k}&=\Delta\dot{y}_{k|k} + a_{y,k} \cdot \Delta t\\ \Delta x_{k+1|k} &= \Delta x_{k|k} + \Delta\dot{x}_{k|k}\cdot \Delta t + \tfrac{1}{2}\,a_{x,k}\cdot \Delta t^2 \\ \Delta y_{k+1|k} &= \Delta y_{k|k} + \Delta\dot{y}_{k|k}\cdot \Delta t + \tfrac{1}{2}\,a_{y,k}\cdot \Delta t^2 \end{align*}$$

These equations give our predicted state update $\hat{\textbf{x}}_{k+1|k}$.

Note: The world-frame acceleration values $a_{x,k}$ and $a_{y,k}$ are calculated using an AHRS filter combining data from the IMU. I’ve omitted the AHRS step for brevity.

Next, we update $P$. To do so we need our prediction Jacobian, $F$, calculated by taking the partial derivatives of the above equations:

$$F=\frac{\partial f}{\partial \textbf{x}}=\begin{bmatrix}1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{bmatrix}$$

Diagonal entries of $1$ mean each value carries forward. The two off-diagonal $\Delta t$s integrate $\Delta x$ via $\Delta\dot{x}$ and $\Delta y$ via $\Delta\dot{y}$ over a single step. We assume constant acceleration between steps.

We then follow the prediction equations defined above to get $\hat{\textbf{x}}_{k+1|k}$ and $P_{k+1|k}$. $Q$ is a tuned matrix of constants.

Step 3: The Measurement Function $h(\textbf{x})$

Now that we have our estimates $\hat{\textbf{x}}_{k+1|k}$ and $P_{k+1|k}$, we need to check our work. Our measurement function is pretty simple:

$$h(\textbf{x}) = \sqrt{\Delta x ^2 + \Delta y^2} = r$$

All this does is take our deltas and convert them to a distance comparable to the UWB reading. Taking the partial derivative:

$$H = \frac{\partial h}{\partial \textbf{x}} = \begin{bmatrix}\displaystyle\frac{\Delta x}{r} & \displaystyle\frac{\Delta y}{r}& 0 & 0\end{bmatrix}$$

The pair $(\Delta x/r, \Delta y/r)$ is the unit vector from this device to the other. Movement along that direction changes the distance one-for-one; movement perpendicular to it doesn’t change the distance at all. Setting $z_{k+1}$ to our UWB reading, we update with the above equations. Similarly, $R$ is a tuned scalar constant. $R$ isn’t a matrix because the measurement function returns a scalar.

Step 4: Extracting the Results

Whenever we want to get what the filter thinks is our current bearing, we run a simple function to extract the values we need, the bearing $\beta$ and the $1\sigma$ uncertainty $\sigma_\beta$, both in radians:

$$\begin{align*} r^2 &= \Delta x^2 + \Delta y^2 \\ \beta &= \textrm{atan2}(\Delta y, \Delta x) \\ \frac{\partial \beta}{\partial \Delta x} = \beta_{\Delta x} &= \frac{-\Delta y}{r^2} \\ \frac{\partial \beta}{\partial \Delta y} = \beta_{\Delta y} &= \frac{\Delta x}{r^2} \\ v&= P_{(0, 0)} \beta_{\Delta x}^2 + 2 P_{(0, 1)} \beta_{\Delta x} \beta_{\Delta y} + P_{(1, 1)} \beta_{\Delta y}^2 \\ \sigma_\beta &= \begin{cases} \pi &\textrm{if $r^2 < 10^{-6}$} \\ \sqrt{\max(0, v)} &\text{otherwise} \end{cases} \end{align*}$$

We set $\sigma_\beta = \pi$ (no knowledge of bearing) if the distance is so small that it’s basically impossible to tell, i.e. the filter thinks $r$ is less than a millimeter. Otherwise, we take $\sigma_\beta = \sqrt{\max(0, v)}$, with $v$ being the variance.

Demo

Now we have the entire pipeline! Let’s see what it can do:

Drag the Beacons (multitouch supported), or click the canvas and use WASD for 1 / IJKL for 2 / R to reset.

↺ Reset ❚❚ Pause

$$\hat{\mathbf{x}} = \begin{bmatrix} 0.00 \\ 0.00 \\ 0.00 \\ 0.00 \end{bmatrix}$$

$\beta$ 0.00° $\sigma_\beta$ 0.00° $\hat{r}$ 0.00 m $z$ 0.00 m NaN resets 0

The colored region is the filter’s belief about where the other Beacon is.

---

There are tons of applications for EKFs, ranging from robotics to digital signal processing to navigation systems, and it all becomes so much more interesting when you can see how they work through all of the complicated matrix math. They are the perfect solution for Beacons, and I hope you someday experience the joy of using one, too.

Further Reading

I used this explanation of regular and extended Kalman filters extensively when writing this post; I highly recommend reading it yourself if you want to dive deeper!

If you want to read up on a more recent innovation, look into unscented Kalman filters! They turned out to be a bit overkill for my use case, but I would love to use them in the future.