def __call__(self, tree):
numStrings = len(tree)
length = tree.length
goingUp = [True] * numStrings
hue = [ii / self.length for ii in range(self.length)]
colors = [HSV(hh, 1.0, 1.0).toRgb() for hh in hue]
red = np.array([cc.red for cc in colors])
green = np.array([cc.green for cc in colors])
blue = np.array([cc.blue for cc in colors])
stringEnd = length - self.length
location = [
int((0.5 * np.cos(ii / numStrings * np.pi) + 0.5) * stringEnd)
for ii in range(numStrings)
]
tree.clear()
while True:
for ii, string in enumerate(tree):
loc = location[ii]
string[loc:loc + self.length] = (red, green, blue)
if goingUp[ii] and loc == stringEnd:
goingUp[ii] = False
elif not goingUp[ii] and loc == 0:
goingUp[ii] = True
if goingUp[ii]:
string.right()
location[ii] += 1
else:
string.left()
location[ii] -= 1
yield
def main():
max_all = 10
while True:
mic.record(samples_bit, len(samples_bit))
samples = ulab.array(samples_bit[3:])
spectrogram1 = ulab.fft.spectrum(samples)
# spectrum() is always nonnegative, but add a tiny value
# to change any zeros to nonzero numbers
spectrogram1 = ulab.vector.log(spectrogram1 + 1e-7)
spectrogram1 = spectrogram1[1:(fft_size // 2) - 1]
min_curr = ulab.numerical.min(spectrogram1)[0]
max_curr = ulab.numerical.max(spectrogram1)[0]
if max_curr > max_all:
max_all = max_curr
else:
max_curr = max_curr - 1
print(min_curr, max_all)
min_curr = max(min_curr, 3)
# Plot FFT
data = (spectrogram1 - min_curr) * (51. / (max_all - min_curr))
# This clamps any negative numbers to zero
data = data * ulab.array((data > 0))
graph.show(data)
def rveci_from_ecef(r_ecef, v_ecef, era):
"""Get rv in eci from ecef and earth rotation angle.
Args:
r_ecef: position in ecef (km)
v_ecef: velocity in ecef wrt ecef (km/s)
era: earth rotation angle (radians)
Returns:
r_eci: position in eci (km)
v_eci: velocity in eci wrt eci (km/s)
"""
sin_theta = math.sin(era)
cos_theta = math.cos(era)
r_eci = np.array([
cos_theta * r_ecef[0] - sin_theta * r_ecef[1],
sin_theta * r_ecef[0] + cos_theta * r_ecef[1], r_ecef[2]
])
omega_earth = 7.292115146706979e-5
v_eci = np.array([
cos_theta * v_ecef[0] - sin_theta * v_ecef[1] - omega_earth * r_eci[1],
sin_theta * v_ecef[0] + cos_theta * v_ecef[1] + omega_earth * r_eci[0],
v_ecef[2]
])
return r_eci, v_eci
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 dynamics_xdot(t, x, m):
# spacecraft properties
J = np.array([[1.959e-4, 2016.333e-9, 269.176e-9],
[2016.333e-9, 1.999e-4, 2318.659e-9],
[269.176e-9, 2318.659e-9, 1.064e-4]])
invJ = 1e3 * np.array(
[[5.105190043647774, -0.051357738055630, -0.011796180015288],
[-0.051357738055630, 5.004282688631113, -0.108922940075563],
[-0.011796180015288, -0.108922940075563, 9.400899721840833]])
max_moments = np.array([8.8e-3, 1.373e-2, 8.2e-3])
# magnetic field in ECI
T = 50 * 60
B_eci = 3e-5 * np.array([
math.sin(2 * math.pi * t / T + math.pi / 2),
math.sin(2 * math.pi * t / T - math.pi / 3),
math.sin(2 * math.pi * t / T + 3 * math.pi / 2)
])
# unpack state
p = x[0:3]
w = x[3:6]
# MRP
p1 = p[0]
p2 = p[1]
p3 = p[2]
# angular velocity
wx = w[0]
wy = w[1]
wz = w[2]
# magnetic field in the body frame
B_body = mul2(dcm_from_p(p).transpose(), B_eci.transpose())
# mrp kinematics
pdot = pdot_from_w(p, w)
# scale the magnetic moment with the tanh
actual_m = max_moments * np.vector.tanh(m)
# resulting torque on the spacecraft
actual_tau = mul2(hat(actual_m), B_body)
# euler dynamics
wdot = mul2(invJ, actual_tau - mul2(hat(w), mul2(J, w.transpose())))
# final state derivative
xdot = np.array([
pdot[0][0], pdot[1][0], pdot[2][0], wdot[0][0], wdot[1][0], wdot[2][0]
])
return xdot
def rotate_z(theta, xyz, deg=False):
if deg:
theta = deg2rad(theta)
mat = u.array([[m.cos(theta), m.sin(theta), 0],
[-m.sin(theta), m.cos(theta), 0], [0, 0, 1]])
xyz = u.array(xyz)
xyz = xyz.reshape((3, 1))
return u.linalg.dot(mat, xyz)
def match(tester,dataset):
codex = np.array(dataset)
t = np.array(tester)
for i in range(codex.shape()[0]):
codex[i,:] = codex[i,:] - t
data = np.sum(codex * codex, axis=1)
data = np.sqrt(data)
if codex.shape()[0] == 1:
return data,1
ind = np.argmin(data)
return data[ind][0],ind + 1
def dynamics_jacobians(t, x, u):
Nx = 2
m = 1.0
b = 0.1
lc = 0.5
I = 0.25
g = 9.81
A = np.array([[0, 1], [-m * g * lc * math.cos(x[0]) / I, -b / I]])
B = np.array([[0], [1 / I]])
return A, B
def solve(min, max):
# Uses linear algebra to find the soloution of 2 linear equations
a = ulab.array([[min, 1], [max,
1]]) # Stores the equation side of the matrix
b = ulab.array([[0], [32726]]) # Stores the resultant side of the matrix
x = ulab.linalg.dot(
ulab.linalg.inv(a),
b) # Uses the formula inv(A) dot (B) = Soloution matrix
return [x[0][0], x[1][0]] # Returns soloution matrix
def inference(feature_list, task, img):
x = -1
name = ""
dist = 0
for x in [0, 80, 160]:
for y in [0, 60, 120]:
print(x, y)
gc.collect()
#img2 = img.copy((x,y,160,120))
img2 = img.copy((0, 0, 320, 240))
fmap = kpu.forward(task, img2)
#p = np.array(fmap[:])
kpu.fmap_free(fmap)
'''
# get nearest target
name,dist,v = get_nearest(feature_list, p)
print("name:" + name + " dist:" + str(dist) + " v:" + str(v) + " mem:" + str(gc.mem_free()))
if dist < 200:
break
if dist < 200:
break
'''
#img2 = img.copy((x,y,160,120))
fmap = kpu.forward(task, img)
p = np.array(fmap[:])
kpu.fmap_free(fmap)
# get nearest target
name, dist, v = get_nearest(feature_list, p)
print("name:" + name + " dist:" + str(dist) + " v:" + str(v) + " mem:" +
str(gc.mem_free()))
print(x)
return name, dist, x
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 verifyargmax(n):
for p in permutations(n):
m1 = np.argmax(p)
m2 = np.argmax(np.array(p))
if m1 != m2 or p[m1] != max(p):
return 0
return p[m1]
def Rz(theta):
# utility function to get the z rotation matrix
# uses radians
return np.array([[math.cos(theta),math.sin(theta),0],\
[-math.sin(theta),math.cos(theta),0],\
[0, 0, 1]])
def ECEF2ANG(r_ecef, station):
# takes in the ecef position and the lat long alt of station
# station is [lat,long]
earth = Earth()
# constants
deg2rad = math.pi / 180
# get station ECEF
N = earth.R / math.sqrt(1 - earth.e**2 * math.sin(station[0] * deg2rad)**2)
r_station = np.array(\
[N * np.vector.cos(station[0]*deg2rad) * np.vector.cos(station[1]*deg2rad),\
N * np.vector.cos(station[0]*deg2rad) * np.vector.sin(station[1]*deg2rad),\
N*(1-earth.e ** 2) * np.vector.sin(station[0]*deg2rad)])
diff = r_ecef - r_station.transpose()
diff_normalized = diff / (np.vector.sqrt(diff[0]**2 + diff[1]**2 +
diff[2]**2))
r_station_normalized = r_station / (
np.vector.sqrt(r_station[0]**2 + r_station[1]**2 + r_station[2]**2))
ang_dot = (diff_normalized[0] * r_station_normalized[0] + \
diff_normalized[1] * r_station_normalized[1] + \
diff_normalized[2] * r_station_normalized[2])
ang_from_vert = math.acos(ang_dot[0])
return ang_from_vert
def render(self):
try:
reverse = True
sensor = self.amg8833.pixels # Get sensor_data data
# This clever bit of code, taken from https://thispointer.com/python-reverse-a-list-sub-list-or-list-of-list-in-place-or-copy/
# reverses the lists-of-lists along both "axis" if you want to think of it like that
# Useful if your render is "upside down and back-to-front"!
if reverse:
sensor = [elem[::-1] for elem in sensor][::-1]
self.sensor_data = ulab.array(sensor) # Copy to narray
for row in range(0, 8):
for col in range(0, 8):
self.sensor_data[col, row] = min(max(self.sensor_data[col, row], 0), 80)
# Normalize temperature to index values and interpolate
self.sensor_data = (self.sensor_data - self.MIN_RANGE_C) / (self.MAX_RANGE_C - self.MIN_RANGE_C)
self.grid_data[::2, ::2] = self.sensor_data # Copy sensor data to the grid array
self.ulab_bilinear_interpolation() # Interpolate to produce 15x15 result
# Display image or histogram
self.update_image_frame()
except Exception as err:
print("Error in render()")
print(str(err))
while True:
pass
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 find_blobs(img, dxy, nsd=1.0):
""" Detect continues area(s) ("blobs") with pixels above a certain
threshold in an image. `img` contains the flattened image (1D),
`dxy` image width and height, and `nsd` a factor to calculate the blob
threshold from image mean and s.d. (thres = avg +sd *nsd).
"""
img_array = np.array(img)
return user.blobs(img_array, dxy, nsd)
def volt_to_pH(self, volt):
if set(('pH7', )) == set(self.user_samples):
pH_value = self.slope * (volt - self.volt_pH7) + 7.0
else:
pH_array, volt_array = self.calibration_data
fit = ulab.poly.polyfit(volt_array, pH_array, 1)
pH_value = ulab.poly.polyval(fit, ulab.array([volt]))[0]
return pH_value
def pH_to_volt(self, pH):
if set(('pH7', )) == set(self.user_samples):
volt_value = (1.0 / self.slope) * (pH - 7.0) + self.volt_pH7
else:
pH_array, volt_array = self.calibration_data
fit = ulab.poly.polyfit(pH_array, volt_array, 1)
volt_value = ulab.poly.polyval(fit, ulab.array([pH]))[0]
return volt_value
def get_feature(task):
img = sensor.snapshot()
img.draw_rectangle(1, 46, 222, 132, color=(255, 0, 0), thickness=3)
lcd.display(img)
feature = kpu.forward(task, img)
print('get_feature')
gc.collect()
return np.array(feature[:])
def predict(new_data):
"""
Predict label for a single new data instance.
"""
gc.collect()
score = ulab.array([(ulab.linalg.dot(new_data, w) + b)
for w, b in zip(weights, bias)])
return labels[ulab.numerical.argmin(score)]
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 get_feature(task, img):
print("--- 1")
free()
print("--- 4")
feature = kpu.forward(task,img)
print("--- 5")
free()
print("--- 6")
ar = np.array(feature[:])
print("--- 7")
return ar
def DH(self, d, theta, alpha, r):
DH = u.array([[
m.cos(theta), -m.sin(theta) * m.cos(alpha),
m.sin(theta) * m.sin(alpha), r * m.cos(theta)
],
[
m.sin(theta),
m.cos(theta) * m.cos(alpha),
-m.cos(theta) * m.sin(alpha), r * m.sin(theta)
], [0, m.sin(alpha), m.cos(alpha), d], [0, 0, 0, 1]])
return DH
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 propagatorinfo_from_gps(GNSS_week, TOW, r_ecef, v_ecef):
"""Get propagator starting info from GPS data.
Args:
GNSS_week: Weeks since january 6th 1980
TOW: Seconds into current week
r_ecef: ecef position (.01 meters)
v_cef: ecef velocity wrt ecef (0.01 m/s)
Returns:
era: Earth rotation angle (radians)
r_eci: position in eci (km)
v_eci: velocity in eci wrt eci (km/s)
Comments:
Inputs are raw GPS data, no pre-scaling needed.
"""
# get MJD from gps time info
MJD_int, MJD_float = mjd_from_gps(GNSS_week, TOW)
# compute earth rotation angle from the MJD
era = earth_rotation_angle_gps(MJD_int, MJD_float)
# scale position and velocity
r_ecef = np.array([r_ecef[0], r_ecef[1], r_ecef[2]]) / 100000
v_ecef = np.array([v_ecef[0], v_ecef[1], v_ecef[2]]) / 100000
# convert to inertial position and velocity
r_eci, v_eci = rveci_from_ecef(r_ecef, v_ecef, era)
# debugging printouts
print('MJD int', MJD_int)
print('MJD float', MJD_float)
print('Earth Rotation Angle', era)
print('r_ecef', r_ecef)
print('v_ecef', v_ecef)
print('r_eci', r_eci)
print('v_eci', v_eci)
return era, r_eci, v_eci
def main():
while True:
_ = mic.record(samples_bit, len(samples_bit))
if startup_samples > 0:
samples = ulab.array(memoryview(samples_bit)[startup_samples:])
else:
samples = ulab.array(samples_bit)
peak_meter.value = edubit.read_sound_sensor()
# spectrum() is always nonnegative, but add a tiny value
# to change any zeros to nonzero numbers (for log())
full_spectrogram_log = ulab.vector.log(
ulab.extras.spectrogram(samples) + 1e-7)
# Lose the dc component and second half which is mirror image
spectrogram_log = full_spectrogram_log[1:(fft_size // 2)]
### Ignore everything below min_val by setting to zero
spectrogram_log_floored = spectrogram_log - min_val
spectrogram_log_floored = (spectrogram_log_floored *
ulab.array(spectrogram_log_floored > 0))
### These might be ~63Hz buckets
bass = spectrogram_log_floored[0:3]
mid = spectrogram_log_floored[3:12]
treble_l = spectrogram_log_floored[12:64]
treble_h = spectrogram_log_floored[64:]
edubit.set_led(Edubit.GREEN, loudness(bass) > loud_threshold)
edubit.set_led(Edubit.YELLOW, loudness(mid) > loud_threshold)
edubit.set_led(
Edubit.RED,
max(loudness(treble_l), loudness(treble_h)) > loud_threshold)
# Plot FFT
data = spectrogram_log_floored * ((PALETTE_LEN - 1) / max_val)
graph.show(data)
peak_meter.value = edubit.read_sound_sensor()
def calc(self, theta):
d = u.array([0, 0, 0, 0])
r = [self.r_off, self.L[0], self.L[1], self.L[2]]
theta = [self.theta_off, theta[0], theta[1], theta[2]]
pos = [self.DH(d[0], theta[0], self.alpha[0], r[0])]
for i in range(1, 4):
pos.append(
u.linalg.dot(pos[-1],
self.DH(d[i], theta[i], self.alpha[i], r[i])))
return pos
def dynamics(x,Earth):
"""FODE + J2 dynamics function
Args:
x: [rx,ry,rz,vx,vy,vz] in km, km/s
earth: Earth class
Returns:
xdot: [vx, vy, vz, ax, ay, az]
"""
# precompute a few terms to reduce number of operations
r = norm(x[0:3])
Re_r_sqr = 1.5*Earth.J2*(Earth.R/r)**2
five_z_sqr = 5*x[2]**2/(r**2)
# two body and J2 acceleration together
accel = (-Earth.mu/(r**3))*np.array([x[0]*(1 - Re_r_sqr*(five_z_sqr - 1)),
x[1]*(1 - Re_r_sqr*(five_z_sqr - 1)),
x[2]*(1 - Re_r_sqr*(five_z_sqr - 3))])
return np.array([x[3],x[4],x[5],accel[0],accel[1],accel[2]])
