# TDOA Localization

The time difference of arrival (TDOA) measurement is given as:

$d = |\vec{a} - \vec{n}| - |\vec{b} - \vec{n}|$

where $\vec{a}$ and $\vec{b}$ are anchor nodes, $\vec{n}$ is the vehicle, and d is the difference in time of arrival of the anchor messages at the vehicle, converted to $m$ using the speed of light.

We are going to linearize this measurement equation and then plot the localization accuracy in a given volume with known anchor locations.

See [Kalman Filtering](https://en.wikipedia.org/wiki/Kalman_filter)

See [Multilateration/TDOA](https://en.wikipedia.org/wiki/Multilateration)


```python
import sympy
from itertools import combinations 
import numpy as np
import matplotlib.pyplot as plt
sympy.init_printing()

def derive_measurement_jacobian():
    """
    Compute the measurement jacobian.
    
    @return: Returns the symbolic Jacobian H, and a function that
        computes H given the anchors (a, b), and the vehicle/node (n).
    """
    a = sympy.Matrix(sympy.symbols('a_0:3', real=True))
    b = sympy.Matrix(sympy.symbols('b_0:3', real=True))
    n = sympy.Matrix(sympy.symbols('n_0:3', real=True))

    d = sympy.Matrix.norm(a - n) - sympy.Matrix.norm(b - n)

    da = sympy.symbols('da')
    db = sympy.symbols('db')


    H = sympy.Matrix([d]).jacobian(n)
    H.simplify()
    H
    f_H = sympy.lambdify((a, b, n), H)
    return {'H': H, 'f_H': f_H}

meas = derive_measurement_jacobian()
meas['H']
```




$\displaystyle \left[\begin{matrix}- \frac{a_{0} - n_{0}}{\sqrt{\left(a_{0} - n_{0}\right)^{2} + \left(a_{1} - n_{1}\right)^{2} + \left(a_{2} - n_{2}\right)^{2}}} + \frac{b_{0} - n_{0}}{\sqrt{\left(b_{0} - n_{0}\right)^{2} + \left(b_{1} - n_{1}\right)^{2} + \left(b_{2} - n_{2}\right)^{2}}} & - \frac{a_{1} - n_{1}}{\sqrt{\left(a_{0} - n_{0}\right)^{2} + \left(a_{1} - n_{1}\right)^{2} + \left(a_{2} - n_{2}\right)^{2}}} + \frac{b_{1} - n_{1}}{\sqrt{\left(b_{0} - n_{0}\right)^{2} + \left(b_{1} - n_{1}\right)^{2} + \left(b_{2} - n_{2}\right)^{2}}} & - \frac{a_{2} - n_{2}}{\sqrt{\left(a_{0} - n_{0}\right)^{2} + \left(a_{1} - n_{1}\right)^{2} + \left(a_{2} - n_{2}\right)^{2}}} + \frac{b_{2} - n_{2}}{\sqrt{\left(b_{0} - n_{0}\right)^{2} + \left(b_{1} - n_{1}\right)^{2} + \left(b_{2} - n_{2}\right)^{2}}}\end{matrix}\right]$




```python
def find_max_error(anchors, n):
    """
    Find the lhe maximum error of the UWB localization method.
    
    @param anchors: A list of 3d points representing anchor locations
    @param n: A 3d point representing the location of the node/ vehicle.
    """
    P = 100000*np.eye(3) # this represents that we don't know where we are initially, it is the covariance matrix
    R = (0.2)**2  # the measurement variance
    for a, b in combinations(anchors, 2):
        # if evaluating at an anchor, shift anchor slightly
        if np.linalg.norm(a-n) == 0:
            a = np.array(a) + 1e-6*np.array([1, 1, 1])
        if np.linalg.norm(b-n) == 0:
            b = np.array(b) + 1e-6*np.array([1, 1, 1])
        # Kalman correction
        H = meas['f_H'](a, b, n)
        S = H.dot(P).dot(H.T) + R
        K = P.dot(H.T).dot(np.linalg.inv(S))
        P = P - K.dot(H).dot(P)
    # Find max error standard deviation
    evals, evecs = np.linalg.eig(P)
    return np.sqrt(np.max(evals))

def plot_accuracy(anchors, nz, border=2, vmin=0, vmax=1):
    """
    Plot a contour of localization accuracy as a fixed height.
    
    @param anchors: A list of 3d points representing anchor locations
    @param nz: The z height of the node for the heat map.
    @param vmin: The min value of the heat map.
    @param vmax: The max value of the heat map.
    """
    nx_list = np.linspace(np.min(anchors[:, 0]) - border, np.max(anchors[:, 0]) + border, 20)
    ny_list = np.linspace(np.min(anchors[:, 1]) - border, np.max(anchors[:, 1]) + border, 20)
    levels=np.arange(vmin, vmax + 0.1, 0.1)
    data = []
    for ny in ny_list:
        x_data = []
        for nx in nx_list:
            x_data.append(find_max_error(anchors, [nx, ny, nz]))
        data.append(x_data)
    data = np.array(data)
    X, Y = np.meshgrid(nx_list, ny_list)
    plt.contourf(X, Y, data, cmap='jet', levels=levels, vmin=vmin, vmax=vmax)
    for anchor_id, anchor in enumerate(anchors):
        plt.text(anchor[0]+ 0.1*anchor[2], anchor[1], str(anchor_id))  
        plt.plot(anchor[0], anchor[1], 'kx')
    plt.xlabel('x, m')
    plt.ylabel('y, m')
    cbar = plt.colorbar()
    return plt.gca()
```

Now we can plot a heat map of the localization accuracy of the method within the flight volume.


```python
sx = 3.8  # The x separation of the anchors.
sy = 6  # The y separation of the anchors.
z0 = 0.57 # lower height on tripod
z1 = 2.36 # upper anchor height on tripod
anchors = np.array([
    [0, 0, z0],
    [0, 0, z1],
    [0, sy, z0],
    [0, sy, z1],
    [sx, sy, z0],
    [sx, sy, z1],
    [sx, 0, z0],
    [sx, 0, z1]
])

z_list = [0, 1, 2, 3, 4]
for zi, z in enumerate(z_list):
    border = 2
    plt.figure()
    ax = plot_accuracy(anchors, nz=z, vmin=0, vmax=0.5, border=border)
    plt.title('TDOA Localization Accuracy at z={:g} m'.format(z));
    plt.axis('equal')
```

We will now try to move some of the anchors back and up and see how this changes the localization accuracy.


```python
sx = 2
sy = 5
z0 = 0.1
z1 = 2.3
anchors = np.array([
    [0, 0, z0],
    [0, 0, z1],
    [0, sy, z0],
    [0, sy, z1],
    #[sx, sy, z0],
    [sx, sy, z1],
    #[sx, 0, z0],
    [sx, 0, z1],
    [5, 0, 5],
    [5, sy, 5],
    #[10, 0, 10],
    #[10, sy, 10]
])

z_list = [0, 1, 2, 3, 4]
for zi, z in enumerate(z_list):
    plt.figure()
    ax = plot_accuracy(anchors, nz=z, vmin=0, vmax=0.5, border=2)
    plt.title('TDOA Localization Accuracy at z={:g} m'.format(z));
    plt.axis('equal')
```

e22c4868285f04feb58f1313b91fceac7db9bdd2
ipynb
Jupyter Notebook
notebooks/08-TDOA-Localization.ipynb
tiiuae/bswarm
b4378a37f66ca609993cbfce3417039e428ba09a
BSD-3-Clause
2019-07-03T17:12:10.000Z
2022-03-29T23:48:48.000Z
notebooks/08-TDOA-Localization.ipynb
tiiuae/bswarm
b4378a37f66ca609993cbfce3417039e428ba09a
BSD-3-Clause
2019-06-26T18:59:26.000Z
2019-06-26T19:08:03.000Z
notebooks/08-TDOA-Localization.ipynb
tiiuae/bswarm
b4378a37f66ca609993cbfce3417039e428ba09a
BSD-3-Clause
2019-06-26T18:38:19.000Z
2021-07-27T10:18:25.000Z
Qwen/Qwen-72B
1. YES
2. YES