def rgb2hsv(red, green, blue):
# https://en.wikipedia.org/wiki/HSL_and_HSV#From_RGB
# red, green, blue in 0..1hue, sat and value all 0..1
# Returns hue, sat and value in 0..1
try:
length = len(red)
except Exception:
length = 1
# Having to create and insert is due to an unfortunate limitation in ulab
rgb = np.zeros((3, length), dtype=np.float)
try:
rgb[0] = red
rgb[1] = green
rgb[2] = blue
except Exception:
rgb[0] = red.flatten()
rgb[1] = green.flatten()
rgb[2] = blue.flatten()
# These indexing games are because we can't index with integer arrays in ulab
index = np.arange(length, dtype=np.uint16)
index2d = np.zeros((length, 3), dtype=np.uint16)
index2d[:, 0] = 0
index2d[:, 1] = 1
index2d[:, 2] = 2
brightest = np.numerical.argmax(rgb, axis=0)
isBrightest = index2d == brightest
value = rgb[isBrightest]
cc = value - np.min(rgb, axis=0)
hueSelect = np.array([0, 2, 4])
# Some unnecessary calculations, but still faster than a python loop
hueCalc = np.array([green - blue, blue - red, red - green])
hue = (hueSelect[index == brightest] + hueCalc[isBrightest] / cc) / 6
sat = np.where(value == 0, 0.0, cc / value)
return hue, sat, value
def tdoa_multilateration_from_multiple_segments_gauss_newton(
anchor_locations: list,
range_diffs: list,
init_soln: list,
max_iters: int = 10,
eta: float = 1E-3,
abs_tol: float = 1E-6):
# Gauss-Newton Iterations
soln = np.array(init_soln)
# Main iterations
for j in range(max_iters):
# ==== Calculate the LHS and RHS of Normal Equation: J'*J*d = -J'*res,
# where J is Jacobian, d is descent direction, res is residual ====== #
jTj = np.zeros((2, 2), dtype=np.float)
jTres = np.zeros((2, 1), dtype=np.float)
for locations, diffs in zip(anchor_locations, range_diffs):
if use_ulab:
m = locations.shape()[0]
else:
m = locations.shape[0]
denom_1 = np.linalg.norm(soln - locations[0])
for i in range(1, m):
denom_2 = np.linalg.norm(soln - locations[i])
res = (diffs[i - 1] - (denom_1 - denom_2))
del_res = np.array([[((soln[0] - locations[i][0]) / denom_2 -
(soln[0] - locations[0][0]) / denom_1)],
[((soln[1] - locations[i][1]) / denom_2 -
(soln[1] - locations[0][1]) / denom_1)]])
if use_ulab:
jTj = jTj + np.linalg.dot(del_res, del_res.transpose())
else:
jTj = jTj + np.dot(del_res, del_res.transpose())
jTres = jTres + (del_res * res)
# ===================== calculation ENDs here ======================================================= #
# Take next Step:
# Modification according to Levenberg-Marquardt suggestion
jTj = jTj + np.diag(np.diag(jTj) * eta)
eta = eta / 2.0
# Invert 2x2 matrix
denom = jTj[0][0] * jTj[1][1] - jTj[1][0] * jTj[0][1]
numer = np.array([[jTj[1][1], -jTj[0][1]], [-jTj[1][0], jTj[0][0]]])
if denom >= 1E-9:
inv_jTj = numer / denom
if use_ulab:
temp = np.linalg.dot(inv_jTj, jTres)
else:
temp = np.dot(inv_jTj, jTres)
del_soln = np.array([temp[0][0], temp[1][0], 0.0])
soln = soln - del_soln
if np.linalg.norm(del_soln) <= abs_tol:
break
else:
print("Can't invert the matrix!")
break
return soln
def main():
# This precomputes the color palette for maximum speed
# You could change it to compute the color palette of your choice
w = [wheel(i) for i in range(256)]
# This sets up the initial wave as a smooth gradient
u = ulab.zeros(num_pixels)
um = ulab.zeros(num_pixels)
f = ulab.zeros(num_pixels)
slope = ulab.linspace(0, 256, num=num_pixels)
th = 1
# the first time is always random (is that a contradiction?)
r = 0
while True:
# Some of the time, add a random new wave to the mix
# increase .15 to add waves more often
# decrease it to add waves less often
if r < .01:
ii = random.randrange(1, num_pixels - 1)
# increase 2 to make bigger waves
f[ii] = (random.random() - .5) * 2
# Here's where to change dx, dt, and c
# try .2, .02, 2 for relaxed
# try 1., .7, .2 for very busy / almost random
u, um = step(u, um, f, num_pixels, .1, .02, 1), u
v = u * 200000 + slope + th
for i, vi in enumerate(v):
# Scale up by an empirical value, rotate by th, and look up the color
pixels[i] = w[round(vi) % 256]
# Take away a portion of the energy of the waves so they don't get out
# of control
u = u * .99
# incrementing th causes the wheel to slowly cycle even if nothing else is happening
th = (th + .25) % 256
pixels.show()
# Clear out the old random value, if any
f[ii] = 0
# and get a new random value
r = random.random()
def __init__(self, rv_eci, earth_rotation_angle_offset, t_epoch,
ground_stations):
"""Initialize the propagator with the stuff we received from
the ground station, as well as pre-allocate other parameters.
Args:
rv_eci: state [r (km);v (km/s)] at t_epoch
earth_rotation_angle_offset: GMST at t_epoch (radian)
t_epoch: on board satellite time when propagator is initialized (s)
"""
# [r;v] in km, km/s
self.rv_eci = 1 * rv_eci
# current earth rotation angle
self.earth_rotation_angle_offset = earth_rotation_angle_offset
# initial time that propagator was started
self.t_epoch = t_epoch
# store ecef location
self.r_ecef = np.zeros(3)
# constants
self.earth = Earth()
# visible to ground station
self.visible = False
# visible to ground station (previous step)
self.old_visible = False
# ground stations
self.ground_stations = ground_stations
def diag(a: ulab.array) -> ulab.array:
assert isinstance(a, ulab.array), "Input should be of type ulab.array!"
m, n = a.shape()
itemsize = a.itemsize()
if itemsize == 1:
dtype = ulab.int8
elif itemsize == 2:
dtype = ulab.int16
elif itemsize == 8:
dtype = ulab.float
else:
raise Exception("Unknown datatype of the input array!")
if m <= 1 and n <= 1:
return a
elif (m == 1 and n > 1) or (m > 1 and n == 1):
r = m if m > n else n
b = ulab.eye(r, dtype=dtype)
for i in range(r):
b[i, i] = a[i]
return b
elif m == n:
b = ulab.zeros(m, dtype=dtype)
for i in range(m):
b[i] = a[i][i]
return b
else:
raise Exception("Input should be square matrix!")
def calc(self, xyz):
'''
Inverse kinematic model of leg, with angles theta(1;2;3)
L0: horizontal distance between the second joint (up down on hip)
and the end effector
D: absolute distance between the second joint (up down on
hip) and the end effector
Alpha: internal angle formed by the thigh and shin
Beta: Angle fored between end effector and horizontal about theta2
Gamma: Internal angle between thigh and end effector about theta2
'''
xyz = rotate_z(self.offset, xyz)
xyz = [xyz[0][0], xyz[1][0], xyz[2][0]]
Theta = u.zeros(3)
Theta[0] = m.atan2(xyz[1], xyz[0])
L0 = m.sqrt(xyz[0]**2 + xyz[1]**2) - self.L[0]
D = m.sqrt(L0**2 + xyz[2]**2)
Alpha = m.acos((-D**2 + self.L[1]**2 + self.L[2]**2) /
(2 * self.L[1] * self.L[2]))
Beta = m.atan2(xyz[2], L0)
Gamma = m.asin(self.L[2] * m.sin(Alpha) / D)
Theta[1] = Gamma + Beta
Theta[2] = m.pi + Alpha
for i in range(3):
if Theta[i] > m.pi:
Theta[i] = Theta[i] - 2 * pi
if Theta[i] < -m.pi:
Theta[i] = Theta[i] + 2 * pi
return Theta
def __init__(self, lengths, offsets):
self.num_legs = len(offsets)
self.angles = u.zeros((self.num_legs, 3))
self.legs = []
for offset in offsets:
self.legs.append(Leg_IK(lengths, offset))
def dynamics(x,Earth):
# generate the position and velocity
pos = x[0:3]
vel = x[3:6]
# first we find the acceleration from two body
#pos_norm = np.vector.sqrt(np.numerical.sum(pos ** 2))
pos_norm = norm(pos)
accel_twobody = -Earth.mu * \
(pos_norm ** -3) * pos
# now we find the J2 acceleration
accel_J2 = np.zeros(3)
accel_J2[0] = -3/2 * Earth.J2 * Earth.mu * Earth.R ** 2\
* pos_norm ** -5 \
* pos[0] * (1 - 5 * pos[2] ** 2 / pos_norm ** 2)
accel_J2[1] = -3/2 * Earth.J2 * Earth.mu * Earth.R ** 2\
* pos_norm ** -5 \
* pos[1] * (1 - 5 * pos[2] ** 2 / pos_norm ** 2)
accel_J2[2] = -3/2 * Earth.J2 * Earth.mu * Earth.R ** 2\
* pos_norm ** -5 \
* pos[2] * (3 - 5 * pos[2] ** 2 / pos_norm ** 2)
# add together accelerations
# accel = accel_twobody + accel_J2
accel = accel_twobody + accel_J2
return np.array([vel[0],vel[1],vel[2],accel[0],accel[1],accel[2]])
def create_grid(x):
N = x.shape[0]
z = np.zeros((N, N, 3))
z[:, :, 0] = x.reshape(-1, 1)
z[:, :, 1] = x
fast_grid = z.reshape(N * N, 3)
return fast_grid
def check_mask(db, mask=(1, 0, 1)):
out = np.zeros(db.shape[0],dtype=bool)
for idx, line in enumerate(db):
target, vector = line[0], line[1:]
if (mask == np.bitwise_and(mask, vector)).all():
if target == 1:
out[idx] = 1
return out
def __init__(self, palette):
self.palette = palette
ulab_palette = ulab.zeros((len(palette), 3))
for i in range(len(palette)):
rgb = int(palette[i])
ulab_palette[i, 2] = rgb & 0xff
ulab_palette[i, 1] = (rgb >> 8) & 0xff
ulab_palette[i, 0] = rgb >> 16
self.ulab_palette = ulab_palette
def main2():
N = 75
Nx = 2
Nu = 1
Qf = np.eye(Nx) * 30
Q = np.eye(Nx) * 0.01
R = np.eye(1) * 0.03
x0 = np.zeros(2)
xg = np.array([math.pi, 0])
utraj0 = np.zeros((Nu, N - 1))
dt = 0.1
tol = 0.35
xtraj, utraj, K = iLQRsimple_py(x0, xg, utraj0, Q, R, Qf, dt, tol)
def sortByVoltage(arrays, voltages):
sort_idx_v = ulab.numerical.argsort(voltages, axis=0)
o_arrays = [ulab.zeros(len(a)) for a in arrays]
### ulab doesn't seem to implement indexing by an array, hence
### the for loop
for a, o_a in zip(arrays, o_arrays):
for idx in range(len(sort_idx_v)):
o_a[idx] = a[sort_idx_v[idx]]
return o_arrays
def averageVI(store, cycles_n, scale=1):
"""Calculate the voltages and currents for a cycle by averaging all cycles.
Calculate the resistance from those values."""
avg_len = store.shape[0] // cycles_n
avg_v = ulab.zeros(avg_len)
avg_i = ulab.zeros(avg_len)
for s_idx, (v_set_neg, v_set_pos, v_meas_neg, v_meas_pos) in enumerate(store):
i5 = (v_meas_neg - v_set_neg) / r_neg
i6 = (v_set_pos - v_meas_pos) / r_pos
c_v = v_meas_pos - v_meas_neg
c_i = (i5 + i6) / 2
v_idx = s_idx % avg_len
avg_v[v_idx] += c_v
avg_i[v_idx] += c_i
avg_r = avg_v / avg_i
avg_v *= scale / cycles_n
avg_i *= scale / cycles_n
return (avg_r, avg_v, avg_i)
def __call__(self, leds):
num = len(leds)
left = np.zeros(num, dtype=np.uint8)
right = np.zeros(num, dtype=np.uint8)
right[:self.length] = np.linspace(0, 255, self.length)
while True:
# Shift left/right
offLeft = left[0]
offRight = right[num - 1]
left[:-1] = left[1:]
right[1:] = right[:-1]
right[0] = offLeft
left[num - 1] = offRight
# Display
value = np.clip * ((left + right) / 255, 0.0, 1.0)
leds[:] = hsv2rgb(self.hue, self.sat, value)
yield self.pause
def perform_robust_multilateration(loc_range_diff_info, depth):
num_segment = len(loc_range_diff_info)
estimated_locations = []
# estimate location from each segment
for i in range(num_segment):
loc_range_diff = loc_range_diff_info[i]
num_anchors = len(loc_range_diff)
# create numpy array to hold anchor locations and range-differences
locations = np.zeros((num_anchors, 3), dtype=np.float)
range_diffs = np.zeros(num_anchors - 1, dtype=np.float)
# extract locations and range-differences from list and store them into respective numpy array
zone = None
zone_letter = None
for j in range(num_anchors):
if j == 0:
zone, zone_letter = loc_range_diff[j][0][0], loc_range_diff[j][
0][1]
locations[j] = np.array(loc_range_diff[j][1])
else:
locations[j] = np.array(loc_range_diff[j][0:3])
range_diffs[j - 1] = loc_range_diff[j][3]
# Call Gauss Newton Method for location estimation
initial_soln = np.mean(locations, axis=0)
# replace 3 coordinate with sensor depth
initial_soln[2] = depth
estimated_location = tdoa_multilateration_from_single_segement_gauss_newton(
locations, range_diffs, initial_soln)
# estimated_location = TDOALocalization.perform_tdoa_multilateration_closed_form(locations, range_diffs, depth)
if use_ulab:
bflag_estimated = estimated_location.size() > 0
else:
bflag_estimated = estimated_location.size > 0
# Convert to Lat/Lon
if bflag_estimated > 0:
lat, lon = to_latlon(estimated_location[0], estimated_location[1],
zone, zone_letter)
estimated_locations.append([lat, lon, estimated_location[2]])
return estimated_locations
def __init__(self, N, dt, Nx, Nu):
self.K = [np.zeros((Nu, Nx)) for _ in range(N - 1)]
self.l = [np.zeros(Nu) for _ in range(N - 1)]
self.xtraj = [np.zeros(Nx) for _ in range(N)]
self.utraj = [np.zeros(Nu) for _ in range(N - 1)]
self.xnew = [np.zeros(Nx) for _ in range(N)]
self.unew = [np.zeros(Nu) for _ in range(N - 1)]
def median(a: ulab.array, axis=None):
assert isinstance(a, ulab.array), "Input should be ulab.array!"
m, n = a.shape()
a = ulab.numerical.sort(a, axis=axis)
if axis is None:
s = m * n
if (s % 2) == 0: # even
l = math.floor((s + 1) / 2)
u = math.ceil((s + 1) / 2)
result = (a[l-1] + a[u-1]) / 2
else:
l = math.floor((s + 1)/2)
result = a[l-1]
return result
elif axis == 0:
result = ulab.zeros(n, dtype=ulab.float)
if m % 2 == 0:
l = math.floor((m + 1) / 2)
u = math.ceil((m + 1) / 2)
for i in range(n):
result[i] = (a[l-1, i] + a[u-1, i]) / 2
else:
l = math.floor((m + 1) / 2)
for i in range(n):
result[i] = a[l-1, i]
return result
elif axis == 1:
result = ulab.zeros(m, dtype=ulab.float)
if n % 2 == 0:
l = math.floor((n + 1) / 2)
u = math.ceil((n + 1) / 2)
for i in range(m):
result[i] = (a[i, l-1] + a[i, u-1]) / 2
else:
l = math.floor((n + 1) / 2)
for i in range(m):
result[i] = a[i, l-1]
return result
else:
raise Exception("Allowed axis are None, 0, and 1!")
def __init__(self, num, pin=None, order="grb", positions=None):
self.pin = pin
if pin is not None:
pin.init(pin.OUT)
self.order = order
if positions is None:
positions = {}
self.positions = positions
self.num = num
self._red0 = order.lower().find("r")
self._green0 = order.lower().find("g")
self._blue0 = order.lower().find("b")
self._state = np.zeros((num, 3), dtype=np.uint8)
def dynamics_xdot(t, x, u):
Nx = 2
m = 1.0
b = 0.1
lc = 0.5
I = 0.25
g = 9.81
xdot = np.zeros(Nx)
xdot[0] = x[1]
xdot[1] = (u - m * g * lc * math.sin(x[0]) - b * x[1]) / I
return xdot
def spatial_filter(img, kernel, dxy=None):
""" Convolves `img` with `kernel` assuming a stride of 1. If `img` is linear,
`dxy` needs to hold the dimensions of the image.
"""
# Make sure that the image is 2D
_img = np.array(img)
if dxy is None:
dx, dy = _img.shape()
isInt = type(img[0:0])
isFlat = False
else:
dx, dy = dxy
if dx * dy != _img.size():
raise TypeError("Dimensions do not match number of `img` elements")
isInt = type(img[0])
isFlat = True
img2d = _img.reshape((dx, dy))
# Check if kernel is a square matrix with an odd number of elements per row
# and column
krn = np.array(kernel)
dk, dk2 = krn.shape()
if dk != dk2 or dk % 2 == 0 or dk <= 1:
raise TypeError(
"`kernel` is not a square 2D matrix or does not have an "
"odd number of row / column elements")
# Make a padded copy of the image; pad with mean of image to reduce edge
# effects
padd = dk // 2
img2dp = np.ones((dx + padd * 2, dy + padd * 2)) * numerical.mean(img2d)
img2dp[padd:dx + padd, padd:dy + padd] = img2d
imgRes = np.zeros(img2dp.shape())
# Convolve padded image with kernel
for x in range(0, dx):
for y in range(0, dy):
imgRes[x + padd, y + padd] = numerical.sum(
img2dp[x:x + dk, y:y + dk] * krn[:, :])
# Remove padding, flatten and restore value type if needed
_img = imgRes[padd:dx + padd, padd:dy + padd]
if isFlat:
_img = _img.flatten()
if isInt:
_img = list(np.array(_img, dtype=np.int16))
return _img
def dynamics(t, x, u):
Nx = 2
m = 1.0
b = 0.1
lc = 0.5
I = 0.25
g = 9.81
xdot = np.zeros(Nx)
xdot[0] = x[1]
xdot[1] = (u - m * g * lc * math.sin(x[0]) - b * x[1]) / I
A = np.array([[0, 1], [-m * g * lc * math.cos(x[0]) / I, -b / I]])
B = np.array([[0], [1 / I]])
# dxdot = np.hstack((A, B))
return xdot, A, B
def __init__(self, width, height, i2c, display_bus, display, reverse, font):
# The board's integral display size (as informed by product page adafru.it/1770)
self.WIDTH = width
self.HEIGHT = height
self.i2c = i2c
self.display = display
self.colours = Colours()
self.reverse = reverse
self.font = font
# The image sensor's design-limited temperature range
self.MIN_SENSOR_C = 0
self.MAX_SENSOR_C = 80
self.MIN_SENSOR_F = self.celsius_to_fahrenheit(self.MIN_SENSOR_C)
self.MAX_SENSOR_F = self.celsius_to_fahrenheit(self.MAX_SENSOR_C)
# Convert default alarm and min/max range values from config file
self.MIN_RANGE_F = 60
self.MAX_RANGE_F = 120
self.MIN_RANGE_C = self.fahrenheit_to_celsius(self.MIN_RANGE_F)
self.MAX_RANGE_C = self.fahrenheit_to_celsius(self.MAX_RANGE_F)
# New stuff
self.ALARM_F = 120
self.ALARM_C = self.fahrenheit_to_celsius(self.ALARM_F)
self.grid_size = 8
self.sensor_data = ulab.array(range(self.grid_size * self.grid_size)).reshape((self.grid_size, self.grid_size)) # Color index narray
self.grid_data = ulab.zeros(((2 * self.grid_size) - 1, (2 * self.grid_size) - 1)) # 15x15 color index narray
#self.histogram = ulab.zeros((2 * self.grid_size) - 1) # Histogram accumulation narray
self.GRID_AXIS = (2 * self.grid_size) - 1 # Number of cells along the grid x or y axis
self.GRID_SIZE = self.HEIGHT # Maximum number of pixels for a square grid
self.GRID_X_OFFSET = self.WIDTH - self.GRID_SIZE # Right-align grid with display boundary
self.CELL_SIZE = self.GRID_SIZE // self.GRID_AXIS # Size of a grid cell in pixels
self.PALETTE_SIZE = 100 # Number of colors in spectral palette (must be > 0)
self.param_colors = [("ALARM", self.colours.WHITE), ("RANGE", self.colours.RED), ("RANGE", self.colours.CYAN)]
# Define the sensor
self.amg8833 = adafruit_amg88xx.AMG88XX(self.i2c)
def __call__(self, tree):
num = sum(len(ss) for ss in tree)
indices = np.zeros((num, 2), dtype=int)
start = 0
for ii in range(len(tree)):
length = len(tree[ii])
ss = slice(start, start + length)
indices[ss, 0] = ii
indices[ss, 1] = np.arange(length)
start += length
while True:
tree.clear()
order = sorted([(randFloat(), ii) for ii in range(num)])
for _, ii in order:
ss, tt = indices[ii]
tree[ss][tt] = randomColor()
yield
for _, ii in order:
ss, tt = indices[ii]
tree[ss][tt] = BLACK
yield
def main2():
N = 25
Nx = 6
Nu = 3
qp = 1
qw = .01
Q = np.array([[qp, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, qp, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, qp, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, qw, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, qw, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0,
qw]])
qpf = 100.0
qwf = 10 * qpf
Qf = np.array([[qpf, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, qpf, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, qpf, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, qwf, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, qwf, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, qwf]])
R = .1 * np.eye(3)
#x0 = np.array([0.07500539420980451, 0.7678669790425526, 0.3299131467179552, 0.0, 0.0, 0.0])
x0 = np.array([
-0.274710632045161, 0.6780834943980876, 0.4108832368302142, 0.0, 0.0,
0.0
])
xg = np.array([0, 0, 0, 0, 0, 0.0])
utraj0 = np.zeros((Nu, N - 1))
dt = 12.0
tol = 0.5
xtraj, utraj, K = iLQRsimple_py(x0, xg, utraj0, Q, R, Qf, dt, tol)
print(xtraj[-1])
humidity_svc = HumidityService() humidity_svc.measurement_period = 100 humidity_last_update = 0 light_svc = LightSensorService() light_svc.measurement_period = 100 light_last_update = 0 # Send 256 16-bit samples at a time. MIC_NUM_SAMPLES = 256 mic_svc = MicrophoneService() mic_svc.number_of_channels = 1 mic_svc.measurement_period = 100 mic_last_update = 0 mic_samples = ulab.zeros(MIC_NUM_SAMPLES, dtype=ulab.uint16) temp_svc = TemperatureService() temp_svc.measurement_period = 100 temp_last_update = 0 ble = BLERadio() # The Web Bluetooth dashboard identifies known boards by their # advertised name, not by advertising manufacturer data. ble.name = "Sense" # The Bluefruit Playground app looks in the manufacturer data # in the advertisement. That data uses the USB PID as a unique ID. # Feather Bluefruit Sense USB PID: # This board is not yet support on the app. # Arduino: 0x8087, CircuitPython: 0x8088
except ImportError:
import numpy as np
use_ulab = False
x = np.linspace(-np.pi, np.pi, num=8)
y = np.sin(x)
if use_ulab:
a, b = np.fft.fft(y)
c, d = np.fft.ifft(a, b)
# c should be equal to y
cmp_result = []
for p, q in zip(list(y), list(c)):
cmp_result.append(math.isclose(p, q, rel_tol=1e-09, abs_tol=1e-09))
print(cmp_result)
z = np.zeros(len(x))
a, b = np.fft.fft(y, z)
c, d = np.fft.ifft(a, b)
# c should be equal to y
cmp_result = []
for p, q in zip(list(y), list(c)):
cmp_result.append(math.isclose(p, q, rel_tol=1e-09, abs_tol=1e-09))
print(cmp_result)
g = np.fft.spectrogram(y)
h = np.sqrt(a * a + b * b)
cmp_result = []
for p, q in zip(list(g), list(h)):
cmp_result.append(math.isclose(p, q, rel_tol=1e-09, abs_tol=1e-09))
print(cmp_result)
else:
def least_median_square_estimate(loc_range_diff_info: list, depth: float,
max_iters: int) -> list:
num_segments = len(loc_range_diff_info)
lead_anchor_locations_list = []
asst_anchor_locations_list = []
range_diffs_list = []
zone_info_list = []
lut = []
for i in range(num_segments):
loc_range_diff = loc_range_diff_info[i]
num_anchors = len(loc_range_diff)
lut += [i] * (num_anchors - 1)
for j in range(num_anchors):
if j == 0:
zone, zone_letter = loc_range_diff[j][0][0], loc_range_diff[j][
0][1]
zone_info_list.append([zone, zone_letter])
lead_anchor_locations_list.append(loc_range_diff[j][1])
else:
asst_anchor_locations_list.append(loc_range_diff[j][0:3])
range_diffs_list.append(loc_range_diff[j][3])
# Check if all anchors belong to same zone
bflag_zone = True
zone, zone_letter = zone_info_list[0][0], zone_info_list[0][1]
for zone_info in zone_info_list:
if zone_info != [zone, zone_letter]:
bflag_zone = False
break
if not bflag_zone:
raise ValueError("All the anchors are not in same zone!")
num_asst_anchors = len(lut)
# Generate subsets of assistant anchors with each subset containing 3 elements
subsets = list(random_combinations(range(num_asst_anchors), 3, max_iters))
# For each subset, calculate the unknown location using multilateration
# and then calculate median of square residues = (range_diff - (SA - SB))**2 ,
# where A and B represent the lead and the assistant anchor, respectively, and
# S represent estimated unknown location
min_median = np.inf
robust_location = None
iter_count = 0
for subset in subsets:
# Pack together the assistant and the corresponding lead anchor's location
anchor_locations = []
range_diffs = []
for index in subset:
lead_anchor = lead_anchor_locations_list[lut[index]]
asst_anchor = asst_anchor_locations_list[index]
anchors = np.array([lead_anchor, asst_anchor])
range_diff = np.array([range_diffs_list[index]])
anchor_locations.append(anchors)
range_diffs.append(range_diff)
# Calculate initial soln as mean of Anchor Positions
mean_anchor_location = np.zeros(3, dtype=np.float)
num_anchors = 0
for anchor in anchor_locations:
mean_anchor_location = mean_anchor_location + np.sum(anchor,
axis=0)
if use_ulab:
num_anchors += anchor.shape()[0]
else:
num_anchors += anchor.shape[0]
init_soln = mean_anchor_location / num_anchors
init_soln[2] = depth
# Estimate location from selected subset of anchors using Gauss-Newton method
sensor_location = tdoa_multilateration_from_multiple_segments_gauss_newton(
anchor_locations, range_diffs, init_soln)
# For each estimated location calculate the square of residues, i.e., (observed_range_diff - calculated_range_diff)**2
sq_residues = []
for index in subset:
lead_anchor = np.array(lead_anchor_locations_list[lut[index]])
asst_anchor = np.array(asst_anchor_locations_list[index])
range_diff = np.array([range_diffs_list[index]])
lead_to_sensor = np.linalg.norm((lead_anchor - sensor_location))
asst_to_sensor = np.linalg.norm((asst_anchor - sensor_location))
estimated_range_diff = lead_to_sensor - asst_to_sensor
sq_residues.append((range_diff - estimated_range_diff)**2)
# Calculate Median of Square-Residues
current_median = np.median(np.array(sq_residues))
# Select the subset which results into lowest median of errors
if current_median <= min_median:
min_median = current_median
robust_location = sensor_location
if iter_count >= max_iters:
break
iter_count += 1
# Convert the location into Lat/Lon system
try:
lat, lon = to_latlon(robust_location[0], robust_location[1], zone,
zone_letter)
estimated_location = [lat, lon, robust_location[2]]
except Exception as e:
print("Not a valid estimate: " + str(e))
estimated_location = []
return estimated_location
def tdoa_multilateration_from_single_segement_gauss_newton(
locations: np.array,
deltas: np.array,
initial_soln: np.array,
max_iters: int = 10,
eta: float = 1E-3,
abs_tol: float = 1E-6):
"""
tdoa_multilateration_gauss_newton solves the problem: argmin_x sum_{i=1}^{N} (d_i - || x - a_0 ||_2 - || x - a_i ||_2)^{2}
by Gauss-Newton Method, which finds a local optimum.
:param locations: Nx3 array of anchor locations
:param deltas: (N-1)x1 array of range-differences
:param initial_soln: 3x1 array of initial solution
:param max_iters: int
:param eta: float
:param abs_tol: float
:return: 3x1 array of estimated solution
"""
if use_ulab:
m, n = locations.shape()
else:
m, n = locations.shape
soln = np.array(initial_soln)
for j in range(max_iters):
# ==== Calculate the LHS and RHS of Normal Equation: J'*J*d = -J'*res,
# where J is Jacobian, d is descent direction, res is residual ====== #
jTj = np.zeros((n - 1, n - 1))
jTres = np.zeros((n - 1, 1))
denom_1 = np.linalg.norm(soln - locations[0])
for i in range(1, m):
denom_2 = np.linalg.norm(soln - locations[i])
res = (deltas[i - 1] - (denom_1 - denom_2))
del_res = np.array([[((soln[0] - locations[i, 0]) / denom_2 -
(soln[0] - locations[0, 0]) / denom_1)],
[((soln[1] - locations[i, 1]) / denom_2 -
(soln[1] - locations[0, 1]) / denom_1)]])
if use_ulab:
jTj = jTj + np.linalg.dot(del_res, del_res.transpose())
else:
jTj = jTj + np.dot(del_res, del_res.transpose())
jTres = jTres + (del_res * res)
# ===================== calculation ENDs here ======================================================= #
# ==================== Take next Step ========================= #
# Modification according to Levenberg-Marquardt suggestion
jTj = jTj + np.diag(np.diag(jTj) * eta)
eta = eta / 2.0
# Invert 2x2 matrix and update solution
denom = jTj[0, 0] * jTj[1, 1] - jTj[1, 0] * jTj[0, 1]
numer = np.array([[jTj[1, 1], -jTj[0, 1]], [-jTj[1, 0], jTj[0, 0]]])
if denom >= 1E-9:
inv_jTj = numer / denom
if use_ulab:
temp = np.linalg.dot(inv_jTj, jTres)
else:
temp = np.dot(inv_jTj, jTres)
del_soln = np.array([temp[0, 0], temp[1, 0], 0.0])
soln = soln - del_soln
if np.linalg.norm(del_soln) <= abs_tol:
break
else:
print("Can't invert the matrix!")
break
return soln
def perform_tdoa_multilateration_closed_form(anchor_locations, range_diffs,
sensor_depth):
"""
Calculates the unknown location given the anchor locations and TDoA info
:param anchor_locations: an array of dims [n,3]
:param sensor_depth: float
:param range_diffs: an array of dims [n-1,1]
:return: an array of dim [3,1]
"""
soln = np.array([])
if use_ulab:
bflag_proceed = anchor_locations.size() > 0 and range_diffs.size() > 0
else:
bflag_proceed = anchor_locations.size > 0 and range_diffs.size > 0
if bflag_proceed:
if use_ulab:
n = range_diffs.size()
else:
n = range_diffs.size
mat_m = np.zeros((n, 2), dtype=np.float)
for i in range(n):
mat_m[i, :] = np.array(
[(anchor_locations[0, 0] - anchor_locations[i + 1, 0]),
(anchor_locations[0, 1] - anchor_locations[i + 1, 1])]) * 2.0
vec_c = range_diffs * 2.0
vec_d = np.zeros(n, dtype=np.float)
for i in range(n):
vec_d[i] = np.linalg.norm(anchor_locations[i + 1]) ** 2 - np.linalg.norm(anchor_locations[0]) ** 2 \
- range_diffs[i] ** 2 + 2.0 * (anchor_locations[0, 2] - anchor_locations[i + 1, 2]) * sensor_depth
# Invert the matrix and find the solution if feasible
if use_ulab:
mTm = np.linalg.dot(mat_m.transpose(), mat_m)
else:
mTm = np.dot(mat_m.transpose(), mat_m)
# Invert the matrix mTm
denom = mTm[0, 0] * mTm[1, 1] - mTm[1, 0] * mTm[0, 1]
numer = np.array([[mTm[1, 1], -mTm[0, 1]], [-mTm[1, 0], mTm[0, 0]]])
if denom >= 1E-9:
mTm_inv = numer / denom
if use_ulab:
mat_m_inv = np.linalg.dot(mTm_inv, mat_m.transpose())
else:
mat_m_inv = np.matmul(mTm_inv, mat_m.transpose())
# Solve the quadratic equations
if use_ulab:
vec_a = -np.linalg.dot(mat_m_inv, vec_c)
vec_b = -np.linalg.dot(mat_m_inv, vec_d)
alpha = np.linalg.dot(vec_a, vec_a) - 1.0
beta = 2.0 * np.linalg.dot(vec_a,
(vec_b - anchor_locations[0][0:2]))
gamma = np.linalg.dot(
(vec_b - anchor_locations[0][0:2]),
(vec_b - anchor_locations[0][0:2])) + (
sensor_depth - anchor_locations[0][2])**2
else:
vec_a = -np.dot(mat_m_inv, vec_c)
vec_b = -np.dot(mat_m_inv, vec_d)
alpha = np.dot(vec_a, vec_a) - 1.0
beta = 2.0 * np.dot(vec_a, (vec_b - anchor_locations[0][0:2]))
gamma = np.dot((vec_b - anchor_locations[0][0:2]),
(vec_b - anchor_locations[0][0:2])) + (
sensor_depth - anchor_locations[0][2])**2
delta = beta**2 - 4.0 * alpha * gamma
if delta < 0.0:
pass
elif math.isclose(delta, 0.0, abs_tol=1E-5) and beta < 0.0:
root = -beta / (2.0 * alpha)
soln = vec_a * root + vec_b
soln = np.array([soln[0], soln[1], sensor_depth])
elif math.isclose(alpha, 0.0, abs_tol=1E-5) and beta < 0.0:
root = -gamma / beta
soln = vec_a * root + vec_b
soln = np.array([soln[0], soln[1], sensor_depth])
elif delta > 0.0 and not math.isclose(alpha, 0.0, abs_tol=1E-5):
# print("delta > 0 and alpha != 0")
sqrt_delta = np.sqrt(delta)
root1 = (-beta - sqrt_delta) / (2 * alpha)
root2 = (-beta + sqrt_delta) / (2 * alpha)
# print("Root1={:0.4f} and Root2={:0.4f}".format(root1, root2))
if root2 < 0.0 < root1:
soln = vec_a * root1 + vec_b
soln = np.array([soln[0], soln[1], sensor_depth])
elif root1 < 0.0 < root2:
soln = vec_a * root2 + vec_b
soln = np.array([soln[0], soln[1], sensor_depth])
elif root1 > 0.0 and root2 > 0.0:
pass
else: # Both roots are negative
pass
else:
pass
else:
pass
return soln
