{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Appendix C: TDOA Localization Algorithm\n",
    "<a id=\"app-locating-algorithms\"></a>\n",
    "\n",
    "## Time-Difference Of Arrival (TDOA) Algorithm\n",
    "<a id=\"sec-loc-tdoalocalization\"></a>\n",
    "\n",
    "Let $\\mathbf{x} = [x,y]^T$ be the location of the car, and call $\\mathbf{x}_i = [x_i,y_i]^T$, $i = 1, \\cdots, N$ the known locations of the microphones. The Time Difference of Arrival (TDOA) for each sensor pair $(i,j)$ is translated to a range difference (by multiplying by the speed of sound) $r_{ij}$, where\n",
    "\n",
    "$\n",
    "r_{ij} = d_i - d_j\\,,\\qquad  d_i = \\|\\mathbf{x} - \\mathbf{x}_i\\|\\,,\\qquad  d_j = \\|\\mathbf{x} - \\mathbf{x}_j\\|\n",
    "$\n",
    "\n",
    "The objective is to compute from the available measurements, $\\{r_{ij};\\; i,j = 1, \\cdots, N, i<j\\}$ the location $\\mathbf{x}$ of the car. Each range measurement specifies a hyperbolic curve in the 2-D plane of possible locations $\\mathbf{x}$, and the combination of measurements asks for the intersection of the curves (this is called *multilateration*). Equivalently, we have to solve a system of quadratic equations.\n",
    "\n",
    "Fortunately, it is possible to transform these into a system of *linear* equations, augmented with some additional \"nuisance\" parameters, as follows. Write\n",
    "\n",
    "\n",
    "$r_{ij} = d_i - d_j \n",
    "\\quad \\Rightarrow \\quad \n",
    "(r_{ij} + d_j)^2 = d_i^2\n",
    "\\quad \\Leftrightarrow \\quad \n",
    "r_{ij}^2 + d_j^2 + 2 r_{ij}d_j = d_i^2$\n",
    "\n",
    "\n",
    "We will use\n",
    "\n",
    "\n",
    "$\\begin{aligned}\n",
    "d_i^2 &= \\|\\mathbf{x} - \\mathbf{x}_i\\|^2 = \\|\\mathbf{x}\\|^2 + \\|\\mathbf{x}_i\\|^2 - 2\\mathbf{x}_i^T \\mathbf{x} \\\\\n",
    "\\text{and}\\quad d_j^2 &= \\|\\mathbf{x} - \\mathbf{x}_j\\|^2 = \\|\\mathbf{x}\\|^2 + \\|\\mathbf{x}_j\\|^2 - 2\\mathbf{x}_j^T \\mathbf{x}\n",
    "\\end{aligned}$\n",
    "\n",
    "\n",
    "Inserting this into the previous equation, we see that the quadratic term $\\|\\mathbf{x}\\|^2$ is eliminated:\n",
    "\n",
    "$\n",
    "\\begin{aligned}\n",
    "r_{ij}^2 + d_j^2 & + 2 r_{ij}d_j = d_i^2 \\\\\n",
    "&\\Leftrightarrow \\quad r_{ij}^2 + \\|\\mathbf{x}\\|^2 + \\|\\mathbf{x}_j\\|^2 - 2\\mathbf{x}_j^T \\mathbf{x} + 2 r_{ij} d_j \\\\\n",
    "& \\qquad \\qquad = \\|\\mathbf{x}\\|^2 + \\|\\mathbf{x}_i\\|^2 - 2 \\mathbf{x}_i^T \\mathbf{x} \\\\\n",
    "&\\Leftrightarrow \\quad r_{ij}^2 - \\|\\mathbf{x}_i\\|^2 + \\|\\mathbf{x}_j\\|^2 = 2 (\\mathbf{x}_j - \\mathbf{x}_i)^T \\mathbf{x} - 2 r_{ij} d_j\n",
    "\\end{aligned}\n",
    "$\n",
    "\n",
    "This can be written in vector notation as\n",
    "\n",
    "$\n",
    "[2(\\mathbf{x}_j - \\mathbf{x}_i)^T \\quad -2r_{ij}] \\begin{bmatrix} \\mathbf{x} \\\\ d_j \\end{bmatrix} = r_{ij}^2 - \\|\\mathbf{x}_i\\|^2 + \\|\\mathbf{x}_j\\|^2\n",
    "$\n",
    "\n",
    "where the row vector and RHS are known, and $\\mathbf{x}$ and $d_j$ are unknown. If we now write the equations for all pairs $(i,j)$, $i<j$ and assume $N=4$ sensors, we obtain a matrix equation as\n",
    "\n",
    "\n",
    "\\begin{bmatrix}\n",
    "2(\\mathbf{x}_2 - \\mathbf{x}_1)^T & -2r_{12} & & \\\\\n",
    "2(\\mathbf{x}_3 - \\mathbf{x}_1)^T &           & -2r_{13} & \\\\\n",
    "2(\\mathbf{x}_4 - \\mathbf{x}_1)^T &           &           & -2r_{14}  \\\\\n",
    "2(\\mathbf{x}_3 - \\mathbf{x}_2)^T &           & -2r_{23} & \\\\\n",
    "2(\\mathbf{x}_4 - \\mathbf{x}_2)^T &           &           & -2r_{24} \\\\\n",
    "2(\\mathbf{x}_4 - \\mathbf{x}_3)^T &           &           & -2r_{34} \n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix} \n",
    "\\mathbf{x} \\\\ d_2 \\\\ d_3 \\\\ d_4 \n",
    "\\end{bmatrix}\n",
    "=\n",
    "\\begin{bmatrix} \n",
    "r_{12}^2 - \\|\\mathbf{x}_1\\|^2 + \\|\\mathbf{x}_2\\|^2 \\\\\n",
    "r_{13}^2 - \\|\\mathbf{x}_1\\|^2 + \\|\\mathbf{x}_3\\|^2 \\\\\n",
    "r_{14}^2 - \\|\\mathbf{x}_1\\|^2 + \\|\\mathbf{x}_4\\|^2 \\\\\n",
    "r_{23}^2 - \\|\\mathbf{x}_2\\|^2 + \\|\\mathbf{x}_3\\|^2 \\\\\n",
    "r_{24}^2 - \\|\\mathbf{x}_2\\|^2 + \\|\\mathbf{x}_4\\|^2 \\\\\n",
    "r_{34}^2 - \\|\\mathbf{x}_3\\|^2 + \\|\\mathbf{x}_4\\|^2 \n",
    "\\end{bmatrix}\n",
    "\n",
    "\n",
    "This is an overdetermined linear set of equations (6 equations, 5 unknowns) of the form $\\mathbf{A} \\mathbf{y} = \\mathbf{b}$, and can be solved by computing the pseudo-inverse (or left-inverse) of $\\mathbf{A}$, i.e., $\\mathbf{y} = \\mathbf{A}^\\dagger \\mathbf{b} = (\\mathbf{A}^T\\mathbf{A})^{-1} \\mathbf{A}^T \\mathbf{b}$.\n",
    "\n",
    "We thus obtain $\\mathbf{x}$, as well as the nuisance parameters $d_2,d_3,d_4$ that depend on $\\mathbf{x}$.\n",
    "\n",
    "Although there are 6 equations, they are linearly dependent: verify this (e.g., on a noiseless testcase in Python).\n",
    "\n",
    "**Question:** What happens if $r_{12} = 0$?\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}