mirror of
https://gitlab.ewi.tudelft.nl/ee2l1/2025-2026/A.K.03.git
synced 2025-12-12 16:00:56 +01:00
105 lines
5.1 KiB
Plaintext
105 lines
5.1 KiB
Plaintext
{
|
|
"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
|
|
}
|