{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "d192a2e3-76c9-40f0-9f83-e13e1b516426",
   "metadata": {},
   "source": [
    "# Signals and Systems (ECE-Y425)\n",
    "\n",
    "## Electrical and Computer Engineering Department\n",
    "\n",
    "### University of Patras, Greece\n",
    "\n",
    "**Instructor:** Konstantinos Chatzilygeroudis\n",
    "\n",
    "**Email:** [costashatz@upatras.gr](mailto:costashatz@upatras.gr)\n",
    "\n",
    "#### Hands-On Session on Sound Localization\n",
    "\n",
    "**Objective:** Localize a sound source in 2D from signals coming into 4 microphones."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fa2c1fda-0331-4bab-a0ea-563944081bb9",
   "metadata": {},
   "source": [
    "## Overview\n",
    "\n",
    "In this session, we will build a simple **sound source localization pipeline** using three microphones.\n",
    "\n",
    "The goal is to simulate a sound source, generate the signals received by three microphones, add noise and delays, and then estimate where the sound came from.\n",
    "\n",
    "This session connects several signals and systems concepts:\n",
    "\n",
    "- signal propagation delay,\n",
    "- sampling,\n",
    "- filtering,\n",
    "- cross-correlation,\n",
    "- time-difference-of-arrival estimation,\n",
    "- least-squares localization,\n",
    "- calibration and error analysis.\n",
    "\n",
    "## Learning Objectives\n",
    "\n",
    "By the end of the session, students should be able to:\n",
    "\n",
    "1. Simulate microphone signals from a sound source.\n",
    "2. Model propagation delay and attenuation.\n",
    "3. Add realistic noise to measured signals.\n",
    "4. Apply IIR filter to improve delay estimation.\n",
    "5. Estimate time delays using cross-correlation.\n",
    "6. Use time-difference-of-arrival measurements to estimate source position.\n",
    "7. Understand how microphone placement, noise, filtering, and sampling rate affect localization accuracy.\n",
    "\n",
    "## Scenario\n",
    "\n",
    "We place three microphones in a 2D room.\n",
    "\n",
    "The sound source emits a short signal, such as a clap, pulse, or chirp. Because the source is at different distances from each microphone, each microphone receives the same signal at a slightly different time.\n",
    "\n",
    "The localization problem is:\n",
    "\n",
    "> Given the three microphone signals, estimate the position of the sound source.\n",
    "\n",
    "## Microphone Geometry\n",
    "\n",
    "We assume the microphones are placed at known positions:\n",
    "\n",
    "```text\n",
    "Mic 2      Mic 4\n",
    "(0, 1)     (1, 1)\n",
    "\n",
    "* -------- * \n",
    "   \n",
    "   \n",
    "   \n",
    "* -------- * \n",
    "Mic 1      Mic 3\n",
    "(0, 0)     (1, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ecf7cad5-fa34-4b9b-a4d2-af4c48ef52d2",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "# Microphones positions\n",
    "# TO-DO: You can play with the positioning of the microphones\n",
    "mics = np.array([\n",
    "    [0.0, 0.0],   # Mic 1\n",
    "    [0.0, 1.0],   # Mic 2\n",
    "    [1.0, 0.0],   # Mic 3\n",
    "    [1.0, 1.0]    # Mic 4\n",
    "])\n",
    "\n",
    "num_mics = mics.shape[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f6e2347c-ae11-4752-830d-18fc3de18ed8",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Source position\n",
    "source_pos = np.array([0.65, 0.45])\n",
    "\n",
    "# Speed of sound\n",
    "c = 343.0  # m/s"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "91610b80-5b5e-4373-9725-aaff783b37f5",
   "metadata": {},
   "source": [
    "### Part 1: Generation of the source signal\n",
    "\n",
    "A pure sinusoid is not ideal for localization because it is periodic, making time delay estimation ambiguous.\n",
    "\n",
    "Instead, we use a short chirp signal.\n",
    "\n",
    "A chirp is useful because it contains a range of frequencies and has a clear time structure."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e5551d11-c3e6-4976-885c-641d57d19316",
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "from scipy.signal import chirp\n",
    "\n",
    "fs = 44100\n",
    "duration = 0.05\n",
    "\n",
    "t = np.arange(int(fs * duration)) / fs\n",
    "\n",
    "source_signal = chirp(\n",
    "    t,\n",
    "    f0=500,\n",
    "    f1=5000,\n",
    "    t1=duration,\n",
    "    method=\"quadratic\"\n",
    ")\n",
    "\n",
    "plt.figure(figsize=(8, 3))\n",
    "plt.plot(t, source_signal)\n",
    "plt.xlabel(\"Time [s]\")\n",
    "plt.ylabel(\"Amplitude\")\n",
    "plt.title(\"Original Source Signal\")\n",
    "plt.grid(True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9511c1bb-a255-45da-81e7-4dec51d1d7af",
   "metadata": {},
   "source": [
    "### Part 2: Compute Distances and Arrival Times\n",
    "\n",
    "Each microphone receives the signal after a delay determined by distance:\n",
    "\n",
    "$$t_i = \\frac{d_i}{c}$$\n",
    "\n",
    "where:\n",
    "\n",
    "- $t_i$ is the arrival time at microphone i,\n",
    "- $d_i$ is the distance from the source to microphone i,\n",
    "- $c$ is the speed of sound.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d61eaffe-9da5-435e-b7e8-dfec807be5f5",
   "metadata": {},
   "outputs": [],
   "source": [
    "distances = np.linalg.norm(mics - source_pos, axis=1)\n",
    "arrival_times = distances / c\n",
    "\n",
    "print(\"Microphone distances:\")\n",
    "print(distances)\n",
    "\n",
    "print(\"Arrival times:\")\n",
    "print(arrival_times)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "56986513-4aba-4eb8-a958-91d993bc186b",
   "metadata": {},
   "source": [
    "Because of sensor inaccuracies and artifacts, and because of environmental conditions, the signal might travel in a slightly different speed. Let's emulate that with some noise."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "30772512-cad0-43ce-b47b-ffaa729a50f8",
   "metadata": {},
   "outputs": [],
   "source": [
    "sigma_t = 1e-5  # seconds\n",
    "arrival_times_noisy = arrival_times + np.random.normal(\n",
    "    loc=0.0,\n",
    "    scale=sigma_t,\n",
    "    size=arrival_times.shape\n",
    ")\n",
    "\n",
    "print(\"True Arrival times:\")\n",
    "print(arrival_times)\n",
    "\n",
    "print(\"Noisy Arrival times:\")\n",
    "print(arrival_times_noisy)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dbaa8898-389b-4375-beb2-914c29396c69",
   "metadata": {},
   "source": [
    "### Part 3: Simulate Microphone Signals\n",
    "\n",
    "Each microphone signal is a delayed and attenuated version of the source signal.\n",
    "\n",
    "For simplifying implementation, we use integer-sample delays."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1158da25-0f63-4c13-8bfd-296bc7d9aa32",
   "metadata": {},
   "outputs": [],
   "source": [
    "def delay_signal_integer(x, delay_seconds, fs):\n",
    "    delay_samples = int(round(delay_seconds * fs))\n",
    "    return np.concatenate([np.zeros(delay_samples), x])\n",
    "\n",
    "mic_signals = []\n",
    "\n",
    "for distance, arrival_time in zip(distances, arrival_times_noisy):\n",
    "    delayed = delay_signal_integer(source_signal, arrival_time, fs)\n",
    "\n",
    "    # Simple distance-based attenuation\n",
    "    k = 10.\n",
    "    attenuated = delayed / (1. + k * distance**2)\n",
    "\n",
    "    mic_signals.append(attenuated)\n",
    "\n",
    "# Pad all signals to the same length\n",
    "max_len = max(len(s) for s in mic_signals)\n",
    "\n",
    "mic_signals = np.array([\n",
    "    np.pad(s, (0, max_len - len(s)))\n",
    "    for s in mic_signals\n",
    "])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "009a3516-9792-47e3-83ce-0cef109478d3",
   "metadata": {},
   "outputs": [],
   "source": [
    "time_axis = np.arange(max_len) / fs\n",
    "\n",
    "plt.figure(figsize=(10, 4))\n",
    "\n",
    "for i in range(num_mics):\n",
    "    plt.plot(time_axis, mic_signals[i], label=f\"Mic {i+1}\")\n",
    "\n",
    "plt.xlabel(\"Time [s]\")\n",
    "plt.ylabel(\"Amplitude\")\n",
    "plt.title(\"Clean Delayed Microphone Signals\")\n",
    "plt.legend()\n",
    "plt.grid(True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "877cb51c-a957-421d-b13e-0362f9d9427e",
   "metadata": {},
   "source": [
    "### Part 4: Add Noise\n",
    "\n",
    "Real microphone signals are noisy. We add two types of noise:\n",
    "\n",
    "- Broadband random noise.\n",
    "- Low-frequency hum or vibration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7ebbbdf4-226a-4d40-aa15-cb400827cc8b",
   "metadata": {},
   "outputs": [],
   "source": [
    "def add_white_noise(x, snr_db):\n",
    "    signal_power = np.mean(x**2)\n",
    "    noise_power = signal_power / (10 ** (snr_db / 10))\n",
    "    noise = np.sqrt(noise_power) * np.random.randn(*x.shape)\n",
    "    return x + noise\n",
    "\n",
    "snr_db = 5\n",
    "\n",
    "noisy_signals = np.array([\n",
    "    add_white_noise(mic_signals[i], snr_db)\n",
    "    for i in range(num_mics)\n",
    "])\n",
    "\n",
    "t_full = np.arange(max_len) / fs\n",
    "\n",
    "hum = 0.05 * np.sin(2 * np.pi * 50 * t_full)\n",
    "\n",
    "noisy_signals = noisy_signals + hum"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "78a9b6d6-db01-4e43-b866-22a9f17ca239",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=(10, 4))\n",
    "\n",
    "for i in range(num_mics):\n",
    "    plt.plot(time_axis, noisy_signals[i], label=f\"Mic {i+1}\")\n",
    "\n",
    "plt.xlabel(\"Time [s]\")\n",
    "plt.ylabel(\"Amplitude\")\n",
    "plt.title(\"Noisy Microphone Signals\")\n",
    "plt.legend()\n",
    "plt.grid(True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cf6dd776-04c9-4e5e-82ce-b282ab95a9a7",
   "metadata": {},
   "source": [
    "### Part 5: Filtering\n",
    "\n",
    "The source chirp occupies approximately the frequency range from 500 Hz to 5000 Hz.\n",
    "\n",
    "A suitable first filter is a bandpass filter.\n",
    "\n",
    "Example passband: 400 Hz to 6000 Hz.\n",
    "\n",
    "This should keep most of the chirp while removing unwanted low- and high-frequency components."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "66551e3f-7fdc-44ca-9f6b-b3277b7de1ae",
   "metadata": {},
   "outputs": [],
   "source": [
    "from scipy.signal import butter, filtfilt, freqz\n",
    "\n",
    "lowcut = 400\n",
    "highcut = 6000\n",
    "order = 4\n",
    "\n",
    "b_iir, a_iir = butter(\n",
    "    order,\n",
    "    [lowcut / (fs / 2), highcut / (fs / 2)],\n",
    "    btype=\"bandpass\"\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9a2a7541-5851-4184-af53-7a35bda79a48",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Apply a window to reduce spectral leakage\n",
    "for i in range(num_mics):\n",
    "    noisy_signals[i] = noisy_signals[i] * np.hanning(len(noisy_signals[i]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a58a52cf-9254-4d5b-a919-65baf5d6e825",
   "metadata": {},
   "outputs": [],
   "source": [
    "filtered_iir = np.array([\n",
    "    filtfilt(b_iir, a_iir, noisy_signals[i])\n",
    "    for i in range(num_mics)\n",
    "])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0b9a6e05-9d43-49a0-9ee4-9cac555d017e",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=(10, 4))\n",
    "\n",
    "for i in range(num_mics):\n",
    "    plt.plot(time_axis, filtered_iir[i], label=f\"Mic {i+1}\")\n",
    "\n",
    "plt.xlabel(\"Time [s]\")\n",
    "plt.ylabel(\"Amplitude\")\n",
    "plt.title(\"IIR-Filtered Microphone Signals\")\n",
    "plt.legend()\n",
    "plt.grid(True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5e73094b-0418-4981-a890-2f6e9610a64c",
   "metadata": {},
   "source": [
    "### Part 6: Estimate Time Delay Using Cross-Correlation\n",
    "\n",
    "To estimate the delay between two microphone signals, we use cross-correlation.\n",
    "\n",
    "If two signals are similar but shifted in time, the cross-correlation reaches its maximum at the shift that best aligns them."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a7773ab1-5680-442f-96ef-33176a693a1d",
   "metadata": {},
   "outputs": [],
   "source": [
    "from scipy.signal import correlate\n",
    "\n",
    "def estimate_delay(x_ref, x, fs):\n",
    "    corr = correlate(x, x_ref, mode=\"full\")\n",
    "    lags = np.arange(-len(x_ref) + 1, len(x))\n",
    "\n",
    "    best_lag = lags[np.argmax(corr)]\n",
    "    delay_seconds = best_lag / fs\n",
    "\n",
    "    return delay_seconds, corr, lags"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "978c92d4-b5a7-49b4-a2ba-4348f7d65604",
   "metadata": {},
   "outputs": [],
   "source": [
    "from itertools import combinations\n",
    "\n",
    "def estimate_all_pairwise_delays(signals, fs):\n",
    "    \"\"\"\n",
    "    Estimate pairwise TDOAs between all microphone pairs.\n",
    "\n",
    "    Returns a list of tuples:\n",
    "        (i, j, tau_ji, corr, lags)\n",
    "\n",
    "    where tau_ji means:\n",
    "        arrival_time_at_mic_j - arrival_time_at_mic_i\n",
    "    \"\"\"\n",
    "    pairwise_delays = []\n",
    "\n",
    "    num_mics = len(signals)\n",
    "\n",
    "    for i, j in combinations(range(num_mics), 2):\n",
    "        tau_ji, corr, lags = estimate_delay(signals[i], signals[j], fs)\n",
    "        pairwise_delays.append((i, j, tau_ji, corr, lags))\n",
    "\n",
    "    return pairwise_delays\n",
    "\n",
    "pairwise_delays = estimate_all_pairwise_delays(filtered_iir, fs)\n",
    "\n",
    "for i, j, tau_ji, _, _ in pairwise_delays:\n",
    "    print(f\"Estimated delay Mic {j+1} relative to Mic {i+1}: {tau_ji * 1000:.4f} ms\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "471f0aa1-6803-47f0-8500-264e7583127c",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=(8, 3))\n",
    "plt.plot(pairwise_delays[0][4] / fs, pairwise_delays[0][3]) # lags/corr\n",
    "plt.xlim([-0.005, 0.005])\n",
    "plt.xlabel(\"Lag [s]\")\n",
    "plt.ylabel(\"Correlation\")\n",
    "plt.title(\"Cross-Correlation: Mic 2 vs Mic 1\")\n",
    "plt.grid(True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "03ab83fc-7ce9-48ca-98c0-9b22d88ab38d",
   "metadata": {},
   "source": [
    "### Part 7: Localize the Sound Source\n",
    "\n",
    "#### Goal\n",
    "\n",
    "After estimating the time delays between microphone signals, we want to estimate the position of the sound source.\n",
    "\n",
    "The unknown source position is:\n",
    "\n",
    "$$\n",
    "p =\n",
    "\\begin{bmatrix}\n",
    "x \\\\\n",
    "y\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "where \\(x\\) and \\(y\\) are the coordinates of the source.\n",
    "\n",
    "We assume that the microphone positions are known:\n",
    "\n",
    "$$\n",
    "m_i =\n",
    "\\begin{bmatrix}\n",
    "x_i \\\\\n",
    "y_i\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "and that the speed of sound is known:\n",
    "\n",
    "$$\n",
    "c = 343 \\text{ m/s}\n",
    "$$\n",
    "\n",
    "#### Distance from Source to Each Microphone\n",
    "\n",
    "If the source is at position \\(p\\), the distance from the source to microphone \\(i\\) is:\n",
    "\n",
    "$$\n",
    "d_i(p) = \\|p - m_i\\|\n",
    "$$\n",
    "\n",
    "or explicitly:\n",
    "\n",
    "$$\n",
    "d_i(p) =\n",
    "\\sqrt{(x - x_i)^2 + (y - y_i)^2}\n",
    "$$\n",
    "\n",
    "The corresponding arrival time at microphone \\(i\\) is:\n",
    "\n",
    "$$\n",
    "t_i(p) = \\frac{d_i(p)}{c}\n",
    "$$\n",
    "\n",
    "#### Time Difference of Arrival\n",
    "\n",
    "In practice, we usually do not know the absolute emission time of the sound.\n",
    "\n",
    "Instead, we estimate **time differences of arrival**, or **TDOAs**.\n",
    "\n",
    "For two microphones \\(i\\) and \\(j\\), the predicted TDOA is:\n",
    "\n",
    "$$\n",
    "\\hat{\\tau}_{ji}(p)\n",
    "=\n",
    "t_j(p) - t_i(p)\n",
    "$$\n",
    "\n",
    "Substituting the distance model:\n",
    "\n",
    "$$\n",
    "\\hat{\\tau}_{ji}(p)\n",
    "=\n",
    "\\frac{d_j(p) - d_i(p)}{c}\n",
    "$$\n",
    "\n",
    "or:\n",
    "\n",
    "$$\n",
    "\\hat{\\tau}_{ji}(p)\n",
    "=\n",
    "\\frac{\\|p - m_j\\| - \\|p - m_i\\|}{c}\n",
    "$$\n",
    "\n",
    "This equation predicts how much later the sound should arrive at microphone \\(j\\) compared to microphone \\(i\\), assuming the source is at position \\(p\\).\n",
    "\n",
    "#### Measured TDOAs\n",
    "\n",
    "From the microphone signals, we estimate delays using cross-correlation.\n",
    "\n",
    "For example:\n",
    "\n",
    "```python\n",
    "tau_ji, corr, lags = estimate_delay(signal_i, signal_j, fs)\n",
    "```\n",
    "\n",
    "Here:\n",
    "\n",
    "```text\n",
    "tau_ji = measured delay of microphone j relative to microphone i\n",
    "```\n",
    "\n",
    "So the measured TDOA is:\n",
    "\n",
    "$$\n",
    "\\tau_{ji}^{\\text{measured}}\n",
    "$$\n",
    "\n",
    "The localization problem is to find the source position \\(p\\) such that:\n",
    "\n",
    "$$\n",
    "\\hat{\\tau}_{ji}(p)\n",
    "\\approx\n",
    "\\tau_{ji}^{\\text{measured}}\n",
    "$$\n",
    "\n",
    "for all microphone pairs.\n",
    "\n",
    "#### Residual Error for One Microphone Pair\n",
    "\n",
    "For one pair of microphones \\(i\\) and \\(j\\), define the residual:\n",
    "\n",
    "$$\n",
    "r_{ji}(p)\n",
    "=\n",
    "\\hat{\\tau}_{ji}(p)\n",
    "-\n",
    "\\tau_{ji}^{\\text{measured}}\n",
    "$$\n",
    "\n",
    "Substituting the predicted TDOA:\n",
    "\n",
    "$$\n",
    "r_{ji}(p)\n",
    "=\n",
    "\\frac{\\|p - m_j\\| - \\|p - m_i\\|}{c}\n",
    "-\n",
    "\\tau_{ji}^{\\text{measured}}\n",
    "$$\n",
    "\n",
    "If the guessed source position \\(p\\) is correct, this residual should be close to zero.\n",
    "\n",
    "#### Residual Vector for Multiple Pairs\n",
    "\n",
    "With four microphones, we can use all pairwise TDOAs:\n",
    "\n",
    "```text\n",
    "Mic 2 - Mic 1\n",
    "Mic 3 - Mic 1\n",
    "Mic 4 - Mic 1\n",
    "Mic 3 - Mic 2\n",
    "Mic 4 - Mic 2\n",
    "Mic 4 - Mic 3\n",
    "```\n",
    "\n",
    "This gives a vector of residuals:\n",
    "\n",
    "$$\n",
    "r(p)\n",
    "=\n",
    "\\begin{bmatrix}\n",
    "r_{21}(p) \\\\\n",
    "r_{31}(p) \\\\\n",
    "r_{41}(p) \\\\\n",
    "r_{32}(p) \\\\\n",
    "r_{42}(p) \\\\\n",
    "r_{43}(p)\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Each entry measures how far the predicted delay is from the measured delay.\n",
    "\n",
    "#### Least-Squares Problem\n",
    "\n",
    "We estimate the source position by minimizing the sum of squared residuals:\n",
    "\n",
    "$$\n",
    "\\hat{p}\n",
    "=\n",
    "\\arg\\min_p\n",
    "\\sum_{(i,j)}\n",
    "\\left(\n",
    "\\frac{\\|p - m_j\\| - \\|p - m_i\\|}{c}\n",
    "-\n",
    "\\tau_{ji}^{\\text{measured}}\n",
    "\\right)^2\n",
    "$$\n",
    "\n",
    "Equivalently:\n",
    "\n",
    "$$\n",
    "\\hat{p}\n",
    "=\n",
    "\\arg\\min_p\n",
    "\\|r(p)\\|^2\n",
    "$$\n",
    "\n",
    "This is a **nonlinear least-squares problem**, because the residuals contain Euclidean distances involving square roots.\n",
    "\n",
    "#### Why Least Squares?\n",
    "\n",
    "The measured delays are not perfect. They are affected by:\n",
    "\n",
    "- noise,\n",
    "- finite sampling rate,\n",
    "- filtering distortion,\n",
    "- reverberation,\n",
    "- microphone placement errors,\n",
    "- cross-correlation errors.\n",
    "\n",
    "Therefore, the equations may not all agree perfectly.\n",
    "\n",
    "Least squares finds the source position that gives the best overall compromise."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2e5e8ad7-b4dc-4020-89fc-5aa7d34295f3",
   "metadata": {},
   "outputs": [],
   "source": [
    "from scipy.optimize import least_squares\n",
    "\n",
    "def localization_residuals_all_pairs(p, mics, pairwise_delays, c):\n",
    "    distances = np.linalg.norm(mics - p, axis=1)\n",
    "\n",
    "    residuals = []\n",
    "\n",
    "    for i, j, measured_tau_ji, _, _ in pairwise_delays:\n",
    "        predicted_tau_ji = (distances[j] - distances[i]) / c\n",
    "        residuals.append(predicted_tau_ji - measured_tau_ji)\n",
    "\n",
    "    return np.array(residuals)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c018aafc-9303-4ab8-b5b3-7b1032d8b489",
   "metadata": {},
   "outputs": [],
   "source": [
    "result = least_squares(\n",
    "    localization_residuals_all_pairs,\n",
    "    x0=np.array([0.5, 0.5]),\n",
    "    args=(mics, pairwise_delays, c)\n",
    ")\n",
    "\n",
    "estimated_pos = result.x\n",
    "\n",
    "print(\"True source position:\", source_pos)\n",
    "print(\"Estimated source position:\", estimated_pos)\n",
    "print(f\"Position error: {np.linalg.norm(estimated_pos - source_pos) * 1000.0:.2f} mm\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7e04b51b-202f-463b-9d9a-c02007399b59",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=(5, 5))\n",
    "\n",
    "plt.scatter(mics[:, 0], mics[:, 1], s=100, label=\"Microphones\")\n",
    "plt.scatter(source_pos[0], source_pos[1], marker=\"x\", s=120, label=\"True source\")\n",
    "plt.scatter(estimated_pos[0], estimated_pos[1], marker=\"o\", s=120, label=\"Estimated source\")\n",
    "\n",
    "for i, mic in enumerate(mics):\n",
    "    plt.text(mic[0] + 0.02, mic[1] + 0.02, f\"Mic {i+1}\")\n",
    "\n",
    "plt.xlabel(\"x [m]\")\n",
    "plt.ylabel(\"y [m]\")\n",
    "plt.title(\"Sound Source Localization\")\n",
    "plt.axis(\"equal\")\n",
    "plt.legend()\n",
    "plt.grid(True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4e0a56d1-d635-4462-bb7b-fb4e27d3434e",
   "metadata": {},
   "source": [
    "#### Interpreting the Result\n",
    "\n",
    "The solver starts from an initial guess and iteratively updates the source position.\n",
    "\n",
    "At each iteration, it asks:\n",
    "\n",
    "```text\n",
    "If the source were at this position, what delays would I predict?\n",
    "How different are those predicted delays from the measured delays?\n",
    "How should I move the source estimate to reduce that difference?\n",
    "```\n",
    "\n",
    "The final estimate is the position where the residuals are as small as possible.\n",
    "\n",
    "#### Geometric Interpretation\n",
    "\n",
    "Each TDOA measurement defines a curve of possible source locations.\n",
    "\n",
    "For a pair of microphones \\(i\\) and \\(j\\):\n",
    "\n",
    "$$\n",
    "\\|p - m_j\\| - \\|p - m_i\\|\n",
    "=\n",
    "c \\tau_{ji}^{\\text{measured}}\n",
    "$$\n",
    "\n",
    "This is a branch of a hyperbola.\n",
    "\n",
    "So each microphone pair gives one hyperbola.\n",
    "\n",
    "The source should lie near the intersection of all these hyperbolas.\n",
    "\n",
    "With noise, the hyperbolas may not intersect at exactly one point. Least squares finds the point that best fits all of them."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.14.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}