def __init__(self):
self.localization = localization.Localization()
self.start = time.time()
#anchors
anc = ac.getAnchors()
self.anchors = anc['anchors']
self.anchors_ids = anc['anchors_ids']
self.anchor_id_keys = anc['idKeys']
#kalman
self.history_length = 100
n_dist = len(self.anchors)
x0_dist = np.array([[1], [0]] * n_dist) #np.zeros((n*2,1))
P0_dist = np.diag([1] * (2 * n_dist)) #np.zeros((2*n,2*n))
self.kalman_dist = kalman.Kalman(x0_dist, P0_dist)
n_pos = 3 #x,y,z #len(self.anchors)
x0_pos = np.array([[1], [0]] * n_pos) #np.zeros((n*2,1))
P0_pos = np.diag([1] * (2 * n_pos)) #np.zeros((2*n,2*n))
self.kalman_pos = kalman.Kalman(x0_pos, P0_pos)
self.estimates_history = [
[] for i in range(0, (len(self.anchors)))
] #inizialization as list of empty lists (as many lists as the number of anchors)
#self.last_times = np.zeros((n,1))
self.last_time = None
#udp
self.dao = DAO.UDP_DAO("localhost",
12348) #Receive data (from nodered)
self.data_interval = 0 #1000
self.min_diff_anchors_ratio = 0.75
self.min_diff_anchors = 3 #math.ceil(len(self.anchors)*self.min_diff_anchors_ratio)
self.alpha = 1.9 #0.9722921
self.TxPower = -67.5
self.decimal_approximation = 3
self.batch_size = 0 #if 0: batch_size = len(measures) else batch_size = self.batch_size
self.techniques = ['localization_kalman', 'localization_unfiltered']
def process(self, frame, points):
w, h = frame.shape[1], frame.shape[0]
pointsL, pointsR = self.divide(frame, points)
centerL, valL1, valL2, angleL = self.calcPCA(pointsL)
centerR, valR1, valR2, angleR = self.calcPCA(pointsR)
kalmanInputL = np.array([centerL[0], centerL[1], valL1, valL2, angleL])
kalmanInputR = np.array([centerR[0], centerR[1], valR1, valR2, angleR])
if self.first == True:
self.filterL = kalman.Kalman(kalmanInputL, self.mNoise,
self.pNoise)
self.filterR = kalman.Kalman(kalmanInputR, self.mNoise,
self.pNoise)
self.first = False
_, pdtL = self.filterL.update(kalmanInputL)
_, pdtR = self.filterR.update(kalmanInputR)
centerL, centerR = (int(pdtL[0]), int(pdtL[1])), (int(pdtR[0]),
int(pdtR[1]))
valL1, valL2, angleL = pdtL[2], pdtL[3], pdtL[4]
valR1, valR2, angleR = pdtR[2], pdtR[3], pdtR[4]
self.drawArrow(frame, centerL, valL1, valL2, angleL)
self.drawArrow(frame, centerR, valR1, -valR2, angleR)
addImg = copy(frame)
self.drawEllipses(addImg, centerL, valL1, valL2, angleL)
self.drawEllipses(addImg, centerR, valR1, valR2, angleR)
endL = centerL[0] + math.cos(
(angleL - 90) * math.pi / 180) * valL2, centerL[1] + math.sin(
(angleL - 90) * math.pi / 180) * valL2
endR = centerR[0] - math.cos(
(angleR - 90) * math.pi / 180) * valR2, centerR[1] - math.sin(
(angleR - 90) * math.pi / 180) * valR2
endL = self.rotate((0, h), endL, -30 * math.pi / 180)
endR = self.rotate((w, h), endR, 30 * math.pi / 180)
x = (endL[0] + endR[0]) / 2
y = h * 3 / 5
poly = np.array([(x, y), (w / 3, h * 7 / 8), (w * 2 / 3, h * 7 / 8)],
dtype='int32')
cv2.fillConvexPoly(frame, poly, red)
cv2.addWeighted(frame, 0.6, addImg, 0.4, 0, frame)
#cv2.line(frame, (int(w / 2), 0), (int(w / 2), int(h)), red)
#cv2.line(frame, (x, y), (int(w / 2), y), red, 2)
return int(x - w / 2)
def __init__(self, udp_port):
self.trilateration = trilateration.Trilateration()
self.fingerprinting = fingerprinting.Fingerprinting()
self.start = time.time()
#anchors
anc = ac.getAnchors()
self.anchors = anc['anchors']
self.anchors_ids = anc['anchors_ids']
self.anchor_id_keys = anc['idKeys']
#kalman
self.history_length = 100
n = len(self.anchors)
x0 = np.array([[-50], [0]]*n)#np.zeros((n*2,1))
P0 = np.diag([20]*(2*n))#np.zeros((2*n,2*n))
self.kalman = kalman.Kalman(x0, P0)
self.estimates_history = [[] for i in range(0,(len(self.anchors)))] #inizialization as list of empty lists (as many lists as the number of anchors)
self.last_times = np.zeros((n,1))
self.last_time = None
#udp
self.dao = DAO.UDP_DAO("localhost", udp_port) #Receive data (from nodered 12346, from simulation 12348)
self.data_interval = -1000 #1000
self.min_diff_anchors_ratio = 0.75
self.min_diff_anchors = 8 #math.ceil(len(self.anchors)*self.min_diff_anchors_ratio)
assert n >= self.min_diff_anchors, 'Not enough anchors: ' + str(n)
# model
self.alpha = 1.9 #0.9722921
self.TxPower = -67.5
self.decimal_approximation = 3
self.batch_size = 1 #if 0: batch_size = len(measures) else batch_size = self.batch_size
self.techniques = ['localization_trilateration_kalman',
'localization_trilateration_unfiltered',
'localization_fingerprinting_kalman',
'localization_fingerprinting_unfiltered']
def __init__(self, name, x_pos, y_pos, mmDist):
super().__init__(name, mmDist)
self.type = 1
self.xPos = x_pos
self.yPos = y_pos
self.kalman = kalman.Kalman(np.array([0]), np.eye(1), 0.01, 1e-5)
self.kDist = 0
def __init__(self):
#(debug)plot spectrum flag
self.PLOT = 0
#Parameters
self.BUFFER_SIZE = 1500 #This decide how many samples you want to wait before doing frequency estimation (default:1500)
self.NSAMPLES = 1024 #This decide the FFT Ns when you do Welch's spectrum estimation (mlab.psd)
self.RATE = 35738 #This is the sampling rate of our sound (each Ch) used to normalize the frequencies.
self.SOUND_MESSAGE_SIZE = 254 #The number of sampels contained in one message
self.OVERLAP_PERCENTAGE = 0.7 #The overlap ratio in Welch's Spectrum Estimation.
self.N_OVERLAP = int(self.NSAMPLES * self.OVERLAP_PERCENTAGE
) #Compute the number of samples overlapping.
#energy detection flag
self.ENERGY_SWITCH = 0
self.ENERGY_THRESHOLD = 1000
self.RESONANT_FREQ_LOWER_BOUND = 10
#initialize class member variables
self.resonant_freq = 0
self.count = 0
self.signal_1 = np.array([])
self.signal_2 = np.array([])
self.amp_sum_1 = 0
self.amp_sum_2 = 0
self.f_est = 0
self.msg = PretouchAcoustic()
self.msg.header.frame_id = "/r_gripper_r_finger_tip_link"
self.msg_volume = NoiseVolume()
self.msg_volume.volume = 0.5
self.seq = 0
#adaptive noise generator related
self.VOLUME_GAIN = 0.01 #default: 0.005
self.SNR_TARGET = 6.0 #default: 10
self.snr_current = 0.0
self.SNR_LIST_SIZE = 5 #default: 5
self.snr_list = []
#initialize kalman filter
rospy.loginfo("***** Initialize Kalman Filter *********")
ndim = 1
Sigma_x = 1e-5 #Process Vriance
R = 0.02**2 #Measurement Variance = 0.01**2 #R = 0.02**2
self.k = kalman.Kalman(ndim, Sigma_x, R)
self.mu_init = np.array([2000])
#ROS node, subscriber, and publishers
rospy.init_node('pretouch_acoustic')
side = rospy.get_param('~side', 'right')
self.pub = rospy.Publisher('pretouch', PretouchAcoustic)
self.pub_volume = rospy.Publisher('pretouch/noise_volume', NoiseVolume)
self.sub = rospy.Subscriber('sound/' + side, SoundRaw,
self.sound_callback)
rospy.spin()
def getLocation(self, filtered = True):
measures = self.getMeasures()
if filtered:
x0 =
P0 =
kalman = kalman.Kalman(x0, P0)
delta_t = []
#F(k) - state transition model
F = np.zeros((2*n,2*n))
for i in range(1,2*n,2):
F[i-1][i-1] = 1
F[i-1][i] = delta_t
F[i][i] = 1
measures = kalman.estimate(measures, F, H, Q, G, R)
location = self.trilateration(measures)
return location
def __init__(self):
self.localization = localization.Localization()
#anchors
anc = ac.getAnchors()
self.anchors = anc['anchors']
self.anchors_ids = anc['anchors_ids']
self.anchor_id_keys = anc['idKeys']
#kalman
self.history_length = 5
n = len(self.anchors)
x0 = np.zeros((n * 2, 1))
P0 = np.ones((2 * n, 2 * n)) #np.diag([1]*(2*n))
self.kalman = kalman.Kalman(x0, P0, self.history_length)
#udp
self.dao = DAO.UDP_DAO("localhost", 12346)
self.data_interval = 0 #1000
self.min_diff_anchors_ratio = 0.75
self.min_diff_anchors = 4 #math.ceil(len(self.anchors)*self.min_diff_anchors_ratio)
self.alpha = 0.9722921
self.TxPower = -66.42379
self.decimal_approximation = 3
#batch of measures
self.dist_history = []
#!/usr/bin/python
import numpy as np
import kalman
import utm
import time
k = kalman.Kalman()
wgs84 = [[5.684360726298299, 58.97657658712378],
[5.684343840300778, 58.97657962580959],
[5.684335632553317, 58.9765728650051],
[5.684338895060761, 58.97655124847552],
[5.684352277694142, 58.97652182815653],
[5.684359059522381, 58.97650329593002],
[5.684368949876058, 58.97648081847628],
[5.684376727880238, 58.97646619934486],
[5.684426626357828, 58.97647126823811],
[5.684417220190003, 58.97648076666268],
[5.684397123298792, 58.97649285096912],
[5.684386473993475, 58.97651277632222],
[5.684382063516651, 58.97653523879878],
[5.684376236917328, 58.97655378118837],
[5.684365039916921, 58.97656813672509]]
n = len(wgs84)
for i in range(n):
c = utm.from_latlon(wgs84[i][1], wgs84[i][0])
print("%7.3f , %7.3f" % (c[1], c[0]))
for i in range(10):
Q_red = np.eye(3) * 0.0006**2
Q_red[1,1] = Q_red[2,2] = 0.00006**2
Q_nir = np.eye(3) * 0.006**2
Q_nir[1,1] = Q_nir[2,2] = 0.0006**2
# initial conditions
x0_red = np.array([0.08996554, 0.0083535, 0.04541085])
x0_nir = np.array([0.44320984, 0.3960118, 0. ])
C_red = np.eye(3) * 0.2**2
C_nir = np.eye(3) * 0.5**2
import kalman
# red
kfr = kalman.Kalman(red,x0_red,C_red,R_red,M,Q_red,H,doys,reverse=True)
# do a kernel set for angular normalisation
vzan = np.zeros_like(kfr.alldoys)
szan = vzan + np.mean(vza[mask])
kernelnorm = Kernels(vzan,szan,vzan,LiType='Dense',RossHS=False,doIntegrals=False)
plt.clf()
kfr.run()
plt.ylim(0,0.15)
plt.errorbar(kfr.alldoys,kfr.x[:,0],yerr=np.sqrt(kfr.C[:,0,0]),fmt='ro',label='fiso')
plt.errorbar(kfr.alldoys,kfr.x[:,1],yerr=np.sqrt(kfr.C[:,1,1]),fmt='bo',label='fvol')
plt.errorbar(kfr.alldoys,kfr.x[:,2],yerr=np.sqrt(kfr.C[:,2,2]),fmt='go',label='fgeo')
plt.legend(loc='best')
plt.savefig('figures/kernelsKFred2.png')
import thread
import localization
import udpy
import kalman
localization = localization.Localization()
udp_writer = udpy.UDP_DAO("localhost", 12347)
kalman = kalman.Kalman()
while True:
udp_writer.writeData(localization.getLocation(True))
udp_writer.writeData(localization.getLocation(False))
return math.degrees(radians)
def get_roll(x, y, z): #x-rotation
radians = -math.atan2(y, z)
return math.degrees(radians)
def get_yaw(x,y,z):
radians = math.atan(z / math.sqrt(x*x + z*z))
return math.degrees(radians)
bus = smbus.SMBus(1)
address = 0x68
bus.write_byte_data(address, power_mgmt_1, 0)
#Initialize kalman objects for RPY
Roll = kalman.Kalman()
Pitch = kalman.Kalman()
Yaw = kalman.Kalman()
#make initial guesses
time_pre = time.time()
ax_raw = read_word_2c(0x3b)
ay_raw = read_word_2c(0x3d)
az_raw = read_word_2c(0x3f)
ax = (ax_raw - ax_offset) / acc_sen
ay = (ay_raw - ay_offset) / acc_sen
az = (az_raw - az_offset) / acc_sen
#Initial guesses
def sensor_fuse(self, data):
gps = data["gps"]
accel = data["acc"]
gyro = data["gyro"]
mag = data["mag"]
#Get Orientation from Magnetometer
mx = int(mag['x'])
my = int(mag['y']) - 100
if mx != 0:
self.theta = math.atan2(my,mx)*180/math.pi
if self.theta < 0:
self.orient = 360 + self.theta
else:
self.orient = self.theta
print(self.orient)
#text = "Current Position: Lat %f Lng %f Orient %d degrees" % (self.markers['curpos'][0],self.markers['curpos'][1], self.orient)
#self.curposlab.setText(text)
#calc velocity
ax = float(accel["x"]) / 100
ay = float(accel["y"]) / 100
self.veloc = [self.veloc[0] + ax*self.t, self.veloc[1] + ay*self.t]
s = [self.veloc[0]*self.t + (ax/2 * (self.t**2)), self.veloc[1]*self.t + (ay/2 * (self.t**2))]
print(s)
#GPS Data
latitude = float(gps["lat"])
longitude = float(gps["long"])
if latitude != 0 and longitude != 0:
if self.startfilt == 0:
ndim = 4
xcoord = longitude
ycoord = latitude
vx = 0 #m.s
vy = 0 #m/s
dt = 1 #sec
meas_error = 5 #m
#init filter
proc_error = 0.5;
init_error = 0.5;
x_init = np.array( [xcoord, ycoord, vx, vy] ) #introduced initial xcoord error of 10m
cov_init=init_error*np.eye(ndim)
self.kalman = kal.Kalman(x_init, cov_init, meas_error/10, proc_error)
print("INITKALMAN\n\n\n")
self.startfilt = 1
self.w.moveMarker("curpos", latitude, longitude)
self.w.centerAt(latitude, longitude)
else:
print("accel based localise")
#no new gps so calc via accel + orient
s = np.array([[s[0]], [s[1]]])
print(s)
thetarad = (360 - self.orient) * math.pi / 180
rotmat = np.array([[math.cos(thetarad), -math.sin(thetarad)], [math.sin(thetarad), math.cos(thetarad)]])
print(rotmat)
rots = np.matmul(rotmat, s)
print(rots)
rots[0] = rots[0] * 0.000010526
rots[1] = rots[1] * 0.00000898
latitude = self.latlng[0] + rots[1] # y is lat
longitude = self.latlng[1] + rots[0] #x is long
if self.kalman != 0:
self.kalman.update([longitude, latitude])
lnglat = self.kalman.x_hat
longitude = lnglat[0]
latitude = lnglat[1]
print(longitude, latitude)
self.w.moveMarker("curpos", latitude, longitude)
self.w.centerAt(latitude, longitude)
self.marker_move("curpos", latitude, longitude)
wx = self.waypt[1] / 0.0000105
wy = self.waypt[0] / 0.000009
cx = self.latlng[1] / 0.0000105
cy = self.latlng[0] / 0.000009
vect = [wx - cx, wy - cy]
auto = math.atan2(vect[0], vect[1]) * 180 / math.pi
if auto < 0:
auto = 360 + auto
print("autopilot theta: %d" % auto)
cmd = "stop"
if auto > self.orient:
if auto - self.orient < 180 and auto - self.orient > 20:
cmd = "right"
elif auto - self.orient > 180 and auto - self.orient < 340:
cmd = "left"
else:
cmd = "straight"
elif self.orient > auto:
if abs(auto - self.orient) < 180 and abs(auto - self.orient) > 20:
cmd = "left"
elif abs(auto - self.orient) > 180 and abs(auto - self.orient) < 340:
cmd = "right"
else:
cmd = "straight"
v = np.array(vect)
dist = np.linalg.norm(v)
if dist < 2:
cmd = "stop"
self.golab.setText(cmd)
def process(self, frame, points):
pointsL = []
pointsR = []
for p in points:
if p[0] < frame.shape[1] / 2 and p[1] > frame.shape[0] / 4 and p[
1] < frame.shape[0] * 9 / 10:
pointsL.append(p)
elif p[0] > frame.shape[1] / 2 and p[1] > frame.shape[0] / 4 and p[
1] < frame.shape[0] * 9 / 10:
pointsR.append(p)
if (len(pointsL) == 0 or len(pointsR) == 0): return 0
pointsL.sort(key=lambda x: x[1])
pointsL.append([0, pointsL[-1][1]])
pointsL.append([0, pointsL[0][1]])
pointsR.sort(key=lambda x: x[1])
pointsR.append([frame.shape[1], pointsR[-1][1]])
pointsR.append([frame.shape[1], pointsR[0][1]])
hullL = cv2.convexHull(np.array(pointsL))
hullR = cv2.convexHull(np.array(pointsR))
canvas = np.zeros((frame.shape[0], frame.shape[1]), dtype='uint8')
cv2.fillConvexPoly(canvas, hullL, white)
cv2.fillConvexPoly(canvas, hullR, white)
if self.first == True:
self.lineSegmentL = math.pi, (0, 0), (0, 0)
self.lineSegmentR = 0, (0, 0), (0, 0)
for i in range(hullL.shape[0] - 3):
x, y = hullL[i + 1][0][0] - hullL[i][0][0], hullL[
i + 1][0][1] - hullL[i][0][1]
theta = math.atan2(y, x)
if theta > math.pi / 2 and theta < self.lineSegmentL[0]:
self.lineSegmentL = (theta, (hullL[i][0][0],
hullL[i][0][1]),
(hullL[i + 1][0][0],
hullL[i + 1][0][1]))
for i in range(2, hullR.shape[0] - 1):
x, y = hullR[i + 1][0][0] - hullR[i][0][0], hullR[
i + 1][0][1] - hullR[i][0][1]
theta = math.atan2(y, x) + math.pi
if theta < math.pi / 2 and theta > self.lineSegmentR[0]:
self.lineSegmentR = (theta, (hullR[i][0][0],
hullR[i][0][1]),
(hullR[i + 1][0][0],
hullR[i + 1][0][1]))
else:
error = 10000
preTheta = self.lineSegmentL[0]
buf = 0, (0, 0), (0, 0)
for i in range(hullL.shape[0] - 3):
x, y = hullL[i + 1][0][0] - hullL[i][0][0], hullL[
i + 1][0][1] - hullL[i][0][1]
theta = math.atan2(y, x)
e = math.pow(theta - preTheta, 2)
if (e < error):
error = e
buf = theta, (hullL[i][0][0],
hullL[i][0][1]), (hullL[i + 1][0][0],
hullL[i + 1][0][1])
if error < math.pi * 5 / 180:
self.lineSegmentL = buf
error = 10000
preTheta = self.lineSegmentR[0]
buf = 0, (0, 0), (0, 0)
for i in range(2, hullR.shape[0] - 1):
x, y = hullR[i + 1][0][0] - hullR[i][0][0], hullR[
i + 1][0][1] - hullR[i][0][1]
theta = math.atan2(y, x) + math.pi
e = math.pow(theta - preTheta, 2)
if (e < error):
error = e
buf = theta, (hullR[i][0][0],
hullR[i][0][1]), (hullR[i + 1][0][0],
hullR[i + 1][0][1])
if error < math.pi * 5 / 180:
self.lineSegmentR = buf
# cv2.circle(frame, self.lineSegmentL[1], 3, blue, 4)
# cv2.circle(frame, self.lineSegmentL[2], 3, blue, 4)
# cv2.circle(frame, self.lineSegmentR[1], 3, blue, 4)
# cv2.circle(frame, self.lineSegmentR[2], 3, blue, 4)
topY = frame.shape[0] / 3
bottomY = frame.shape[0]
interceptL = self.lineSegmentL[1][1] - math.tan(
self.lineSegmentL[0]) * self.lineSegmentL[1][0]
interceptR = self.lineSegmentR[1][1] - math.tan(
self.lineSegmentR[0]) * self.lineSegmentR[1][0]
lt = (topY - interceptL) / math.tan(self.lineSegmentL[0]), topY
rt = (topY - interceptR) / math.tan(self.lineSegmentR[0]), topY
lb = (bottomY - interceptL) / math.tan(self.lineSegmentL[0]), bottomY
rb = (bottomY - interceptR) / math.tan(self.lineSegmentR[0]), bottomY
if self.first == True:
self.filter = kalman.Kalman(np.array([lt[0], lb[0], rt[0], rb[0]]),
self.mNoise, self.pNoise)
self.first = False
_, pdt = self.filter.update(np.array([lt[0], lb[0], rt[0], rb[0]]))
lt = [pdt[0], lt[1]]
lb = [pdt[1], lb[1]]
rt = [pdt[2], rt[1]]
rb = [pdt[3], rb[1]]
centerX = int((lt[0] + lb[0] + rt[0] + rb[0]) / 4)
hull = cv2.convexHull(np.array([lt, lb, rb, rt], dtype='int32'))
canvas = np.zeros((frame.shape[0], frame.shape[1]), dtype='uint8')
cv2.fillConvexPoly(canvas, hull, white)
tmp = [
canvas, canvas,
np.zeros((frame.shape[0], frame.shape[1]), dtype='uint8')
]
addImg = cv2.merge(tmp)
poly = np.array([
np.array([[centerX, frame.shape[0] * 3 / 5]], dtype='int32'),
np.array([[frame.shape[1] / 3, frame.shape[0] * 7 / 8]],
dtype='int32'),
np.array([[frame.shape[1] * 2 / 3, frame.shape[0] * 7 / 8]],
dtype='int32')
])
cv2.fillConvexPoly(addImg, poly, red)
cv2.addWeighted(frame, 0.7, addImg, 0.3, 0, frame)
return centerX - frame.shape[1] / 2
class wicedsense:
# frequency of data
delta_t = .0125 # in seconds (preset to 12.5 ms)
# total duration
endtime = 5.0 # in seconds
garbageiterations = 10
inittime = 0
starttimer = 0 # wait for garbageiterations before the timer starts
calibrate = False
calibrateMagnet = False
accelCal = [0.0, 0.0, 0.0]
gyroCal = [0.0, 0.0, 0.0]
magCal = [0, 0, 0, 0, 0, 0]
vx = 0.0
ax = []
dx = 0.0
# y
vy = 0.0
ay = []
dy = 0.0
# z
vz = 0.0
az = []
dz = 0.0
gyroX = []
gyroY = []
gyroZ = []
magX = []
magY = []
magZ = []
magnitude = []
timestamp = []
def pushToCloud(self, frames, gyrodata, accel):
#print frames[1]
connection = httplib.HTTPSConnection("api.parse.com", 443)
connection.connect()
connection.request(
'PUT', '/1/classes/Putt/12fz4AHTDK',
json.dumps({
"frames": frames,
"gyro": gyrodata,
"accel": accel
}), {
"X-Parse-Application-Id":
"iAFEw9XwderX692l0DGIwoDDHcLTGMGtcBFbgMqb",
"X-Parse-REST-API-Key":
"I0xfoOS0nDqaHxfSBTgLNMuXGtsStl7zO0XZVDZX",
"Content-Type": "application/json"
})
# Init function to connect to wiced sense using mac address
# Blue tooth address is obtained from blescan.py
def __init__(self, bluetooth_adr, calibrate, calibrateMagnet):
self.con = pexpect.spawn('gatttool -b ' + bluetooth_adr +
' --interactive')
#self.con.logfile = sys.stdout
self.con.expect('\[LE\]>', timeout=600)
print "Preparing to connect. You might need to press the side button..."
self.con.sendline('connect')
# test for success of connect
self.con.expect('Connection successful.*\[LE\]>')
print "Connection successful"
self.calibrate = calibrate
self.calibrateMagnet = calibrateMagnet
self.cb = {}
return
# Function to write a value to a particular handle
def char_write_cmd(self, handle, value):
cmd = 'char-write-req 0x%02x 0%x' % (handle, value)
#print cmd
self.con.sendline(cmd)
return
def writeToFile(self, filename, text):
file = open(filename, 'w')
file.write(text)
file.close
def readFromFile(self, filename):
file = open(filename, 'r')
return file.read()
# Notification handle = 0x002b
def notification_loop(self):
if (self.calibrate == False):
calText = self.readFromFile("calibration.txt")
calArray = calText.split(",")
self.gyroCal = calArray[0:3]
self.accelCal = calArray[3:]
calText = self.readFromFile("magnetCalibration.txt")
self.magCal = calText.split(",")
print self.gyroCal
print self.accelCal
print self.magCal
total = math.ceil(self.endtime / self.delta_t)
# LOOP UNTIL TIME EXPIRES
else:
total = math.ceil(30.0 / self.delta_t)
print "set time to 30s for calibration"
iters = 0
while total > iters:
try:
pnum = self.con.expect('Notification handle = .*? \r',
timeout=4)
# wait for the BT to settle -> wait for program to cycle through garbage iterations
if self.starttimer == 1:
iters += 1
except pexpect.TIMEOUT:
print "TIMEOUT exception!"
break
try:
if pnum == 0:
after = self.con.after
hxstr = after.split()[3:]
handle = long(float.fromhex(hxstr[0]))
self.cb[handle](
[long(float.fromhex(n)) for n in hxstr[2:]])
else:
print "pnum not equal to 0"
except Exception, e:
print str(e)
'''else:
print "TIMEOUT!!"
pass'''
# After the while loop has broken...
gyroAvg = [0, 0, 0]
accelAvg = [0, 0, 0]
if (self.calibrate == True):
calText = ""
if (self.calibrateMagnet == True):
xOff = -(max(self.magX) + min(self.magX)) / 2
yOff = -(max(self.magY) + min(self.magY)) / 2
zOff = -(max(self.magZ) + min(self.magZ)) / 2
calText += str(xOff) + "," + str(yOff) + "," + str(zOff)
print calText
self.writeToFile("magnetCalibration.txt", calText)
else:
calText = ""
gyroAvg[0] = math.fsum(self.gyroX) / len(self.gyroX)
gyroAvg[1] = math.fsum(self.gyroY) / len(self.gyroY)
gyroAvg[2] = math.fsum(self.gyroZ) / len(self.gyroZ)
accelAvg[0] = math.fsum(self.ax) / len(self.ax)
accelAvg[1] = math.fsum(self.ay) / len(self.ay)
accelAvg[2] = math.fsum(self.az) / len(self.az)
calText += str(0 - gyroAvg[0]) + "," + str(
0 - gyroAvg[1]) + "," + str(0 - gyroAvg[2]) + ","
calText += str(0 - accelAvg[0]) + "," + str(
0 - accelAvg[1]) + "," + str(8192 - accelAvg[2])
self.writeToFile("calibration.txt", calText)
else:
# FILTER OUT INITIAL ACCELERATION VALUES --------------
thresh = 9.9 # accel treshold must be exceeded to indicate putt has begun (m/s^2)
axnew = [] # new acceleration list in the x direction
aynew = [] # ... y direction
aznew = [] # ... z direction
accelNew = []
gyroXnew = []
gyroYnew = []
gyroZnew = []
magnew = []
for x in range(len(self.magnitude)):
print self.magnitude[x]
#if self.magnitude[x] > thresh:
if True:
print "PUTTING................."
axnew = self.ax[x:]
aynew = self.ay[x:]
aznew = self.az[x:]
magnew = self.magnitude[x:]
gyroXnew = self.gyroX[x:]
gyroYnew = self.gyroY[x:]
gyroZnew = self.gyroZ[x:]
#self.magX = self.magX[x:]
#self.magY = self.magY[x:]
#self.magZ = self.magZ[x:]
break
# ========================
# GET DISPLACEMENT FRAMES
# ========================
for x in range(len(axnew)):
accelNew.append([axnew[x], aynew[x], aznew[x], magnew[x]])
roll = []
pitch = []
yaw = []
roll.append(MathUtil.roll(axnew[0], aynew[0], aznew[0]))
pitch.append(MathUtil.pitch(axnew[0], aynew[0], aznew[0]))
#yaw.append(MathUtil.yaw(roll[0], pitch[0], self.magX[0], self.magY[0], self.magZ[0]))
for x in range(1, len(axnew)):
roll.append(MathUtil.roll(axnew[x], aynew[x], aznew[x]))
pitch.append(MathUtil.pitch(axnew[x], aynew[x], aznew[x]))
#yaw.append(MathUtil.yaw(roll[x], pitch[x], self.magX[x], self.magY[x], self.magZ[x]))
# OUTPUT TO SCREEN ------------------
'''
print "axnew:"
for x in range(0,len(axnew)): print axnew[x]
print
print "axnew length"
print len(axnew)
print
print "xyzframes (frame indicates the x displacement in meters at a given time):"
for x in range(0,len(xyzframes)): print xyzframes[x]
print
print "total duration (in s) "
print (float(len(axnew)))*delta_t
print
print "total samples"
print int (len(axnew)) + 1 # add one for time 0
print
'''
#Kalman filtering
gyrodata = []
kalmanX = kalman.Kalman(roll[0])
kalmanY = kalman.Kalman(pitch[0])
gyrodata.append([roll[0], pitch[0], 0.0])
zAngle = [0.0]
xAngle = [0.0]
yAngle = [0.0]
time = [0.0]
data = [0.0]
angle = [0.0]
for x in range(1, len(gyroXnew)):
time.append(time[-1] + self.delta_t)
data.append(MathUtil.getAngle(gyroZnew[x], self.delta_t))
xAngle.append(xAngle[-1] +
MathUtil.getAngle(gyroXnew[x], self.delta_t))
yAngle.append(yAngle[-1] +
MathUtil.getAngle(gyroYnew[x], self.delta_t))
zAngle.append(zAngle[-1] + data[-1])
gyrodata.append([
kalmanX.updateAngle(roll[x], gyroXnew[x], self.delta_t),
kalmanY.updateAngle(pitch[x], gyroYnew[x], self.delta_t),
zAngle[x]
])
accelRot = MathUtil.rotateAcceleration(
[axnew, aynew, aznew],
[[i[0] for i in gyrodata[:]], [i[1] for i in gyrodata[:]],
[i[2] for i in gyrodata[:]]])
#displacement calculations
xframes = [
MathUtil.displacement(self.dx, self.vx, accelRot[0][0],
self.delta_t)[0]
]
yframes = [
MathUtil.displacement(self.dy, self.vy, accelRot[1][0],
self.delta_t)[0]
]
zframes = [
MathUtil.displacement(self.dz, self.vz, accelRot[2][0],
self.delta_t)[0]
]
xyzframes = [[xframes[-1], yframes[-1], zframes[-1]]]
for x in range(1, len(gyroXnew)):
self.dx, self.vx = MathUtil.displacement(
self.dx, self.vx, accelRot[0][x], self.delta_t)
xframes.append(float(xframes[-1] + self.dx))
self.dy, self.vy = MathUtil.displacement(
self.dy, self.vy, accelRot[1][x], self.delta_t)
yframes.append(float(yframes[-1] + self.dy))
self.dz, self.vz = MathUtil.displacement(
self.dz, self.vz, accelRot[2][x], self.delta_t)
zframes.append(float(zframes[-1] + self.dz))
xyzframes.append([xframes[-1], yframes[-1], zframes[-1]])
accelNew[x] = [
accelRot[0][x], accelRot[1][x], accelRot[2][x],
accelNew[x][3]
]
# =======================
# PUSH FRAMES TO PARSE
# =======================
self.pushToCloud(xyzframes, gyrodata, accelNew)
self.resetVars()
import numpy as np
import time
import math
from decimal import Decimal
import random
import kalman
n = 5
x0 = np.zeros((n * 2, 1))
P0 = np.ones((2 * n, 2 * n)) #np.diag([1]*(2*n))
kalman = kalman.Kalman(x0, P0)
estimates_history = [
[] for i in range(0, n)
] #inizialization as list of empty lists (as many lists as the number of anchors)
last_times = np.zeros((n, 1))
last_time = None
history_length = 10
def updateHistory(measures):
#for each of them update the history of etimated distances
for m in measures:
estimates_history[m['id']].append(m['rssi'])
#if there are enough value, remove oldest
if len(estimates_history[m['id']]) > history_length:
estimates_history[m['id']].pop(0)
def historyMean():
means = [(sum(i) / len(i) if len(i) != 0 else 0)
for i in estimates_history]
def update(self, im1, im2, rect):
self.rect = rect
self.im1 = im1
self.im2 = im2
self.measure()
self.kalman = kalman.Kalman(self.rect, self.measured)
# model
M = np.eye(3)
# model uncertainty
Q_red = np.eye(3) * 0.005**2
Q_nir = np.eye(3) * 0.02**2
# initial conditions
x0_red = np.array([0.08996554, 0.0083535, 0.04541085])
x0_nir = np.array([0.44320984, 0.3960118, 0.])
C_red = np.eye(3) * 0.2**2
C_nir = np.eye(3) * 0.5**2
import kalman
# red
kfr = kalman.Kalman(red, x0_red, C_red, R_red, M, Q_red, H, doys)
plt.clf()
kfr.run()
plt.ylim(0, 1)
plt.errorbar(kfr.alldoys,
kfr.x[:, 0],
yerr=np.sqrt(kfr.C[:, 0, 0]),
fmt='ro',
label='fiso')
plt.errorbar(kfr.alldoys,
kfr.x[:, 1],
yerr=np.sqrt(kfr.C[:, 1, 1]),
fmt='bo',
label='fvol')
plt.errorbar(kfr.alldoys,
kfr.x[:, 2],
########### Initialize ROS Node r ###############
####################################################
rospy.init_node('pretouch_acoustic_sensing_node')
pub = rospy.Publisher('pretouch', PretouchAcoustic)
msg = PretouchAcoustic()
msg.header.frame_id = "/r_gripper_r_finger_tip_link"
seq = 0
####################################################
########### Initialize Kalman Filter ###############
####################################################
print "***** Initialize Kalman Filter ***********"
ndim = 1
Sigma_x = 1e-5 #Process Vriance
R = 0.01**2 #Measurement Variance = 0.01**2
k = kalman.Kalman(ndim, Sigma_x, R)
mu_init = np.array([2000])
####################################################
##### Initialize PyAudio Input Stream ##############
####################################################
# Find the actual device ID used by PyAudio
device_id = helper.find_audio_device(name='Lexicon Omega: USB Audio (hw:1,0)')
print device_id
# Initialize Audio Input Streaming
p = pyaudio.PyAudio()
stream = p.open(format=FORMAT,
channels=CHANNELS,
rate=RATE,
input=True,
input_device_index=device_id,
