survey_es2_allmethods_lanechange.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": "112fb9d8-0bbd-4a87-83b3-4b8ea1db64ab",
   "metadata": {},
   "source": [
    "### Set common simulation configurations (only one run for this ES)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "7921f1dd-ee0c-4a71-9cee-2f8b22bfcbc2",
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|█████████████████████| 1000/1000 [10:49<00:00,  1.54it/s]\n"
     ]
    }
   ],
   "source": [
    "iterations = 1000\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 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",
    "trj_numsamples = 1000 # will correspond to 1s of simulation since vehicle timing in simulations are sampled at 1kHz\n",
    "xRL_to_rxL     = np.concatenate((np.linspace(-1, 1, int(trj_numsamples/2)), np.linspace(1, 3, int(trj_numsamples/2))  ))\n",
    "curved_portion = np.linspace(1,3, int(trj_numsamples/2))**1.2\n",
    "yRL_to_rxL     = np.concatenate((np.linspace(5, 7, int(trj_numsamples/2)), curved_portion+6 ))\n",
    "\n",
    "ha  = 5 # start heading\n",
    "hh  = np.linspace(ha,4,int(trj_numsamples/2)) # curved portion has linearly changing heading\n",
    "hdg = np.concatenate( (np.ones((int(trj_numsamples/2)))*ha, hh) )\n",
    "\n",
    "xRR_to_rxL = np.concatenate( (xRL_to_rxL[0:int(trj_numsamples/2)] + L_target*np.cos(np.deg2rad(ha)) , \n",
    "                              xRL_to_rxL[int(trj_numsamples/2):] + L_target*np.cos(np.deg2rad(hh))) )\n",
    "\n",
    "yRR_to_rxL = np.concatenate( (yRL_to_rxL[0:int(trj_numsamples/2)] + L_target*np.sin(np.deg2rad(ha)) , \n",
    "                              yRL_to_rxL[int(trj_numsamples/2):] + L_target*np.sin(np.deg2rad(hh))) )\n",
    "\n",
    "xRL_to_rxR = xRL_to_rxL - L_ego\n",
    "yRL_to_rxR = yRL_to_rxL\n",
    "xRR_to_rxR = xRR_to_rxL - L_ego\n",
    "yRR_to_rxR = yRR_to_rxL\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",
    "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",
    "### 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",
    "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",
    "delaoa_rxLR_txL = np.zeros((localization_num_steps)); delaoa_rxLR_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",
    "deld_rxLR_txL   = np.zeros((localization_num_steps)); deld_rxLR_txR   = np.zeros((localization_num_steps))\n",
    "\n",
    "run_rxL_x_directbearing = np.zeros((localization_num_steps, iterations)); \n",
    "run_rxL_y_directbearing = np.zeros((localization_num_steps, iterations));\n",
    "run_rxL_x_directrange   = np.zeros((localization_num_steps, iterations)); \n",
    "run_rxL_y_directrange   = np.zeros((localization_num_steps, iterations));\n",
    "cls_x_directbearing     = np.zeros((localization_num_steps, iterations)); \n",
    "cls_y_directbearing     = np.zeros((localization_num_steps, iterations));\n",
    "cls_x_diffbearing       = np.zeros((localization_num_steps, iterations)); \n",
    "cls_y_diffbearing       = np.zeros((localization_num_steps, iterations));\n",
    "cls_x_directrange       = np.zeros((localization_num_steps, iterations)); \n",
    "cls_y_directrange       = np.zeros((localization_num_steps, iterations));\n",
    "cls_x_diffrange         = np.zeros((localization_num_steps, iterations)); \n",
    "cls_y_diffrange         = 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_rxL_txR = np.sin(2*np.pi*sim.f_emitted*(simulation_time + delayRL_sigTime))\n",
    "        wav_rxR_txL = np.sin(2*np.pi*sim.f_emitted*(simulation_time + delayLR_sigTime))\n",
    "        wav_rxR_txR = np.sin(2*np.pi*sim.f_emitted*(simulation_time + delayRR_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",
    "        d_rxL_txR[i] = measure_range_roberts(rxLA_txR + rxLB_txR + rxLC_txR + rxLD_txR, wav_rxL_txR, sim.lightspeed, sim.f_emitted)\n",
    "        d_rxR_txR[i] = measure_range_roberts(rxRA_txR + rxRB_txR + rxRC_txR + rxRD_txR, wav_rxR_txR, sim.lightspeed, sim.f_emitted)\n",
    "\n",
    "        deld_rxLR_txL[i] = d_rxL_txL[i] - d_rxR_txL[i]\n",
    "        deld_rxLR_txR[i] = d_rxL_txR[i] - d_rxR_txR[i]\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_rxL_txR = np.sin(2*np.pi*sim.f_emitted*(simulation_time - delayRL_sigTime))\n",
    "        wav_rxR_txL = np.sin(2*np.pi*sim.f_emitted*(simulation_time - delayLR_sigTime))\n",
    "        wav_rxR_txR = np.sin(2*np.pi*sim.f_emitted*(simulation_time - delayRR_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_rxL_txR[i] = measure_bearing(rxLA_txR, rxLB_txR, rxLC_txR, rxLD_txR, wav_rxL_txR, 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",
    "        aoa_rxR_txR[i] = measure_bearing(rxRA_txR, rxRB_txR, rxRC_txR, rxRD_txR, wav_rxR_txR, sim.rx_config_bundle[\"f_QRX\"], thd)\n",
    "\n",
    "        delaoa_rxLR_txL[i] = aoa_rxL_txL[i] - aoa_rxR_txL[i];\n",
    "        delaoa_rxLR_txR[i] = aoa_rxL_txR[i] - aoa_rxR_txR[i];\n",
    "        \n",
    "        cls_x_directbearing[i,j], cls_y_directbearing[i,j] = classicalfix_directbearing_mle(aoa_rxL_txL[i], aoa_rxR_txL[i], L_ego)\n",
    "        cls_x_diffbearing[i,j], cls_y_diffbearing[i,j]     = classicalfix_diffbearing_mle(delaoa_rxLR_txL[i], delaoa_rxLR_txR[i], L_ego)\n",
    "        cls_x_directrange[i,j], cls_y_directrange[i,j]     = classicalfix_directrange_mle(d_rxL_txL[i], d_rxR_txL[i], L_ego)\n",
    "        cls_x_diffrange[i,j], cls_y_diffrange[i,j]         = classicalfix_diffrange_mle(deld_rxLR_txL[i], deld_rxLR_txR[i], L_ego)\n",
    "        \n",
    "        if(np.mod(i,2)==1):\n",
    "            dv_ns_stdev = 0.00083 # tabulated sensor data, see references on the article\n",
    "            dv = np.sqrt((xRL_to_rxL[smp_hi-1] - xRL_to_rxL[smp_lo-1])**2 + (yRL_to_rxL[smp_hi-1] - yRL_to_rxL[smp_lo-1])**2) + \\\n",
    "                          dv_ns_stdev*np.random.randn(1);\n",
    "            av_ns_stdev = np.deg2rad(0.02) # tabulated sensor data, see references on the article\n",
    "            av = np.arctan2((xRL_to_rxL[smp_hi-1] - xRL_to_rxL[smp_lo-1]),(yRL_to_rxL[smp_hi-1] - yRL_to_rxL[smp_lo-1])) + \\\n",
    "                             av_ns_stdev*np.random.randn(1);\n",
    "\n",
    "            run_rxL_x_directbearing[i-1,j], run_rxL_y_directbearing[i-1,j], \\\n",
    "            run_rxL_x_directbearing[i,j], run_rxL_y_directbearing[i,j] = runningfix_directbearing_mle(aoa_rxL_txL[i-1], \n",
    "                                                                                                      aoa_rxL_txL[i], dv, av)\n",
    "            run_rxL_x_directrange[i-1,j], run_rxL_y_directrange[i-1,j], \\\n",
    "            run_rxL_x_directrange[i,j], run_rxL_y_directrange[i,j] = runningfix_directrange_mle(d_rxL_txL[i-1], \n",
    "                                                                                                d_rxL_txL[i], dv, av)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "026c97bd-ded9-40c6-9d36-e0618eeaa37b",
   "metadata": {},
   "source": [
    "### Save"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "292d07d6-933d-4704-a37e-234083cfc3ee",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.savez(\"results/survey_es2.npz\", \n",
    "         iterations = iterations,\n",
    "         localization_decimation_rate = localization_decimation_rate,\n",
    "         localization_num_steps = 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",
    "         cls_x_directbearing = cls_x_directbearing,\n",
    "         cls_y_directbearing = cls_y_directbearing,\n",
    "         cls_x_diffbearing = cls_x_diffbearing,\n",
    "         cls_y_diffbearing = cls_y_diffbearing,\n",
    "         cls_x_directrange = cls_x_directrange,\n",
    "         cls_y_directrange = cls_y_directrange,\n",
    "         cls_x_diffrange = cls_x_diffrange,\n",
    "         cls_y_diffrange = cls_y_diffrange,\n",
    "         run_rxL_x_directbearing = run_rxL_x_directbearing,\n",
    "         run_rxL_y_directbearing = run_rxL_y_directbearing,\n",
    "         run_rxL_x_directrange = run_rxL_x_directrange,\n",
    "         run_rxL_y_directrange = run_rxL_y_directrange)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3ed5c5b4-ef77-4f27-abb0-3d3fef65092f",
   "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
}