survey_es4_directmethods_hybridmethod_heatmap.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "cbd34c18",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy             as np\n",
    "import scipy             as sp\n",
    "import matplotlib.pyplot as plt\n",
    "import time\n",
    "from numba import njit\n",
    "\n",
    "from tqdm            import tqdm\n",
    "from scipy           import signal\n",
    "\n",
    "from propagation import received_power, quad_distribute_power, propagation_delay, gen_qrx_onlyclocked\n",
    "\n",
    "from parametermeasurement import generate_clocks, measure_range_roberts, measure_bearing\n",
    "\n",
    "from positioning import classicalfix_directbearing_mle, classicalfix_diffbearing_mle, \\\n",
    "                        classicalfix_directrange_mle, classicalfix_diffrange_mle, \\\n",
    "                        runningfix_directbearing_mle, runningfix_directrange_mle\n",
    "\n",
    "from simulation import Simulation, generate_simulation_clocks"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "33d47a27-f5aa-4a4a-a19f-30084b61f2b7",
   "metadata": {},
   "source": [
    "### Set common simulation configurations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "7921f1dd-ee0c-4a71-9cee-2f8b22bfcbc2",
   "metadata": {},
   "outputs": [],
   "source": [
    "iterations = 50\n",
    "\n",
    "### world parameters\n",
    "sim = Simulation(weather        = \"clear\", \n",
    "                 temperature    = 298,   # [K], circuit temperature\n",
    "                 daynight       = \"day_indirectsun\", \n",
    "                 rxconfig       = \"optics/qrx_planoconvex.npz\", \n",
    "                 txconfig       = \"optics/tx_lambertian_20deg_2W.npz\", \n",
    "                 istxlambertian = True, \n",
    "                 f_adc_clk      = 1.0e7,  # [Hz] ADC measurement clock freq\n",
    "                 f_e            = 1.0e6,  # [Hz] emitted wave freq, left TX\n",
    "                 f_sim          = 1.0e10) # [Hz] simulation master clock freq\n",
    "\n",
    "L_ego    = 1.6 # [m]\n",
    "L_target = 1.5 # [m] arbitrary selection, just to make it different than the ego. This is not known, so it's not used in the algo\n",
    "\n",
    "### build heatmap coordinates similar to TVT article\n",
    "x_num_points = 25\n",
    "xRL_to_rxL   = np.linspace(-3 + L_ego/2, 3 + L_ego/2, x_num_points) # SM4 from TVT\n",
    "y_num_points = int(np.round(x_num_points*2.333333))\n",
    "yRL_to_rxL   = np.linspace(1, 15.25, y_num_points) # SM4 from TVT\n",
    "xRL_to_rxR   = xRL_to_rxL - L_ego\n",
    "yRL_to_rxR   = yRL_to_rxL\n",
    "xRR_to_rxL   = xRL_to_rxL + L_target\n",
    "yRR_to_rxL   = yRL_to_rxL\n",
    "xRR_to_rxR   = xRL_to_rxL + L_target - L_ego\n",
    "yRR_to_rxR   = yRL_to_rxL\n",
    "hdg          = np.linspace(-5,5,5) \n",
    "\n",
    "# simulation for each heat map point takes 1 vlp cycle\n",
    "vlp_rate    = 100\n",
    "t_sim_stop  = 1/vlp_rate\n",
    "s_adc_clock = generate_simulation_clocks(t_sim_stop, sim.f_simulation, sim.f_adc_clock, gen_clocked_only=True)\n",
    "\n",
    "f_vehicle   = 1e3; # [Hz], trajectory simulation clock rate, fixed by trajectory generation\n",
    "s_veh_clock = generate_simulation_clocks(t_sim_stop, sim.f_simulation, f_vehicle, gen_clocked_only=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e67f678c-38e7-459c-9ae2-dcfdc3a77f15",
   "metadata": {},
   "source": [
    "### Define full simulation run as a function"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "9ef9647c-1e72-40b5-ad61-119686096e28",
   "metadata": {},
   "outputs": [],
   "source": [
    "def simrun(xRL_to_rxL, yRL_to_rxL, xRL_to_rxR, yRL_to_rxR, \n",
    "           xRR_to_rxL, yRR_to_rxL, xRR_to_rxR, yRR_to_rxR, hdg,\n",
    "           s_adc_clock, sim, iterations):\n",
    "    ### simulate received power\n",
    "    pwr_txL_to_rxL = received_power(xRL_to_rxL, yRL_to_rxL, np.zeros_like(yRL_to_rxL), hdg, sim)\n",
    "    pwr_txL_to_rxR = received_power(xRL_to_rxR, yRL_to_rxR, np.zeros_like(yRL_to_rxR), hdg, sim)\n",
    "    pwr_txR_to_rxL = received_power(xRR_to_rxL, yRR_to_rxL, np.zeros_like(yRR_to_rxL), hdg, sim)\n",
    "    pwr_txR_to_rxR = received_power(xRR_to_rxR, yRR_to_rxR, np.zeros_like(yRR_to_rxR), hdg, sim)\n",
    "    \n",
    "    shared_pwr_txL_to_rxL = quad_distribute_power(xRL_to_rxL, yRL_to_rxL, 0, sim.rx_config_bundle[\"f_QRX\"], pwr_txL_to_rxL)\n",
    "    shared_pwr_txL_to_rxR = quad_distribute_power(xRL_to_rxR, yRL_to_rxR, 0, sim.rx_config_bundle[\"f_QRX\"], pwr_txL_to_rxR)\n",
    "    shared_pwr_txR_to_rxL = quad_distribute_power(xRR_to_rxL, yRR_to_rxL, 0, sim.rx_config_bundle[\"f_QRX\"], pwr_txR_to_rxL)\n",
    "    shared_pwr_txR_to_rxR = quad_distribute_power(xRR_to_rxR, yRR_to_rxR, 0, sim.rx_config_bundle[\"f_QRX\"], pwr_txR_to_rxR)\n",
    "    \n",
    "    delay_txL_to_rxL = propagation_delay(xRL_to_rxL, yRL_to_rxL, sim.lightspeed)\n",
    "    delay_txL_to_rxR = propagation_delay(xRL_to_rxR, yRL_to_rxR, sim.lightspeed)\n",
    "    delay_txR_to_rxL = propagation_delay(xRR_to_rxL, yRR_to_rxL, sim.lightspeed)\n",
    "    delay_txR_to_rxR = propagation_delay(xRR_to_rxR, yRR_to_rxR, sim.lightspeed)\n",
    "    \n",
    "    ### run simulation over the list of points\n",
    "    cls_x_directbearing = np.zeros((iterations)); \n",
    "    cls_y_directbearing = np.zeros((iterations)); \n",
    "    cls_x_directrange   = np.zeros((iterations));\n",
    "    cls_y_directrange   = np.zeros((iterations));\n",
    "    \n",
    "    for j in range(0,iterations):\n",
    "        smp_lo = 0\n",
    "        smp_hi = int(f_vehicle/vlp_rate)\n",
    "        numticks = int(f_vehicle/vlp_rate)\n",
    "        step_time       = s_veh_clock[smp_lo:smp_hi] \n",
    "        simulation_time = s_adc_clock[0:int(sim.f_adc_clock/f_vehicle)]\n",
    "\n",
    "        rxLL, rxLR, rxRL, rxRR, delays = gen_qrx_onlyclocked(shared_pwr_txL_to_rxL, shared_pwr_txL_to_rxR, \n",
    "                                                             shared_pwr_txR_to_rxL, shared_pwr_txR_to_rxR,\n",
    "                                                             delay_txL_to_rxL, delay_txL_to_rxR, \n",
    "                                                             delay_txR_to_rxL, delay_txR_to_rxR, sim, step_time, \n",
    "                                                             simulation_time, smp_lo, smp_hi);\n",
    "        (rxLA_txL, rxLB_txL, rxLC_txL, rxLD_txL) = rxLL\n",
    "        (rxLA_txR, rxLB_txR, rxLC_txR, rxLD_txR) = rxLR\n",
    "        (rxRA_txL, rxRB_txL, rxRC_txL, rxRD_txL) = rxRL\n",
    "        (rxRA_txR, rxRB_txR, rxRC_txR, rxRD_txR) = rxRR \n",
    "        (delayLL_sigTime, delayLR_sigTime, delayRL_sigTime, delayRR_sigTime) = delays\n",
    "\n",
    "        del rxLL, rxLR, rxRL, rxRR, delays \n",
    "\n",
    "        # initial transmitted signal is known (mark the +, that's because the signal does a roundtrip\n",
    "        # which is 1 delay earlier than echo transmission from target vehicle)\n",
    "        wav_rxL_txL = np.sin(2*np.pi*sim.f_emitted*(simulation_time + delayLL_sigTime))\n",
    "        wav_rxR_txL = np.sin(2*np.pi*sim.f_emitted*(simulation_time + delayLR_sigTime))\n",
    "\n",
    "        d_rxL_txL = measure_range_roberts(rxLA_txL + rxLB_txL + rxLC_txL + rxLD_txL, wav_rxL_txL, sim.lightspeed, sim.f_emitted)\n",
    "        d_rxR_txL = measure_range_roberts(rxRA_txL + rxRB_txL + rxRC_txL + rxRD_txL, wav_rxR_txL, sim.lightspeed, sim.f_emitted)\n",
    "\n",
    "        thd = 1e-5; # just to avoid messing up the graphs when aoa detection is too bad.\n",
    "        # assume that the VLC subsystem correctly decodes the signal (mark that the delayed signal \n",
    "        # is decoded, so the delay isn't actually measured, it's implicitly present on the RX signal)\n",
    "        wav_rxL_txL = np.sin(2*np.pi*sim.f_emitted*(simulation_time - delayLL_sigTime))\n",
    "        wav_rxR_txL = np.sin(2*np.pi*sim.f_emitted*(simulation_time - delayLR_sigTime))\n",
    "        aoa_rxL_txL = measure_bearing(rxLA_txL, rxLB_txL, rxLC_txL, rxLD_txL, wav_rxL_txL, sim.rx_config_bundle[\"f_QRX\"], thd)\n",
    "        aoa_rxR_txL = measure_bearing(rxRA_txL, rxRB_txL, rxRC_txL, rxRD_txL, wav_rxR_txL, sim.rx_config_bundle[\"f_QRX\"], thd)\n",
    "\n",
    "        cls_x_directbearing[j], cls_y_directbearing[j] = classicalfix_directbearing_mle(aoa_rxL_txL, aoa_rxR_txL, L_ego)\n",
    "        cls_x_directrange[j], cls_y_directrange[j]     = classicalfix_directrange_mle(d_rxL_txL, d_rxR_txL, L_ego)\n",
    "\n",
    "    return cls_x_directbearing, cls_y_directbearing, cls_x_directrange, cls_y_directrange"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2e49d8af-2ec9-46f0-a355-e0b78d789f08",
   "metadata": {},
   "source": [
    "### Run simulation over the whole grid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "32dc1f24-f1c9-47c8-8912-83c6948db811",
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "  0%|                                  | 0/25 [00:00<?, ?it/s]/home/ubuntu/vehicular-vlp-simulations/positioning.py:102: RuntimeWarning: divide by zero encountered in scalar divide\n",
      "  est_x = L*(1 + np.sin(np.deg2rad(aoaR))*np.cos(np.deg2rad(aoaL))/np.sin(np.deg2rad(aoaL - aoaR)));\n",
      "/home/ubuntu/vehicular-vlp-simulations/positioning.py:103: RuntimeWarning: divide by zero encountered in scalar divide\n",
      "  est_y = L*np.cos(np.deg2rad(aoaR))*np.cos(np.deg2rad(aoaL))/np.sin(np.deg2rad(aoaL - aoaR));\n",
      "/home/ubuntu/vehicular-vlp-simulations/positioning.py:108: RuntimeWarning: invalid value encountered in sqrt\n",
      "  est_y = np.sqrt(dL**2-est_x**2)\n",
      "100%|█████████████████████████| 25/25 [27:26<00:00, 65.88s/it]\n"
     ]
    }
   ],
   "source": [
    "cls_x_directbearing_map = np.zeros((x_num_points, y_num_points, len(hdg), iterations))\n",
    "cls_y_directbearing_map = np.zeros((x_num_points, y_num_points, len(hdg), iterations))\n",
    "cls_x_directrange_map = np.zeros((x_num_points, y_num_points, len(hdg), iterations))\n",
    "cls_y_directrange_map = np.zeros((x_num_points, y_num_points, len(hdg), iterations))\n",
    "for idx_x, _ in enumerate(tqdm(xRL_to_rxL)):\n",
    "    for idx_y, _ in enumerate(yRL_to_rxL):\n",
    "        for idx_h, _ in enumerate(hdg):\n",
    "            xRL_to_rxL_rep = np.ones(( int(f_vehicle/vlp_rate) ))*xRL_to_rxL[idx_x]\n",
    "            yRL_to_rxL_rep = np.ones(( int(f_vehicle/vlp_rate) ))*yRL_to_rxL[idx_y]\n",
    "            xRL_to_rxR_rep = np.ones(( int(f_vehicle/vlp_rate) ))*xRL_to_rxR[idx_x]\n",
    "            yRL_to_rxR_rep = np.ones(( int(f_vehicle/vlp_rate) ))*yRL_to_rxR[idx_y]\n",
    "            xRR_to_rxL_rep = np.ones(( int(f_vehicle/vlp_rate) ))*xRR_to_rxL[idx_x]\n",
    "            yRR_to_rxL_rep = np.ones(( int(f_vehicle/vlp_rate) ))*yRR_to_rxL[idx_y]\n",
    "            xRR_to_rxR_rep = np.ones(( int(f_vehicle/vlp_rate) ))*xRR_to_rxR[idx_x]\n",
    "            yRR_to_rxR_rep = np.ones(( int(f_vehicle/vlp_rate) ))*yRR_to_rxR[idx_y]\n",
    "            hdg_rep        = np.ones(( int(f_vehicle/vlp_rate) ))*hdg[idx_h]\n",
    "            cls_x_directbearing_map[idx_x, idx_y, idx_h, :], cls_y_directbearing_map[idx_x, idx_y, idx_h, :], \\\n",
    "            cls_x_directrange_map[idx_x, idx_y, idx_h, :], cls_y_directrange_map[idx_x, idx_y, idx_h, :], \\\n",
    "            = simrun(xRL_to_rxL_rep, yRL_to_rxL_rep, xRL_to_rxR_rep, yRL_to_rxR_rep, \n",
    "                     xRR_to_rxL_rep, yRR_to_rxL_rep, xRR_to_rxR_rep, yRR_to_rxR_rep, hdg_rep,\n",
    "                     s_adc_clock, sim, iterations);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9ddd60ea-1d69-48a2-a25d-2776d62cbd17",
   "metadata": {},
   "source": [
    "### Save"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "d6a91770-3217-4370-9c98-ca7b49c4b647",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.savez(\"results/survey_es4.npz\",\n",
    "         iterations = iterations,\n",
    "         xRL_to_rxL=xRL_to_rxL, yRL_to_rxL=yRL_to_rxL, hdg=hdg,\n",
    "         cls_x_directbearing_map = cls_x_directbearing_map,\n",
    "         cls_y_directbearing_map = cls_y_directbearing_map,\n",
    "         cls_x_directrange_map = cls_x_directrange_map,\n",
    "         cls_y_directrange_map = cls_y_directrange_map)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fcb58749-1d98-49c1-afd8-800dc10dd6f0",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}