You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
299 lines
12 KiB
299 lines
12 KiB
import numpy as np
|
|
from e2e_agent_sem_lidar2shenron_package.ConfigureRadar import radar
|
|
import matplotlib.pyplot as plt
|
|
from matplotlib import cm
|
|
from mat4py import loadmat
|
|
import math
|
|
from mpl_toolkits.mplot3d import axes3d, Axes3D
|
|
import time
|
|
import scipy.io as sio
|
|
# from numba import jit
|
|
# from numba import int32, float64, complex128
|
|
import pdb
|
|
import torch
|
|
from pynvml import *
|
|
|
|
def get_gpu_id_most_avlbl_mem():
|
|
|
|
nvmlInit()
|
|
deviceCount = nvmlDeviceGetCount()
|
|
free = []
|
|
for i in range(deviceCount):
|
|
h = nvmlDeviceGetHandleByIndex(i)
|
|
info = nvmlDeviceGetMemoryInfo(h)
|
|
free.append(info.free)
|
|
free = np.array(free)
|
|
# h0 = nvmlDeviceGetHandleByIndex(0)
|
|
# h1 = nvmlDeviceGetHandleByIndex(1)
|
|
# h2 = nvmlDeviceGetHandleByIndex(2)
|
|
# h3 = nvmlDeviceGetHandleByIndex(3)
|
|
# info0 = nvmlDeviceGetMemoryInfo(h0)
|
|
# info1 = nvmlDeviceGetMemoryInfo(h1)
|
|
# info2 = nvmlDeviceGetMemoryInfo(h2)
|
|
# info3 = nvmlDeviceGetMemoryInfo(h3)
|
|
|
|
# free = np.array([info0.free,info1.free,info2.free,info3.free])
|
|
|
|
return np.argmax(free), np.max(free)
|
|
|
|
# @jit(nopython=True)
|
|
def heatmap_gen(rho, theta, loss, speed, radar, plot_fig, return_power):
|
|
start = time.time()
|
|
range_res = radar.c / (2 * radar.B)
|
|
max_range = range_res * radar.N_sample
|
|
|
|
Ts = 1 / radar.samp_rate
|
|
t = np.arange(0, radar.chirpT, Ts)
|
|
tau_resolution = 1 / radar.B
|
|
k = radar.B / radar.chirpT
|
|
x = np.exp(1j * 2 * np.pi * (radar.f + 0.5 * k * t) * t)
|
|
|
|
|
|
_lambda = radar.c / radar.f
|
|
sRx = _lambda / 2
|
|
_lambda = radar.c / radar.f
|
|
|
|
# Declare if we should use cpu or cuda
|
|
gpu_id, _ = get_gpu_id_most_avlbl_mem()
|
|
# pdb.set_trace()
|
|
device = torch.device(f'cuda:{gpu_id}' if torch.cuda.is_available() else 'cpu')
|
|
# device = torch.device('cpu')
|
|
print(f"------- Using {device}:{gpu_id} -------")
|
|
|
|
# beamforming_vector_constant = np.zeros((radar.nRx,rho.shape[0]))
|
|
delta = torch.zeros((radar.nRx,rho.shape[0]),device= device)
|
|
|
|
bpm_mode = bool(getattr(radar, 'bpm_mode', 0))
|
|
n_tx = int(getattr(radar, 'nTx', 1))
|
|
tx_positions = np.asarray(getattr(radar, 'tx_positions_m', np.array([0.0])), dtype=float)
|
|
if tx_positions.shape[0] != n_tx:
|
|
tx_positions = np.zeros((n_tx,), dtype=float)
|
|
|
|
|
|
##Noise
|
|
# noise_prop = loadmat('noise.mat')
|
|
# real_fft_ns = np.random.normal(noise_prop['noise_mean_real'], noise_prop['noise_std_real']).T
|
|
# complex_fft_ns = np.random.normal(noise_prop['noise_mean_real'], noise_prop['noise_std_real']).T
|
|
# final_noise = real_fft_ns + 1j * complex_fft_ns
|
|
# signal_Noisy = np.fft.ifft(final_noise, radar.N_sample, 1) * 10
|
|
signal_Noisy = radar.get_noise()
|
|
|
|
##initialising on torch
|
|
loss = torch.tensor(loss*(1 / rho ** 2), device=device).float()
|
|
rho = torch.tensor(rho, device=device).float()
|
|
signal_Noisy = torch.tensor(signal_Noisy, device=device)
|
|
theta = torch.tensor(theta, device=device).float()
|
|
speed = torch.tensor(speed, device=device).float()
|
|
t = torch.tensor(t, device=device).float()
|
|
|
|
torch.set_printoptions(precision=10)
|
|
if radar.doppler_mode:
|
|
measurement = np.zeros((radar.chirps, radar.nRx, radar.N_sample), dtype="complex128") # doppler,AoA,range
|
|
# start = time.time()
|
|
for chirp in range(radar.chirps):
|
|
tau = 2 * (rho / radar.c + chirp * (radar.chirp_rep) * speed / radar.c)
|
|
# tau = 2 * rho / radar.c + chirp * (12*27e-6) * speed / radar.c
|
|
# print(tau[torch.argmax(abs(speed))])
|
|
sin_term = torch.sin(np.pi / 2 - theta)
|
|
for i in range(radar.nRx):
|
|
delta[i,:] = i * sRx * sin_term / radar.c
|
|
|
|
# delta = torch.tensor(delta,device='cuda')
|
|
# tau = torch.tensor(tau,device='cuda')
|
|
# t = torch.tensor(t,device='cuda')
|
|
|
|
# beamforming_vector = np.exp(1j*2*np.pi*(radar.f*delta[:,:,None] - 0.5*k*delta[:,:,None]**2 - k*tau[None,:,None]*delta[:,:,None] + k*delta[:,:,None]@t[None,None,:])) #(80xN)
|
|
dechirped = torch.exp((1j * 2 * np.pi) *(radar.f * tau[:,None] + k * tau[:,None]@t[None,:] - 0.5 * k * tau[:,None]**2))
|
|
# loss_factor = (loss * (1 / rho ** 2))[:,None] ## from network
|
|
loss_factor = (torch.sqrt(loss)[:,None]) ## from network
|
|
|
|
signal_single_antenna = loss_factor*dechirped # (Nx256)
|
|
|
|
# BPM chirp coding for 2TX mode:
|
|
# even chirp: [+1, +1], odd chirp: [+1, -1]
|
|
if bpm_mode and n_tx == 2:
|
|
if chirp % 2 == 0:
|
|
bpm_codes = [1.0, 1.0]
|
|
else:
|
|
bpm_codes = [1.0, -1.0]
|
|
else:
|
|
bpm_codes = [1.0] * n_tx
|
|
|
|
signal = torch.zeros((radar.nRx, radar.N_sample), device=device, dtype=torch.complex64)
|
|
for tx_idx in range(n_tx):
|
|
tx_delay = torch.tensor(tx_positions[tx_idx], device=device, dtype=torch.float32) * sin_term / radar.c
|
|
tx_delta = delta + tx_delay[None, :]
|
|
beamforming_vector = torch.exp(
|
|
1j * 2 * np.pi * (
|
|
radar.f * tx_delta[:, :, None]
|
|
- 0.5 * k * tx_delta[:, :, None] ** 2
|
|
- k * tau[None, :, None] * tx_delta[:, :, None]
|
|
+ k * tx_delta[:, :, None] @ t[None, None, :]
|
|
)
|
|
)
|
|
tx_signal = beamforming_vector * signal_single_antenna[None, :, :]
|
|
tx_signal = torch.squeeze(torch.sum(tx_signal, 1))
|
|
signal = signal + bpm_codes[tx_idx] * tx_signal
|
|
|
|
# pdb.set_trace()
|
|
|
|
# (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * (
|
|
# np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1)))
|
|
|
|
# signal = np.exp(1j*2*np.pi * ())
|
|
|
|
# noise_real = (1j*1j*-1) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1)
|
|
# noise_complex = (1j) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1)
|
|
# noise = (noise_real+noise_complex) * radar.noise_amp
|
|
# signal_Noisy = signal
|
|
|
|
# sum_samp = sum_samp + signal_Noisy ## Here I am not adding individual noise for each point
|
|
|
|
|
|
|
|
# adc_sampled = np.sqrt(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3) * np.conj(signal_Noisy) * (x)
|
|
adc_sampled = torch.sqrt(torch.tensor(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3)) * signal
|
|
adc_sampled = adc_sampled + signal_Noisy
|
|
|
|
adc_sampled = adc_sampled.cpu().numpy()
|
|
|
|
|
|
|
|
measurement[chirp, :, :] = adc_sampled
|
|
# pdb.set_trace()
|
|
del rho
|
|
del loss
|
|
# del signal_noisy
|
|
del speed
|
|
del theta
|
|
del t
|
|
del dechirped
|
|
del signal
|
|
del signal_single_antenna
|
|
with torch.cuda.device(f'cuda:{gpu_id}'):
|
|
torch.cuda.empty_cache()
|
|
# Optionally decode BPM into virtual channels (2TX x nRx).
|
|
if bpm_mode and n_tx == 2 and bool(getattr(radar, 'bpm_decode', 0)):
|
|
n_pairs = radar.chirps // 2
|
|
even_chirps = measurement[0:2*n_pairs:2, :, :]
|
|
odd_chirps = measurement[1:2*n_pairs:2, :, :]
|
|
tx0 = 0.5 * (even_chirps + odd_chirps)
|
|
tx1 = 0.5 * (even_chirps - odd_chirps)
|
|
measurement = np.concatenate((tx0, tx1), axis=1)
|
|
|
|
# pdb.set_trace()
|
|
return measurement
|
|
# end = time.time()
|
|
# diction = {"doppler_cube": measurement}
|
|
# sio.savemat("E:/Radar_sim/simlator/git repo/Heatmap-sim/doppler_cube.mat", diction)
|
|
# RangeFFT = np.fft.fft(measurement, radar.N_sample, 2)
|
|
# AngleFFT = np.fft.fftshift(np.fft.fft(RangeFFT[0, :, :], radar.angle_fft_size, 0), 0)
|
|
# Doppler_data = np.fft.fftshift(np.fft.fft(np.fft.fftshift(np.fft.fft(RangeFFT, radar.angle_fft_size, 1), 1),
|
|
# radar.chirps, 0), 0)
|
|
# Doppler_heatmap = np.sum(np.arange(radar.chirps)[:, None, None] * np.abs(Doppler_data), axis=0) / np.sum(
|
|
# np.abs(Doppler_data), axis=0) - radar.chirps / 2
|
|
|
|
else:
|
|
tau = 2 * rho / radar.c
|
|
_lambda = radar.c / radar.f
|
|
sRx = _lambda / 2 # separation
|
|
_lambda = radar.c / radar.f
|
|
|
|
tau_vec = np.zeros((radar.nRx, rho.shape[0]))
|
|
for i in range(radar.nRx):
|
|
tau_vec[i, :] = tau + (i) * sRx * np.sin(np.pi / 2 - theta) / radar.c
|
|
|
|
sum_samp = np.zeros((radar.nRx, radar.N_sample), dtype="complex128")
|
|
for j in range(rho.shape[0]):
|
|
if (rho[j] != 0):
|
|
if return_power:
|
|
if rho[j] != 0:
|
|
signal = 10**10 * np.sqrt(loss[j]) * np.exp(
|
|
1j * 2 * np.pi * (radar.f + 0.5 * k * (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * (
|
|
np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1)))
|
|
else:
|
|
if rho[j] != 0:
|
|
signal = loss[j] * (1 / rho[j] ** 2) * np.exp(
|
|
1j * 2 * np.pi * (radar.f + 0.5 * k * (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * (
|
|
np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1)))
|
|
|
|
noise_real = (1j*1j*-1) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1)
|
|
noise_complex = (1j) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1)
|
|
noise = (noise_real+noise_complex) * radar.noise_amp
|
|
signal_Noisy = signal + noise
|
|
|
|
sum_samp = sum_samp + signal_Noisy*10**5
|
|
|
|
adc_sampled = np.sqrt(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3) * np.conj(sum_samp) * (x)
|
|
# RangeFFT = np.fft.fft(adc_sampled, radar.N_sample, 1)
|
|
|
|
# pwr_prof = 10*np.log10(np.sum(abs(RangeFFT)**2, 0)+1)
|
|
# plt.plot(radar.range_res*np.arange(radar.N_sample), pwr_prof)
|
|
# plt.axis([0, 256*radar.range_res, 70, 180])
|
|
# plt.show()
|
|
|
|
# AngleFFT = np.fft.fftshift(np.fft.fft(RangeFFT, radar.angle_fft_size, 0), 0)
|
|
|
|
# Doppler_heatmap = np.zeros(np.shape(AngleFFT))
|
|
|
|
return measurement
|
|
|
|
|
|
# print('runtime is ', end - start)
|
|
|
|
# d = 1
|
|
# sine_theta = -2 * np.arange(-radar.angle_fft_size / 2, (radar.angle_fft_size / 2) + 1) / radar.angle_fft_size / d
|
|
# # sine_theta = -2*np.arange(-radar.angle_fft_size/2,(radar.angle_fft_size/2)+1)/radar.angle_fft_size/d
|
|
# cos_theta = np.sqrt(1 - sine_theta ** 2)
|
|
# indices_1D = np.arange(0, radar.N_sample)
|
|
# [R_mat, sine_theta_mat] = np.meshgrid(indices_1D * range_res, sine_theta)
|
|
# [_, cos_theta_mat] = np.meshgrid(indices_1D, cos_theta)
|
|
|
|
# x_axis = R_mat * cos_theta_mat
|
|
# y_axis = R_mat * sine_theta_mat
|
|
# mag_data_static = abs(np.vstack(
|
|
# (AngleFFT, AngleFFT[255, :]))) # np.column_stack((abs(AngleFFT[indices_1D,:]),abs(AngleFFT[indices_1D,0])))
|
|
# mag_data_doppler = abs(np.vstack((Doppler_heatmap, Doppler_heatmap[255, :])))
|
|
# # doppler_cube = np.concatenate((Doppler_data, Doppler_data[:, 255, :][:, np.newaxis, :]), 1)
|
|
|
|
# mag_data_static = np.flipud(mag_data_static)
|
|
# mag_data_doppler = np.flipud(mag_data_doppler)
|
|
# # doppler_cube = np.flipud(doppler_cube)
|
|
|
|
# return x_axis, y_axis, mag_data_static, mag_data_doppler
|
|
|
|
|
|
if __name__ == '__main__':
|
|
radar = radar()
|
|
radar.chirps = 1
|
|
radar.center = np.array([0.0, 0.0]) # center of radar
|
|
radar.elv = np.array([0.0])
|
|
|
|
test_WI=1
|
|
if (test_WI):
|
|
# points = np.load("E:/Radar_sim/simlator/git repo/Heatmap-sim/simulation5.npy")
|
|
cell_array = sio.loadmat("wi_data/single_radar_wi_10m_allangles.mat")
|
|
|
|
for i in range(36):
|
|
print(f"Simulation: {i}")
|
|
points = cell_array['cell_array'][i][0]
|
|
if points.shape[0]==0:
|
|
print("Skipped")
|
|
continue
|
|
rho = np.linalg.norm(points[:, 0:3], axis=1)
|
|
theta = math.pi / 2 - np.arctan(((points[:, 0] - radar.center[0]) / (points[:, 1] - radar.center[1])))
|
|
loss = 10**(points[:, 3]/20)
|
|
speed = np.zeros_like(rho)
|
|
|
|
|
|
adc_data = heatmap_gen(rho, theta, loss, speed, radar, 1, 0)
|
|
diction = {"adc_data": adc_data}
|
|
sio.savemat(f"wi_data/simulation_{i}.mat", diction)
|
|
|
|
# fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
|
|
# ax.view_init(elev=90, azim=180)
|
|
# surf = ax.plot_surface(x_axis, y_axis, plot_data ** 0.7, cmap=cm.coolwarm,
|
|
# linewidth=0, antialiased=False)
|
|
# plt.show()
|
|
|
|
|