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]])