excercise1B1.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "source": [
    "# Excercise 1B.1"
   ],
   "metadata": {}
  },
  {
   "cell_type": "markdown",
   "source": [
    "## Imports, constants\n",
    "Import numpy, set constant light speed."
   ],
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 96,
   "source": [
    "import numpy as np\n",
    "\n",
    "LIGHT_SPEED = 299792.458"
   ],
   "outputs": [],
   "metadata": {}
  },
  {
   "cell_type": "markdown",
   "source": [
    "## Calculate Jacobian\n",
    "This function calculates the Jacobian of system (2). \n",
    "\n",
    "It needs the ```location```, where you estimated the person could be, and the estimated time difference. This is needed in a vector s.t. \n",
    "$\\begin{bmatrix} x \\\\ y \\\\ z \\\\ D \\end{bmatrix}$.\n",
    "\n",
    "The ```positions``` of the satellites, in a ```numpy``` array (matrix) s.t.:\n",
    "$$\n",
    "\\begin{bmatrix} \n",
    "a_1 & b_1 & c_1 \\\\\n",
    "a_2 & b_2 & c_2 \\\\\n",
    "a_3 & b_3 & c_3 \\\\\n",
    "a_4 & b_4 & c_4 \\\\\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "The ```transmission_times```, between the receiver and the satellites in a ```numpy``` array s.t.:\n",
    "$\\begin{bmatrix} t_1 \\\\ t_2 \\\\ t_3 \\\\ t_4 \\end{bmatrix}$"
   ],
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 97,
   "source": [
    "def calculate_jacobian(\n",
    "    location: np.ndarray, \n",
    "    positions: np.ndarray, \n",
    "    transmission_times: np.ndarray\n",
    "):\n",
    "    jacobian = np.zeros((4,4))\n",
    "    \n",
    "    location_x = location[0,0]\n",
    "    location_y = location[1,0]\n",
    "    location_z = location[2,0]\n",
    "    time_difference = location[3,0]\n",
    "\n",
    "    jacobian[:,0] = 2 * (location_x - positions[:, 0])\n",
    "    jacobian[:,1] = 2 * (location_y - positions[:, 1])\n",
    "    jacobian[:,2] = 2 * (location_z - positions[:, 2])\n",
    "    jacobian[:,3] = 2 * (LIGHT_SPEED**2) * transmission_times - 2 * (LIGHT_SPEED**2) * time_difference\n",
    " \n",
    "    return jacobian"
   ],
   "outputs": [],
   "metadata": {}
  },
  {
   "cell_type": "markdown",
   "source": [
    "## Calculate System\n",
    "This function calculates the values of the vector below.\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "(x - a_1)^2 + (y - b_1)^2 + (z - c_1)^2 - (C(t_1 - D))^2 \\\\ \n",
    "(x - a_2)^2 + (y - b_2)^2 + (z - c_2)^2 - (C(t_2 - D))^2 \\\\ \n",
    "(x - a_3)^2 + (y - b_3)^2 + (z - c_3)^2 - (C(t_3 - D))^2 \\\\ \n",
    "(x - a_4)^2 + (y - b_4)^2 + (z - c_4)^2 - (C(t_4 - D))^2\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "It needs the ```location```, where you estimated the person could be, and the estimated time difference. This is needed in a ```numpy``` array (vector) s.t. \n",
    "$\\begin{bmatrix} x \\\\ y \\\\ z \\\\ D \\end{bmatrix}$.\n",
    "\n",
    "The ```positions``` of the satellites, in a ```numpy``` array (matrix) s.t.:\n",
    "$$\n",
    "\\begin{bmatrix} \n",
    "a_1 & b_1 & c_1 \\\\\n",
    "a_2 & b_2 & c_2 \\\\\n",
    "a_3 & b_3 & c_3 \\\\\n",
    "a_4 & b_4 & c_4 \\\\\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "The ```transmission_times```, between the receiver and the satellites in a ```numpy``` array s.t.:\n",
    "$\\begin{bmatrix} t_1 \\\\ t_2 \\\\ t_3 \\\\ t_4 \\end{bmatrix}$"
   ],
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 98,
   "source": [
    "def functions(\n",
    "    location: np.ndarray, \n",
    "    positions: np.ndarray, \n",
    "    transmission_times: np.ndarray\n",
    "):\n",
    "    estimated_location_x = location[0,0]\n",
    "    estimated_location_y = location[1,0]\n",
    "    estimated_location_z = location[2,0]\n",
    "\n",
    "    time_difference = location[3,0]\n",
    "    result = np.zeros((4, 1))\n",
    "\n",
    "    result[:, 0] = (estimated_location_x - positions[:, 0])**2 + (estimated_location_y - positions[:, 1])**2 + (estimated_location_z - positions[:, 2])**2 - (LIGHT_SPEED * (transmission_times - time_difference))**2\n",
    "    return result"
   ],
   "outputs": [],
   "metadata": {}
  },
  {
   "cell_type": "markdown",
   "source": [
    "## Estimate location\n",
    "This function uses Newtons Multivariate Method, to estimate the location using the known locations and transmission time. \n",
    "\n",
    "The ```initial_estimation``` as a ```numpy``` array (vector) is needed.\n",
    "\n",
    "The ```positions``` of the satellites, in a ```numpy``` array (matrix) s.t.:\n",
    "$$\n",
    "\\begin{bmatrix} \n",
    "a_1 & b_1 & c_1 \\\\\n",
    "a_2 & b_2 & c_2 \\\\\n",
    "a_3 & b_3 & c_3 \\\\\n",
    "a_4 & b_4 & c_4 \\\\\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "The ```transmission_times```, between the receiver and the satellites in a ```numpy``` array s.t.:\n",
    "$\\begin{bmatrix} t_1 \\\\ t_2 \\\\ t_3 \\\\ t_4 \\end{bmatrix}$\n",
    "\n",
    "Also the maximum ```iterations``` are needed, as an ```int```."
   ],
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 99,
   "source": [
    "def estimate_location(\n",
    "    inital_estimation: np.ndarray,\n",
    "    positions: np.ndarray, \n",
    "    transmission_times: np.ndarray,\n",
    "    iterations: int,\n",
    "    minimum_step_size: float\n",
    "):\n",
    "    estimation: np.ndarray = inital_estimation\n",
    "    \n",
    "    for _ in range(iterations):\n",
    "        result = functions(estimation, positions, transmission_times)\n",
    "        jacobian = calculate_jacobian(estimation, positions, transmission_times)\n",
    "        H = np.linalg.solve(jacobian, result)\n",
    "        estimation = estimation - H\n",
    "\n",
    "        # Break if step_size is within minimum_step_size\n",
    "        if np.linalg.norm(H) < minimum_step_size:\n",
    "            break\n",
    "\n",
    "        # another break needed, but don't understand\n",
    "    \n",
    "    return estimation"
   ],
   "outputs": [],
   "metadata": {}
  },
  {
   "cell_type": "markdown",
   "source": [
    "## Main\n",
    "This runs our program using the values given."
   ],
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 100,
   "source": [
    "def main():\n",
    "    locations = np.array([\n",
    "        [15600, 7540, 20140], \n",
    "        [18760, 2750, 18610], \n",
    "        [17610, 14630, 13480], \n",
    "        [19170, 610, 18390]\n",
    "    ])\n",
    "\n",
    "    transmission_times = np.array([\n",
    "        0.07074,\n",
    "        0.07220,\n",
    "        0.07690,\n",
    "        0.07242\n",
    "    ])\n",
    "\n",
    "    initial_estimation = np.array([[0], [0], [6370], [0]])\n",
    "    max_iterations = 100\n",
    "    minimum_step_size = 0.0000000001 # 1 micrometer\n",
    "    x, y, z, time_difference = estimate_location(initial_estimation, locations, transmission_times, max_iterations, minimum_step_size).tolist()\n",
    "\n",
    "    print(f\"{x=}, {y=}, {z=}, {time_difference=}\")\n",
    "\n",
    "main()"
   ],
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "x=[-41.77270957082428], y=[-16.789194106523492], z=[6370.059559223344], time_difference=[-0.0032015658295941082]\n"
     ]
    }
   ],
   "metadata": {}
  }
 ],
 "metadata": {
  "orig_nbformat": 4,
  "language_info": {
   "name": "python",
   "version": "3.8.3",
   "mimetype": "text/x-python",
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "pygments_lexer": "ipython3",
   "nbconvert_exporter": "python",
   "file_extension": ".py"
  },
  "kernelspec": {
   "name": "python3",
   "display_name": "Python 3.8.3 64-bit ('base': conda)"
  },
  "interpreter": {
   "hash": "250e59cf0b7b5cec3664e2265f2ec6cf89f1074736b01d93c1f9583811222aad"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}