def test_cart2polar_and_polar2cart(self):
# loop test
expected = np.array([1,1,0,1,-1,0,-1,-1,0])
np.testing.assert_almost_equal(utils.polar2cart(utils.cart2polar(expected)), expected)
np.testing.assert_almost_equal(utils.cart2polar(utils.polar2cart(expected)), expected)
expected = np.array([1,np.pi/4,np.pi/4,4,5,6,7,8,9])
np.testing.assert_almost_equal(utils.polar2cart(utils.cart2polar(expected)), expected)
np.testing.assert_almost_equal(utils.cart2polar(utils.polar2cart(expected)), expected)
expected = np.array([1,np.pi*3/4,-np.pi/4,4,5,6,7,8,9])
np.testing.assert_almost_equal(utils.polar2cart(utils.cart2polar(expected)), expected)
np.testing.assert_almost_equal(utils.cart2polar(utils.polar2cart(expected)), expected)
# approach target
expected_car = np.array([1,1,0,-1,-1,0,0,0,0])
expected_pol = np.array([np.sqrt(2),np.pi/4,0,-np.sqrt(2),0,0,0,0,0])
np.testing.assert_almost_equal(utils.cart2polar(expected_car), expected_pol)
np.testing.assert_almost_equal(utils.polar2cart(expected_pol), expected_car)
# horizontal turn target
expected_car = np.array([1,1,0,-1,1,0,-1,-1,0])
expected_pol = np.array([np.sqrt(2),np.pi/4,0,0,1,0,0,0,0])
np.testing.assert_almost_equal(utils.cart2polar(expected_car), expected_pol)
np.testing.assert_almost_equal(utils.polar2cart(expected_pol), expected_car)
# vertical turn target
expected_car = np.array([1,0,-1,-1,0,-1,-1,0,+1])
expected_pol = np.array([np.sqrt(2),0,np.pi/4,0,0,1,0,0,0])
np.testing.assert_almost_equal(utils.cart2polar(expected_car), expected_pol)
np.testing.assert_almost_equal(utils.polar2cart(expected_pol), expected_car)
def gen_dirty_img(visi, pos_mic_x, pos_mic_y, omega_band, sound_speed,
phi_plt):
"""
Compute the dirty image associated with the given measurements. Here the Fourier transform
that is not measured by the microphone array is taken as zero.
:param visi: the measured visibilites
:param pos_mic_x: a vector contains microphone array locations (x-coordinates)
:param pos_mic_y: a vector contains microphone array locations (y-coordinates)
:param omega_band: mid-band (ANGULAR) frequency [radian/sec]
:param sound_speed: speed of sound
:param phi_plt: plotting grid (azimuth on the circle) to show the dirty image
:return:
"""
img = np.zeros(phi_plt.size, dtype=complex)
x_plt, y_plt = polar2cart(1, phi_plt)
num_mic = pos_mic_x.size
pos_mic_x_normalised = pos_mic_x / (sound_speed / omega_band)
pos_mic_y_normalised = pos_mic_y / (sound_speed / omega_band)
count_visi = 0
for q in xrange(num_mic):
p_x_outer = pos_mic_x_normalised[q]
p_y_outer = pos_mic_y_normalised[q]
for qp in xrange(num_mic):
if not q == qp:
p_x_qqp = p_x_outer - pos_mic_x_normalised[qp] # a scalar
p_y_qqp = p_y_outer - pos_mic_y_normalised[qp] # a scalar
# <= the negative sign converts DOA to propagation vector
img += visi[count_visi] * \
np.exp(-1j * (p_x_qqp * x_plt + p_y_qqp * y_plt))
count_visi += 1
return img / (num_mic * (num_mic - 1))
def compute_weights(self, direction=None):
if direction is not None:
self.direction = direction
phi = self.direction * np.pi / 180.
src = polar2cart(np.array([1, phi]))
# near-field
# dist_m = np.linalg.norm(self.L - np.tile(src, (self.M,1)).T,
# axis=0)
# dist_c = np.linalg.norm(self.center-src)
# dist_m = dist_m-dist_c
# far-field
dist_m = -np.dot(self.L.T, src)
dist_m = dist_m - dist_m.min()
for i, f in enumerate(self.frequencies):
wavenum = 2 * np.pi * f / self.c
# mode_vecs = 1./dist_m * np.exp(-1j*wavenum*dist_m) # near field model
mode_vecs = np.exp(-1j * wavenum * dist_m) # fair field model
w = np.dot(
H(mode_vecs),
np.linalg.pinv(self.Rhat[i, :, :] +
self.mu * np.diag(self.Rhat[i, :, :]) *
np.identity(self.M, dtype=complex)))
self.weights[i, :] = w / np.dot(w, mode_vecs)
def compute_mode(self, freq, phi):
X = polar2cart(np.array([1, phi]))
dist = np.linalg.norm(self.L - np.tile(X, (self.M, 1)).T, axis=0)
wavenum = 2 * np.pi * freq / self.c
return np.exp(1j * wavenum * dist)
def gen_sig_at_mic(sigmak2_k,
phi_k,
pos_mic_x,
pos_mic_y,
omega_band,
sound_speed,
SNR,
Ns=256):
"""
generate complex base-band signal received at microphones
:param sigmak2_k: the variance of the circulant complex Gaussian signal
emitted by the K sources
:param phi_k: source locations (azimuths)
:param pos_mic_x: a vector that contains microphones' x coordinates
:param pos_mic_y: a vector that contains microphones' y coordinates
:param omega_band: mid-band (ANGULAR) frequency [radian/sec]
:param sound_speed: speed of sound
:param SNR: SNR for the received signal at microphones
:param Ns: number of snapshots used to estimate the covariance matrix
:return: y_mic: received (complex) signal at microphones
"""
num_mic = pos_mic_x.size
xk, yk = polar2cart(1, phi_k) # source locations in cartesian coordinates
# reshape to use broadcasting
xk = np.reshape(xk, (1, -1), order='F')
yk = np.reshape(yk, (1, -1), order='F')
pos_mic_x = np.reshape(pos_mic_x, (-1, 1), order='F')
pos_mic_y = np.reshape(pos_mic_y, (-1, 1), order='F')
t = np.reshape(np.linspace(0, 10 * np.pi, num=Ns), (1, -1), order='F')
K = sigmak2_k.size
sigmak2_k = np.reshape(sigmak2_k, (-1, 1), order='F')
# x_tilde_k size: K x length_of_t
# circular complex Gaussian process
x_tilde_k = np.sqrt(sigmak2_k / 2.) * (np.random.randn(K, Ns) +
1j * np.random.randn(K, Ns))
y_mic = np.dot(
np.exp(-1j * (xk * pos_mic_x + yk * pos_mic_y) /
(sound_speed / omega_band)),
x_tilde_k * np.exp(1j * omega_band * t))
signal_energy = linalg.norm(y_mic, 'fro')**2
noise_energy = signal_energy / 10**(SNR * 0.1)
sigma2_noise = noise_energy / (Ns * num_mic)
noise = np.sqrt(sigma2_noise / 2.) * (np.random.randn(*y_mic.shape) +
1j * np.random.randn(*y_mic.shape))
y_mic_noisy = y_mic + noise
return y_mic_noisy, y_mic
def compute_weights(self, direction=None):
if direction is not None:
self.direction = direction
phi = self.direction*np.pi/180.
src = utils.polar2cart(np.array([1, phi]))
dist_m = np.linalg.norm(self.L - np.tile(src, (self.M,1)).T,
axis=0)
for i, f in enumerate(self.frequencies):
wavenum = 2*np.pi*f/self.c
mode_vecs = 1./dist_m * np.exp(-1j*wavenum*dist_m) # near field model
w = np.dot(H(mode_vecs),
np.linalg.pinv(self.Rhat[i,:,:]+0.1*np.identity(self.M,dtype=complex)))
self.weights[i,:] = w / np.dot(w,mode_vecs)
def gen_visibility(alphak, phi_k, pos_mic_x, pos_mic_y):
"""
generate visibility from the Dirac parameter and microphone array layout
:param alphak: Diracs' amplitudes
:param phi_k: azimuths
:param pos_mic_x: a vector that contains microphones' x coordinates
:param pos_mic_y: a vector that contains microphones' y coordinates
:return:
"""
xk, yk = polar2cart(1, phi_k)
num_mic = pos_mic_x.size
visi = np.zeros((num_mic, num_mic), dtype=complex)
for q in xrange(num_mic):
p_x_outer = pos_mic_x[q]
p_y_outer = pos_mic_y[q]
for qp in xrange(num_mic):
p_x_qqp = p_x_outer - pos_mic_x[qp] # a scalar
p_y_qqp = p_y_outer - pos_mic_y[qp] # a scalar
visi[qp, q] = np.dot(np.exp(-1j * (xk * p_x_qqp + yk * p_y_qqp)),
alphak)
return visi
def compute_weights(self, direction=None):
if direction is not None:
self.direction = direction
phi = self.direction * np.pi / 180.
src = polar2cart(np.array([1, phi]))
# near-field
# dist_m = np.linalg.norm(self.L - np.tile(src, (self.M,1)).T,
# axis=0)
# dist_c = np.linalg.norm(self.center-src)
# self.dist_cent = dist_m-dist_c
# far-field
dist_m = -np.dot(self.L.T, src)
self.dist_cent = dist_m - dist_m.max()
for i, f in enumerate(self.frequencies):
wavenum = 2 * np.pi * f / self.c
self.weights[i, :] = np.exp(
1j * wavenum * self.dist_cent) / self.L.shape[1]
def step(self, dt, save_state=True, integration_method='leapfrog'): if integration_method == 'leapfrog': """ Works great, see here why: https://introcs.cs.princeton.edu/java/94diffeq/ """ d = np.hypot(self.x, self.y) a = -constants.muE / d / d / d if self.utmdt is None or self.vtmdt is None: self.utmdt = self.u + a * self.x * -dt/2. self.vtmdt = self.v + a * self.y * -dt/2. ax = a * self.x ay = a * self.y utpdt = self.utmdt + ax * dt vtpdt = self.vtmdt + ay * dt self.x += utpdt * dt self.y += vtpdt * dt self.u = (self.utmdt + utpdt) / 2. self.v = (self.vtmdt + vtpdt) / 2. self.utmdt = utpdt self.vtmdt = vtpdt elif integration_method=='euler-r': """ Euler in polar coordinates Bad because of the many trigo computations """ r = np.hypot(self.x, self.y) _, theta = utils.cart2polar(self.u, self.v) rp = -np.sin(theta) * self.u + np.cos(theta) * self.v rp -= constants.muE / (r * r) * dt du, dv = utils.polar2cart(rp, np.pi/2.+theta) self.u += du self.v += dv self.x += self.u * dt self.y += self.v * dt elif integration_method == 'euler': """ Okay, but not great """ d = np.hypot(self.x, self.y) a = -constants.muE / d / d / d self.u += a * self.x * dt self.v += a * self.y * dt self.x += self.u * dt self.y += self.v * dt if save_state: self.save_state()
def plot(self, time_ticks=1, save=True, highlight_new_orbit=True):
"""
A lot of things here could be computed only once
"""
t = np.arange(len(self.target.speed_x)) * self.dt / self.chaser.P
vtgt = np.hypot(self.target.speed_x, self.target.speed_y) * 1e-3
vcha = np.hypot(self.chaser.speed_x, self.chaser.speed_y) * 1e-3
#################################
chaserspeed = np.array([self.chaser.speed_x, self.chaser.speed_y]).T
vch = np.hypot(chaserspeed[:,0], chaserspeed[:,1])# * 1e-3
chxy = np.array([self.chaser.trajectory_x, np.array(self.chaser.trajectory_y)])
r = u.unit_vector(chxy).T
aalpha = u.vangle_between(chaserspeed, r)
dxo = 3. * np.pi * (self.target.a - self.chaser.a)
v_bar = np.zeros_like(aalpha)
for jjk in range(len(chxy[0])):
try:
vb = (self.distances.trajectory_x[jjk-1] - self.distances.trajectory_x[jjk]) / (self.dt)
except:
vb = np.nan
if jjk == 0:
vb = np.nan
v_bar[jjk] = vb
vz_bar = vch * np.cos(aalpha) * 1e-3
#v_bar *= dxo / (np.nanmean((v_bar[:self.idch])) * 1e3 * self.chaser.P)
##############################
if self.chaser.hp == self.chaser.ha:
txtch = r"$\mathrm{%s:\,%d\,km\,alt.}$" % (self.chaser.name, self.chaser.hp)
else:
txtch = r"$\mathrm{%s}:\,%d\times%d\mathrm{\,km\,alt.}$" % (self.chaser.name, self.chaser.hp, self.chaser.ha)
txttgt = r"$\mathrm{%s:\,%d\,km\,alt.}$" % (self.target.name, self.target.hp)
nimg = 0
for ii in range(len(self.target.speed_x)-1):
if not ii % time_ticks == 0: continue
#if ii > len(self.target.speed_x): continue
fig = plt.figure(figsize=(18,10))
plt.subplots_adjust(wspace=0.)
plt.subplots_adjust(bottom=0.01)
plt.subplots_adjust(top=0.95)
plt.subplots_adjust(right=0.98)
plt.subplots_adjust(left=0.)
gs = gridspec.GridSpec(4, 3)
# This where we plot the Earth orbit at the right scale
ax0 = fig.add_subplot(gs[:2,0])
#ax0.plot([1,2,3,4,5], [10,5,10,5,10], 'r-')
ax0.set_aspect("equal")
circle1 = plt.Circle((0, 0), constants.radiusE, color='b', alpha=0.5)
ax0.add_artist(circle1)
ax0.plot(self.target.trajectory_x[:self.idtgt], -1 * np.array(self.target.trajectory_y[:self.idtgt]), c='k')
ax0.plot(self.chaser.trajectory_x[:self.idch], -1 * np.array(self.chaser.trajectory_y[:self.idch]), c='r')
ax0.scatter(self.target.trajectory_x[ii], -1 * np.array(self.target.trajectory_y[ii]), c='k')
ax0.scatter(self.chaser.trajectory_x[ii], -1 * np.array(self.chaser.trajectory_y[ii]), c='r')
ax0.set_xticks([])
ax0.set_yticks([])
ax0.axis('off')
ax1 = fig.add_subplot(gs[0,1:])
#t = np.arange(len(self.target.speed_x)) * self.dt / self.chaser.P
#vtgt = np.hypot(self.target.speed_x, self.target.speed_y) * 1e-3
#vcha = np.hypot(self.chaser.speed_x, self.chaser.speed_y) * 1e-3
ax1.plot(t, vtgt, c='k', label=self.target.name)
ax1.plot(t, vcha, c='r', label=self.chaser.name)
xmin1, xmax1 = ax1.get_xlim()
ymin1, ymax1 = ax1.get_ylim()
ax1.scatter(t[ii], vtgt[ii], c='k')
ax1.scatter(t[ii], vcha[ii], c='r')
#ax1.plot(t, vcha-vtgt, c='b', label=r"$\Delta v$")
ax1.legend(fontsize=self.txtsize - 2)
ax1.xaxis.set_ticklabels([])
ax1.set_ylabel(r"$v\,\mathrm{[km/s]}$")
ax1.grid()
ax1.set_xlim([xmin1, xmax1])
ax1.set_ylim([ymin1, ymax1])
ax2 = fig.add_subplot(gs[2:,0])
ax2.set_aspect("equal")
r, theta = u.cart2polar(self.target.trajectory_x, self.target.trajectory_y)
r -= constants.radiusE
x, y = u.polar2cart(r, theta)
ax2.plot(x[:self.idtgt] * 1e-3, -1e-3 * y[:self.idtgt], c='k')
ax2.scatter(x[ii] * 1e-3, -1e-3 * y[ii], c='k')
r, theta = u.cart2polar(self.chaser.trajectory_x, self.chaser.trajectory_y)
r -= constants.radiusE
x, y = u.polar2cart(r, theta)
lim2 = 1.15 * np.amax([self.chaser.ha, self.target.ha])
ax2.set_xlim([-lim2, lim2])
ax2.set_ylim([-lim2, lim2])
ax2.plot(x[:self.idtgt] * 1e-3, -1e-3 * y[:self.idtgt], c='r')
ax2.scatter(x[ii] * 1e-3, -1e-3 * y[ii], c='r')
ax2.set_xticklabels([])
ax2.set_yticks([])
ax2.axis('off')
ax3 = fig.add_subplot(gs[1, 1:])#, sharex=ax1)
ax3.set_xlabel("$\mathrm{Time\,[Orbits\,of\,chaser]}$")
ax3.plot(t, v_bar*1e3, c='gold', label=r"$\bar{V}$")
ax3.plot(t, vz_bar*1e3, c='g', label=r"$v_R$")
ymin3, ymax3 = ax3.get_ylim()
ax3.scatter(t[ii], v_bar[ii]*1e3, c='gold')
ax3.scatter(t[ii], vz_bar[ii]*1e3, c='g')
ax3.set_xlim([xmin1, xmax1])
ax3.set_ylim([ymin3, ymax3])
ax3.legend(fontsize=self.txtsize - 2)
ax3.grid()
ax3.axhline(0, c='k', ls='--')
ax3.set_ylabel(r"$\bar{V},\,v_R\,\mathrm{[m/s]}$")
ax4 = fig.add_subplot(gs[2:,1:])
ax4.spines['left'].set_position('zero')
ax4.spines['right'].set_color('none')
ax4.spines['bottom'].set_position('zero')
ax4.spines['top'].set_color('none')
# remove the ticks from the top and right edges
ax4.xaxis.set_ticks_position('bottom')
ax4.yaxis.set_ticks_position('left')
ax4.invert_xaxis()
ax4.invert_yaxis()
ax4.plot(-np.array(self.distances.trajectory_x), -np.array(self.distances.trajectory_y))
ax4.scatter(-np.array(self.distances.trajectory_x)[ii], -np.array(self.distances.trajectory_y)[ii])
# For correctly plotting the vbar axis
ax4.plot([0], [0], c='k')
xmin, xmax = ax4.get_xlim()
ymin, ymax = ax4.get_ylim()
if t[ii] > 1 and highlight_new_orbit:
ax4.axvline(-self.distances.trajectory_x[0], c='k', ls='--')
ax4.axvline(-self.distances.trajectory_x[int(np.ceil(self.chaser.P / self.dt))], c='k', ls='--')
x4t = -(self.distances.trajectory_x[0] + self.distances.trajectory_x[int(np.ceil(self.chaser.P / self.dt))])/2.
y4t = (ymax - ymin) * 0.5 + ymin
y4tt = (ymax - ymin) * 0.51 + ymin
ax4.plot([-self.distances.trajectory_x[0], -self.distances.trajectory_x[int(np.ceil(self.chaser.P / self.dt))]], [y4t, y4t], ls='--', c='k')
ax4.annotate(r'$1\,\mathrm{orbit\,}\Delta x = 3\pi\Delta a\approx %3.0f\,\mathrm{km}$' % (dxo/1e3), xy=(x4t, y4tt), ha='center')
xx = ((1. - xmax / (xmax - xmin)) * 1.005 * (xmax - xmin) + xmin)
yy = ((1. - ymax / (ymax - ymin)) * 1.015 * (ymax - ymin) + ymin)
ax4.annotate(r'$\bar{V}\,\mathrm{[km]}$', xy=((xmax - xmin) * 0.01 + xmin, yy))
ax4.annotate(r'$\bar{R}\,\mathrm{[km]}$', xy=(xx, (ymax - ymin) * 0.02 + ymin))
if self.ax4_xmin is None:
self.ax4_xmin = xmin
self.ax4_xmax = xmax
self.ax4_ymin = ymin
self.ax4_ymax = ymax
gs.update(wspace=0.05, hspace=0.3)
ax4.set_xlim([self.ax4_xmin, self.ax4_xmax])
ax4.set_ylim([self.ax4_ymin, self.ax4_ymax])
plt.suptitle(txtch + r"$\quad$--$\quad$" + txttgt + r"$\quad$--$\quad$" + r"Time {:03d} min".format(int(self.dt*ii/60.)))
if not os.path.exists(self.outdir):
os.mkdir(self.outdir)
if save:
fig.savefig(os.path.join(self.outdir, "img_{:05d}.png".format(nimg)), dpi=200)
plt.close()
else:
plt.show()
nimg += 1