survey_es7_hybridmethod_platooning_differentpatterns_heights.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 = 2000\n",
    "\n",
    "### world parameters\n",
    "sim_lmb20 = 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",
    "sim_lmb50 = Simulation(weather        = \"clear\", \n",
    "                 temperature    = 298,   # [K], circuit temperature\n",
    "                 daynight       = \"day_indirectsun\", \n",
    "                 rxconfig       = \"optics/qrx_planoconvex.npz\", \n",
    "                 txconfig       = \"optics/tx_lambertian_50deg_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",
    "sim_pcwlmb = Simulation(weather        = \"clear\", \n",
    "                 temperature    = 298,   # [K], circuit temperature\n",
    "                 daynight       = \"day_indirectsun\", \n",
    "                 rxconfig       = \"optics/qrx_planoconvex.npz\", \n",
    "                 txconfig       = \"optics/tx_piecewiselambertian_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",
    "sim_real       = Simulation(weather        = \"clear\", \n",
    "                 temperature    = 298,   # [K], circuit temperature\n",
    "                 daynight       = \"day_indirectsun\", \n",
    "                 rxconfig       = \"optics/qrx_planoconvex.npz\", \n",
    "                 txconfig       = \"optics/tx_AgonLbSedan1_2W.npz\", \n",
    "                 istxlambertian = False, \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"
   ]
  },
  {
   "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(sim, z, iterations):\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 lane change trajectory similar to the PIMRC article\n",
    "    # starts with constant heading, sliding, then starts to level out with changing heading (a curved portion at the end on xy plane)\n",
    "    a = np.load('trajectories/generated_platooning.npz')\n",
    "    \n",
    "    # vehicle timing in simulations are sampled at 1kHz\n",
    "    trj_numsamples = 1000 # will correspond to 1s\n",
    "    \n",
    "    # simulation timing parameters, generate simulation clocks\n",
    "    # define a special \"vehicle clock\" to follow the trajectory\n",
    "    f_vehicle = 1e3;         # [Hz], trajectory simulation clock rate, fixed by trajectory generation\n",
    "    t_vehicle = 1/f_vehicle; # [s] , trajectory simulation clock period, fixed by trajectory generation\n",
    "    \n",
    "    t_sim_stop = t_vehicle*trj_numsamples\n",
    "    s_veh_clock = generate_simulation_clocks(t_sim_stop, sim.f_simulation, f_vehicle, gen_clocked_only=True)\n",
    "    \n",
    "    localization_decimation_rate = 10 # to get VLP rate = 100 Hz\n",
    "    localization_rate = 1/(t_vehicle*localization_decimation_rate)\n",
    "    localization_num_steps = int(trj_numsamples/localization_decimation_rate)\n",
    "    \n",
    "    s_adc_clock = generate_simulation_clocks(t_sim_stop, sim.f_simulation, sim.f_adc_clock, gen_clocked_only=True)\n",
    "    \n",
    "    # sample in normal rate\n",
    "    xRL_to_rxL = a['x_RL'][0:trj_numsamples]; yRL_to_rxL = a['y_RL'][0:trj_numsamples]; \n",
    "    xRR_to_rxL = a['x_RR'][0:trj_numsamples]; yRR_to_rxL = a['y_RR'][0:trj_numsamples]\n",
    "    yRL_to_rxR = a['y_RL'][0:trj_numsamples]; yRR_to_rxR = a['y_RR'][0:trj_numsamples]; \n",
    "    hdg  = a['hdg'][0:trj_numsamples];\n",
    "    xRL_to_rxR = a['x_RL'][0:trj_numsamples] - L_ego;\n",
    "    xRR_to_rxR = a['x_RR'][0:trj_numsamples] - L_ego;\n",
    "    \n",
    "    ### simulate received power\n",
    "    pwr_txL_to_rxL = received_power(xRL_to_rxL, yRL_to_rxL, z+np.zeros_like(yRL_to_rxL), hdg, sim)\n",
    "    pwr_txL_to_rxR = received_power(xRL_to_rxR, yRL_to_rxR, z+np.zeros_like(yRL_to_rxR), hdg, sim)\n",
    "    pwr_txR_to_rxL = received_power(xRR_to_rxL, yRR_to_rxL, z+np.zeros_like(yRR_to_rxL), hdg, sim)\n",
    "    pwr_txR_to_rxR = received_power(xRR_to_rxR, yRR_to_rxR, z+np.zeros_like(yRR_to_rxR), hdg, sim)\n",
    "    \n",
    "    shared_pwr_txL_to_rxL = quad_distribute_power(xRL_to_rxL, yRL_to_rxL, z, 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, z, 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, z, 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, z, 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",
    "    aoa_rxL_txL     = np.zeros((localization_num_steps)); aoa_rxR_txL     = np.zeros((localization_num_steps))\n",
    "    aoa_rxL_txR     = np.zeros((localization_num_steps)); aoa_rxR_txR     = np.zeros((localization_num_steps))\n",
    "    d_rxL_txL       = np.zeros((localization_num_steps)); d_rxR_txL       = np.zeros((localization_num_steps))\n",
    "    d_rxL_txR       = np.zeros((localization_num_steps)); d_rxR_txR       = np.zeros((localization_num_steps))\n",
    "    \n",
    "    cls_x_directbearing     = np.zeros((localization_num_steps, iterations)); \n",
    "    cls_y_directrange       = np.zeros((localization_num_steps, iterations));\n",
    "    \n",
    "    for j in tqdm(range(0,iterations)):\n",
    "        for i in range(0, localization_num_steps):\n",
    "            smp_lo = i*localization_decimation_rate\n",
    "            smp_hi = (i+1)*localization_decimation_rate\n",
    "    \n",
    "            step_time       = s_veh_clock[smp_lo:smp_hi]\n",
    "            simulation_time = s_adc_clock[int(i*sim.f_adc_clock/f_vehicle):int((i+1)*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[i] = measure_range_roberts(rxLA_txL + rxLB_txL + rxLC_txL + rxLD_txL, wav_rxL_txL, sim.lightspeed, sim.f_emitted)\n",
    "            d_rxR_txL[i] = 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[i] = measure_bearing(rxLA_txL, rxLB_txL, rxLC_txL, rxLD_txL, wav_rxL_txL, sim.rx_config_bundle[\"f_QRX\"], thd)\n",
    "            aoa_rxR_txL[i] = 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[i,j], _ = classicalfix_directbearing_mle(aoa_rxL_txL[i], aoa_rxR_txL[i], L_ego)\n",
    "            _, cls_y_directrange[i,j]   = classicalfix_directrange_mle(d_rxL_txL[i], d_rxR_txL[i], L_ego)\n",
    "\n",
    "    return cls_x_directbearing, cls_y_directrange, (localization_decimation_rate, localization_num_steps, \n",
    "                                                    xRL_to_rxL, yRL_to_rxL, hdg, s_veh_clock)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ac83e1ea-6657-437d-8fde-959f7b448afa",
   "metadata": {},
   "source": [
    "### Run simulation for different configurations"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1f762816-8bd6-4643-8b4c-1d0eaf87fbdd",
   "metadata": {},
   "source": [
    "different speeds, lambertian and z=0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "32dc1f24-f1c9-47c8-8912-83c6948db811",
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "  0%|                                                  | 0/2000 [00:00<?, ?it/s]/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",
      "/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",
      "100%|███████████████████████████████████████| 2000/2000 [15:24<00:00,  2.16it/s]\n",
      "100%|███████████████████████████████████████| 2000/2000 [14:55<00:00,  2.23it/s]\n",
      "100%|███████████████████████████████████████| 2000/2000 [14:57<00:00,  2.23it/s]\n",
      "  0%|                                                  | 0/2000 [00:00<?, ?it/s]/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",
      "/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",
      "100%|███████████████████████████████████████| 2000/2000 [14:46<00:00,  2.26it/s]\n",
      "100%|███████████████████████████████████████| 2000/2000 [14:46<00:00,  2.26it/s]\n",
      "100%|███████████████████████████████████████| 2000/2000 [14:45<00:00,  2.26it/s]\n",
      "100%|███████████████████████████████████████| 2000/2000 [14:53<00:00,  2.24it/s]\n",
      "100%|███████████████████████████████████████| 2000/2000 [14:54<00:00,  2.24it/s]\n"
     ]
    }
   ],
   "source": [
    "est_x_lmb20, est_y_lmb20, locpkg   = simrun(sim_lmb20, 0, iterations);\n",
    "est_x_lmb50, est_y_lmb50,_   = simrun(sim_lmb50, 0, iterations);\n",
    "est_x_pcwlmb, est_y_pcwlmb,_ = simrun(sim_pcwlmb, 0, iterations);\n",
    "est_x_real, est_y_real,_     = simrun(sim_real, 0, iterations);\n",
    "\n",
    "est_x_lmb20_z0p15, est_y_lmb20_z0p15,_ = simrun(sim_lmb20, 0.15, iterations);\n",
    "est_x_lmb20_z0p30, est_y_lmb20_z0p30,_ = simrun(sim_lmb20, 0.30, iterations);\n",
    "est_x_lmb20_z0p45, est_y_lmb20_z0p45,_ = simrun(sim_lmb20, 0.45, iterations);\n",
    "est_x_lmb20_z0p60, est_y_lmb20_z0p60,_ = simrun(sim_lmb20, 0.60, iterations);\n",
    "\n",
    "localization_decimation_rate = locpkg[0]\n",
    "localization_num_steps       = locpkg[1]\n",
    "xRL_to_rxL                   = locpkg[2]\n",
    "yRL_to_rxL                   = locpkg[3]\n",
    "hdg                          = locpkg[4]\n",
    "s_veh_clock                  = locpkg[5]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bb28cd39-ca70-4cc8-9455-86d37c80c319",
   "metadata": {},
   "source": [
    "### Save"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "c0a34b42-e4b8-411b-ae27-fb6c22aa2fac",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.savez(\"results/survey_es7.npz\", \n",
    "         iterations = iterations,\n",
    "         ldr = localization_decimation_rate,\n",
    "         lns = localization_num_steps,\n",
    "         xRL_to_rxL=xRL_to_rxL, yRL_to_rxL=yRL_to_rxL, hdg=hdg,\n",
    "         s_veh_clock = s_veh_clock,\n",
    "         est_x_lmb20 = est_x_lmb20,\n",
    "         est_y_lmb20 = est_y_lmb20,\n",
    "         est_x_lmb50 = est_x_lmb50,\n",
    "         est_y_lmb50 = est_y_lmb50,\n",
    "         est_x_pcwlmb = est_x_pcwlmb,\n",
    "         est_y_pcwlmb = est_y_pcwlmb,\n",
    "         est_x_real = est_x_real,\n",
    "         est_y_real = est_y_real,\n",
    "         est_x_lmb20_z0p15=est_x_lmb20_z0p15,\n",
    "         est_y_lmb20_z0p15=est_y_lmb20_z0p15,\n",
    "         est_x_lmb20_z0p30=est_x_lmb20_z0p30,\n",
    "         est_y_lmb20_z0p30=est_y_lmb20_z0p30,\n",
    "         est_x_lmb20_z0p45=est_x_lmb20_z0p45,\n",
    "         est_y_lmb20_z0p45=est_y_lmb20_z0p45,\n",
    "         est_x_lmb20_z0p60=est_x_lmb20_z0p60,\n",
    "         est_y_lmb20_z0p60=est_y_lmb20_z0p60)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "43254cd3-caa8-4bbd-84e5-351e580f18ff",
   "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
}