def pix2sky(header,x,y): hdr_info = parse_header(header) x0 = x-hdr_info[1][0]+1. # Plus 1 python->image y0 = y-hdr_info[1][1]+1. x0 = x0.astype(scipy.float64) y0 = y0.astype(scipy.float64) x = hdr_info[2][0,0]*x0 + hdr_info[2][0,1]*y0 y = hdr_info[2][1,0]*x0 + hdr_info[2][1,1]*y0 if hdr_info[3]=="DEC": a = x.copy() x = y.copy() y = a.copy() ra0 = hdr_info[0][1] dec0 = hdr_info[0][0]/raddeg else: ra0 = hdr_info[0][0] dec0 = hdr_info[0][1]/raddeg if hdr_info[5]=="TAN": r_theta = scipy.sqrt(x*x+y*y)/raddeg theta = arctan(1./r_theta) phi = arctan2(x,-1.*y) elif hdr_info[5]=="SIN": r_theta = scipy.sqrt(x*x+y*y)/raddeg theta = arccos(r_theta) phi = artan2(x,-1.*y) ra = ra0 + raddeg*arctan2(-1.*cos(theta)*sin(phi-pi), sin(theta)*cos(dec0)-cos(theta)*sin(dec0)*cos(phi-pi)) dec = raddeg*arcsin(sin(theta)*sin(dec0)+cos(theta)*cos(dec0)*cos(phi-pi)) return ra,dec
def pix2sky(header,x,y): hdr_info = parse_header(header) x0 = x-hdr_info[1][0]+1. # Plus 1 python->image y0 = y-hdr_info[1][1]+1. x0 = x0.astype(scipy.float64) y0 = y0.astype(scipy.float64) x = hdr_info[2][0,0]*x0 + hdr_info[2][0,1]*y0 y = hdr_info[2][1,0]*x0 + hdr_info[2][1,1]*y0 if hdr_info[3]=="DEC": a = x.copy() x = y.copy() y = a.copy() ra0 = hdr_info[0][1] dec0 = hdr_info[0][0]/raddeg else: ra0 = hdr_info[0][0] dec0 = hdr_info[0][1]/raddeg if hdr_info[5]=="TAN": r_theta = scipy.sqrt(x*x+y*y)/raddeg theta = arctan(1./r_theta) phi = arctan2(x,-1.*y) elif hdr_info[5]=="SIN": r_theta = scipy.sqrt(x*x+y*y)/raddeg theta = arccos(r_theta) phi = artan2(x,-1.*y) ra = ra0 + raddeg*arctan2(-1.*cos(theta)*sin(phi-pi), sin(theta)*cos(dec0)-cos(theta)*sin(dec0)*cos(phi-pi)) dec = raddeg*arcsin(sin(theta)*sin(dec0)+cos(theta)*cos(dec0)*cos(phi-pi)) return ra,dec
def transformPicture(self, accelData):
x, y, z = accelData[0], accelData[1], accelData[2]
offset = 512
# rotate
if self.wm.buttons["A"]:
self.c += 1
print(self.c)
if self.c % 2 == 0:
centeredZ = z - offset
centeredX = x - offset
rot_angle = int(
-(scipy.degrees(scipy.arctan2(centeredZ, centeredX)) - 90))
if rot_angle < 0:
rot_angle = 360 + rot_angle
self.image.setPixmap(
self.pixmap.transformed(
QtGui.QTransform().rotate(rot_angle),
1).scaledToHeight(400))
# zoom
if self.wm.buttons["Down"]:
centeredZ = z - offset
centeredY = y - offset
tilt_angle = scipy.degrees(scipy.arctan2(centeredZ,
centeredY)) - 90
scale_val = abs(tilt_angle / 100)
self.image.setPixmap(
self.pixmap.transformed(QtGui.QTransform().scale(
scale_val, scale_val)))
def to_YPR(quaternion):
q0, q1, q2, q3 = quaternion
roll = sp.arctan2(2 * (q0 * q1 + q2 * q3), (q0**2 - q1**2 - q2**2 + q3**2))
pitch = sp.arcsin(-2 * (q1 * q3 - q0 * q2))
yaw = sp.arctan2(2 * (q1 * q2 + q0 * q3), (q0**2 + q1**2 - q2**2 - q3**2))
return roll, pitch, yaw
def detect_skew(img, min_angle=-20, max_angle=20, quality='low'):
img = sp.atleast_2d(img)
rows, cols = img.shape
min_min_angle = min_angle
max_max_angle = max_angle
if quality == 'low':
resolution = sp.arctan2(2.0, cols) * 180.0 / sp.pi
min_target_size = 100
resize_order = 1
elif quality == 'high':
resolution = sp.arctan2(1.0, cols) * 180.0 / sp.pi
min_target_size = 300
resize_order = 3
else:
resolution = sp.arctan2(1.0, cols) * 180.0 / sp.pi
min_target_size = 200
resize_order = 2
# resize the image so it's faster to work with
min_size = min(rows, cols)
target_size = min_target_size if min_size > min_target_size else min_size
resize_ratio = float(target_size) / min_size
img = imresize(img, resize_ratio)
rows, cols = img.shape
# pad the image and invert the colors
img *= -1
img += 255
padded_img = sp.zeros((rows*2, cols*2))
padded_img[rows//2:rows//2+rows, cols//2:cols//2+cols] = img
img = padded_img
# keep dividing the interval in half to achieve O(log(n))
while True:
current_resolution = (max_angle - min_angle) / 30.0
best_angle = None
best_variance = 0.0
# rotate the image, sum the pixel values in each row for each rotation
# then find the variance of all the sums, pick the highest variance
for i in xrange(31):
angle = min_angle + i * current_resolution
rotated_img = rotate(img, angle, reshape=False, order=resize_order)
num_black_pixels = sp.sum(rotated_img, axis=1)
variance = sp.var(num_black_pixels)
if variance > best_variance:
best_angle = angle
best_variance = variance
if current_resolution < resolution:
break
# update the angle range
min_angle = max(best_angle - current_resolution, min_min_angle)
max_angle = min(best_angle + current_resolution, max_max_angle)
return best_angle
def euler_from_qarray(q,tol=0.499):
"""
Get array of euler angles from array of quaternions. Note, array of euler angles
will be in columns of [heading,attitude,bank].
Euler angle convention - similar to NASA standard airplane, but with y and z axes
swapped to conform with x3d. (in order of application)
1.0 rotation about y-axis
2.0 rotation about z-axis
3.0 rotation about x-axis
"""
qw = q[:,0]
qx = q[:,1]
qy = q[:,2]
qz = q[:,3]
head = scipy.zeros((q.shape[0],))
atti = scipy.zeros((q.shape[0],))
bank = scipy.zeros((q.shape[0],))
test = qx*qy - qz*qw # test for north of south pole
# Points not at north or south pole
mask0 = scipy.logical_and(test <= tol, test >= -tol)
qw_0 = qw[mask0]
qx_0 = qx[mask0]
qy_0 = qy[mask0]
qz_0 = qz[mask0]
head[mask0] = scipy.arctan2(2.0*qy_0*qw_0 - 2.0*qx_0*qz_0, 1.0 - 2.0*qy_0**2 - 2.0*qz_0**2)
atti[mask0] = scipy.arcsin(2.0*qx_0*qy_0 + 2.0*qz_0*qw_0)
bank[mask0] = scipy.arctan2(2.0*qx_0*qw_0 - 2.0*qy_0*qz_0, 1.0 - 2.0*qx_0**2 - 2.0*qz_0**2)
# Points at north pole
mask1 = test > tol
qw_1 = qw[mask1]
qx_1 = qx[mask1]
qy_1 = qy[mask1]
qz_1 = qz[mask1]
head[mask1] = 2.0*scipy.arctan2(qx_1,qw_1)
atti[mask1] = scipy.arcsin(2.0*qx_1*qy_1 + 2.0*qz_1*qw_1)
bank[mask1] = scipy.zeros((qw_1.shape[0],))
# Points at south pole
mask2 = test < -tol
qw_2 = qw[mask2]
qx_2 = qx[mask2]
qy_2 = qy[mask2]
qz_2 = qz[mask2]
head[mask2] = -2.0*scipy.arctan2(qx_2,qw_2)
atti[mask2] = scipy.arcsin(2.0*qx_2*qy_2 + 2.0*qz_2*qw_2)
bank[mask2] = scipy.zeros((qw_2.shape[0],))
euler_angs = scipy.zeros((q.shape[0],3))
euler_angs[:,0] = head
euler_angs[:,1] = atti
euler_angs[:,2] = bank
return euler_angs
def update(self, order, dt=0.1): angle = self.X[0] V = 150*speed(self.X) order_stick = order + sp.arctan2(self.X[2], self.X[1])/sp.pi/2 dx0 = self.k*(windForce(self.X, 400) +0.)*(order_stick-0.*sp.sin(sp.arctan2(self.X[2], self.X[1])-sp.pi/2)) dx1 = -V*sp.sin(angle) + self.kdrift*order*sp.cos(angle) dx2 = V*sp.cos(angle) + self.kdrift*order*sp.sin(angle)-30 dX = dt*np.array([dx0[0], dx1[0], dx2[0]]) self.X = self.X + dX self.X = np.array([self.X[0], self.X[1], np.max([0, self.X[2]])])
def cart2sph(xyz):
""" Radius, azimuth follow the canonical definition.
Elevation is the angle in [-pi, pi], with 0 being the
isoluminant plane (z = 0 in cartesian coordinates).
"""
rae = sp.zeros_like(xyz)
x, y, z = xyz[..., 0], xyz[..., 1], xyz[..., 2]
rae[..., 0] = sp.sqrt(x**2 + y**2 + z**2) # radius
rae[..., 1] = sp.arctan2(y, x) # azimuth
rae[..., 2] = sp.arctan2(z, sp.sqrt(x**2 + y**2)) # elevation
return rae
def update(self, order, dt=0.1):
angle = self.X[0]
V = 150 * speed(self.X)
order_stick = order + sp.arctan2(self.X[2], self.X[1]) / sp.pi / 2
dx0 = self.k * (windForce(self.X, 400) + 0.) * (
order_stick -
0. * sp.sin(sp.arctan2(self.X[2], self.X[1]) - sp.pi / 2))
dx1 = -V * sp.sin(angle) + self.kdrift * order * sp.cos(angle)
dx2 = V * sp.cos(angle) + self.kdrift * order * sp.sin(angle) - 30
dX = dt * np.array([dx0[0], dx1[0], dx2[0]])
self.X = self.X + dX
self.X = np.array([self.X[0], self.X[1], np.max([0, self.X[2]])])
def matrixToEuler(m,order='Aerospace',inDegrees=True):
if order == 'Aerospace' or order == 'ZYX':
sp = -m[2,0]
if sp < (1-EPS):
if sp > (-1+EPS):
p = arcsin(sp)
r = arctan2(m[2,1],m[2,2])
y = arctan2(m[1,0],m[0,0])
else:
p = -pi/2.
r = 0
y = pi-arctan2(-m[0,1],m[0,2])
else:
p = pi/2.
y = arctan2(-m[0,1],m[0,2])
r = 0
if inDegrees:
return degrees((y,p,r))
else:
return (y,p,r)
elif order == 'BVH' or order == 'ZXY':
sx = m[2,1]
if sx < (1-EPS):
if sx > (-1+EPS):
x = arcsin(sx)
z = arctan2(-m[0,1],m[1,1])
y = arctan2(-m[2,0],m[2,2])
else:
x = -pi/2
y = 0
z = -arctan2(m[0,2],m[0,0])
else:
x = pi/2
y = 0
z = arctan2(m[0,2],m[0,0])
if inDegrees:
return degrees((z,x,y))
else:
return (z,x,y)
elif order == "ZXZ":
x = arccos(m[2,2])
z2 = arctan2(m[2,0],m[2,1])
z1 = arctan2(m[0,2],-m[1,2])
if inDegrees:
return degrees((z1,x,z2))
else:
return (z1,x,z2)
def detect_skew(img, min_angle=-20, max_angle=20, quality='low'):
img = sp.atleast_2d(img)
rows, cols = img.shape
min_min_angle = min_angle
max_max_angle = max_angle
if quality == 'low':
resolution = sp.arctan2(2.0, cols) * 180.0 / sp.pi
min_target_size = 100
resize_order = 1
elif quality == 'high':
resolution = sp.arctan2(1.0, cols) * 180.0 / sp.pi
min_target_size = 300
resize_order = 3
else:
resolution = sp.arctan2(1.0, cols) * 180.0 / sp.pi
min_target_size = 200
resize_order = 2
# resize the image so it's faster to work with
min_size = min(rows, cols)
target_size = min_target_size if min_size > min_target_size else min_size
resize_ratio = float(target_size) / min_size
img = imresize(img, resize_ratio)
rows, cols = img.shape
img *= -1
img += 255
while True:
current_resolution = (max_angle - min_angle) / 30.0
angles = sp.linspace(min_angle, max_angle, 31)
# do the hough transfer
hough_out = _hough_transform(img, angles * sp.pi / 180)
# determine which angle gives max variance
variances = sp.var(hough_out, axis=0)
max_variance_index = sp.argmax(variances)
best_angle = min_angle + max_variance_index * current_resolution
if current_resolution < resolution:
break
# update the angle range
min_angle = max(best_angle - current_resolution, min_min_angle)
max_angle = min(best_angle + current_resolution, max_max_angle)
return best_angle
def xyz2lbr (x, y, z, d0=dsun):
""" convert galactic xyz into sun-centered lbr coordinates; derived from stCoords.c"""
#if len(xyz.shape) > 1: x, y, z = xyz[:,0], xyz[:,1], xyz[:,2]
#else: x, y, z = xyz[0], xyz[1], xyz[2]
xsun = x + d0
temp = (xsun*xsun) + (y*y)
l = sc.arctan2(y, xsun) * deg
b = sc.arctan2(z, sc.sqrt(temp)) * deg
r = sc.sqrt(temp + (z*z))
if type(l) == type(arr):
for i in range(len(l)):
if l[i] < 0.0: l[i] = l[i] + 360.0
else:
if l < 0.0: l = l + 360.0
return l,b,r
def fast_kappa(z1,r1,w1,d1,z2,r2,w2,d2,ang,same_half_plate, \
th1, phi1, th2, phi2): # SY
rp = abs(r1-r2[:,None])*sp.cos(ang/2)
rt = (r1+r2[:,None])*sp.sin(ang/2)
d12 = d1*d2[:, None]
w12 = w1*w2[:,None]
w = (rp>=kappa.rp_min) & (rp<=kappa.rp_max) & \
(rt<=kappa.rt_max) & (rt>=kappa.rp_min)
rp = rp[w]
rt = rt[w]
w12 = w12[w]
d12 = d12[w]
x = (phi2 - phi1)/sp.sin(th1) # SY
y = th2 - th1 # SY
gamma_ang = sp.arctan2(y,x) # SY
#-- getting model and first derivative
xi_model = kappa.xi2d(rt, rp, grid=False)
xip_model = kappa.xi2d(rt, rp, dx=1, grid=False)
R = 1/(xip_model*rt) # SY
ska = sp.sum( (d12 - xi_model)/R*w12 )
wka = sp.sum( w12/R**2 )
gam1 = -2*sp.cos(2*gamma_ang) * sp.sum( (d12 - xi_model)/R*w12 ) # SY
gam2 = 2*sp.sin(2*gamma_ang) * sp.sum( (d12 - xi_model)/R*w12 ) # SY
return ska, wka, gam1, gam2 # SY
def geodetic_from_ecef(x, y, z):
#http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
"""Convert ECEF coordinates to geodetic.
J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates \
to geodetic coordinates," IEEE Transactions on Aerospace and \
Electronic Systems, vol. 30, pp. 957-961, 1994."""
# load wgs constants
wgs = wgs_constants()
a = wgs.a
b = wgs.b
esq = wgs.esq
e1sq = wgs.e1sq
r = sqrt(x * x + y * y)
Esq = a * a - b * b
F = 54 * b * b * z * z
G = r * r + (1 - esq) * z * z - esq * Esq
C = (esq * esq * F * r * r) / (pow(G, 3))
S = cbrt(1 + C + sqrt(C * C + 2 * C))
P = F / (3 * pow((S + 1 / S + 1), 2) * G * G)
Q = sqrt(1 + 2 * esq * esq * P)
r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - \
P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r)
#U = sqrt(pow((r - esq * r_0), 2) + z * z)
V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z)
Z_0 = b * b * z / (a * V)
#h = U * (1 - b * b / (a * V))
lat = arctan((z + e1sq * Z_0) / r)
lon = arctan2(y, x)
return lat, lon
def resolve_tri(A,B,a,b,up=True):
AB=A-B
c=l.norm(AB)
aa=s.arctan2(AB[1],AB[0])
bb=s.arccos((b**2+c**2-a**2)/(2*b*c))
if up: return B+b*s.array((s.cos(aa+bb),s.sin(aa+bb)))
else: return B+b*s.array((s.cos(aa-bb),s.sin(aa-bb)))
def rect_to_cyl(X,Y,Z):
"""
NAME:
rect_to_cyl
PURPOSE:
convert from rectangular to cylindrical coordinates
INPUT:
X, Y, Z - rectangular coordinates
OUTPUT:
R,phi,z
HISTORY:
2010-09-24 - Written - Bovy (NYU)
"""
R= sc.sqrt(X**2.+Y**2.)
phi= sc.arctan2(Y,X)
return (R,phi,Z)
def vec2polar(vec):
'''transform a vector to polar axis.'''
r = np.linalg.norm(vec, axis=-1, keepdims=True)
theta = np.arccos(vec[..., 2:3] / r)
phi = np.arctan2(vec[..., 1:2], vec[..., :1])
res = np.concatenate([r, theta, phi], axis=-1)
return res
def draw_probe_ellipse(xy, covar, alpha, color=None, **kwargs):
"""Generates an ellipse object based of a point and related
covariance assuming 2 dimensions
Args:
xy (2x1 array): (x,y) of the ellipse position
covar (2x2 array): covariance matrix of landmark point
alpha (float):
Kwargs:
color (string): matplotlib color convention
Returns:
(matplotlib Ellipse Object): Ellipse object for drawing
"""
b24ac = scipy.sqrt(pow(covar[0, 0] - covar[1, 1], 2) + 4 * covar[0, 1])
c2inv = chi2.ppf(alpha, 2.) / 1e2
a = scipy.real(scipy.sqrt(c2inv * .5 *
(covar[0, 0] + covar[1, 1] + b24ac)))
b = scipy.real(scipy.sqrt(c2inv * .5 *
(covar[0, 0] + covar[1, 1] - b24ac)))
theta = .5 * scipy.arctan2(2 * covar[0, 1], covar[0, 0] - covar[1, 1])
return Ellipse(xy, a, b, angle=theta, color=color, **kwargs)
def groove(inclination, rdh, brise, minorgroove):
#~print "newtongroove\n"
#global strutrise, dthgroove
dthgroove = math.pi
#~print "inclination=%f, rdh=%f, brise=%f, minorgroove=%f"%(inclination,rdh,brise,minorgroove)
sa = math.sin(dthgroove - math.pi)
ca = math.cos(dthgroove - math.pi)
strutrise = rdh*math.tan(inclination)*math.sqrt(math.pow(sa,2) +\
math.pow(1 + ca,2)) # rise of strut along its length
f = dthgroove / (2 * math.pi) * brise - strutrise - minorgroove
#~print "dthgroove=%s\nsa=%s\nca=%s\nstrutrise=%s\nf=%s"%(dthgroove, sa,ca,strutrise,f)
for iter in range(
3
): # use Newton's method to find radius on which bases fall for this loop (default: 3 steps)
dstrutdth = rdh*math.tan(inclination)* 0.5*math.pow(math.pow(sa, 2) + \
math.pow(1+ca, 2), -0.5) \
*(2*sa*ca - 2*(1 + ca)*sa)
dfdth = brise / (2 * math.pi) - dstrutdth
dthgroove = dthgroove - f / dfdth
#~print "dstrutdth=%s dfdth=%s dthgroove=%s"%(dstrutdth, dfdth, dthgroove)
sa = math.sin(dthgroove - math.pi)
ca = math.cos(dthgroove - math.pi)
strutrise = rdh * math.tan(inclination) * math.sqrt(
pow(sa, 2) + pow(1 + ca, 2))
f = dthgroove / (2 * math.pi) * brise - strutrise - minorgroove
#~print 'sa=%f ca=%f strutrise=%f f=%f'%(sa, ca, strutrise, f)
strutlength = rdh / math.cos(inclination) * math.sqrt(
pow(sa, 2) + pow(1 + ca, 2)) #length of strut
strutangle = math.pi + scipy.arctan2(
rdh * sa, rdh * (1 + ca)) # angle of strut relative to x axis
# with origin at one end of strut and rotating
# around z axis
return dthgroove, strutrise, strutlength, strutangle
def measurement_model(particle, z):
""" compute the expected measurement for a landmark
and the Jacobian with respect to the landmark"""
# extract the id of the landmark
landmarkId = z['id']
# two 2D vector for the position (x,y) of the observed landmark
landmarkPos = particle['landmarks'][landmarkId]['mu']
# TODO: use the current state of the particle to predict the measurment
landmarkX = landmarkPos[0]
landmarkY = landmarkPos[1]
expectedRange = scipy.sqrt(pow(landmarkX - particle.pose[0], 2) + pow(landmarkY - particle.pose[1], 2))
expectedBearing = normalize_angle(scipy.arctan2(landmarkY - particle.pose[1], landmarkX - particle.pose[0]) - particle.pose[2])
h = scipy.array([expectedRange, expectedBearing])
# TODO: Compute the Jacobian H of the measurement function h wrt the landmark location
H = scipy.zeros(2,2)
H[0,0] = (landmarkX - particle.pose[0])/expectedRange
H[0,1] = (landmarkY - particle.pose[1])/expectedRange
H[1,0] = (particle.pose[1] - landmarkY)/pow(expectedRange, 2)
H[1,1] = (landmarkX - particle.pose[0])/pow(expectedRange, 2)
return h, H
def read_data(self):
unit = 1 + int(self.Lbu.curselection()[0])
if (unit == 1):
self.ff, self.zr, self.zi, self.num = read_three_columns_from_dialog(
'Select Input File', self.master)
else:
self.ff, self.zc, self.num = read_two_columns_from_dialog(
'Select Input File', self.master)
self.zi = zeros(self.num, 'f')
self.zr = zeros(self.num, 'f')
for i in range(0, self.num):
arg = self.ph[i] * (pi / 180)
self.zi[i] = self.zc[i].real
self.zr[i] = self.zc[i].imag
self.zz = zeros(self.num, 'f')
self.ph = zeros(self.num, 'f')
for i in range(0, self.num):
self.zz[i] = sqrt(self.zr[i]**2 + self.zi[i]**2)
self.ph[i] = arctan2(self.zi[i], self.zr[i]) * 180 / pi
self.button_replot.config(state='normal')
self.plotd(self)
def get_freq_response(f0, f1, m, b, n=5000):
"""
Get frequency response for system. Returns gain, phase and frequency.
Inputs:
f0 = starting frequency
f1 = stopping frequency
m = mass of system
b = damping coefficient
Outputs:
mag_db = gain (output/input)
phase = phase shift in degrees
f = array of frequencies
"""
def transfer_func(s, m, b):
return 1.0 / (m * s + b)
f = scipy.linspace(f0, f1, n)
x = 2.0 * scipy.pi * f * 1j
y = transfer_func(x, m, b)
mag = scipy.sqrt(y.real**2 + y.imag**2)
phase = scipy.arctan2(y.imag, y.real)
phase = scipy.rad2deg(phase)
mag_db = 20.0 * scipy.log10(mag)
return mag_db, phase, f
def rect_to_cyl(X, Y, Z):
"""
NAME:
rect_to_cyl
PURPOSE:
convert from rectangular to cylindrical coordinates
INPUT:
X, Y, Z - rectangular coordinates
OUTPUT:
R,phi,z
HISTORY:
2010-09-24 - Written - Bovy (NYU)
"""
R = sc.sqrt(X**2. + Y**2.)
phi = sc.arctan2(Y, X)
if isinstance(phi, nu.ndarray): phi[phi < 0.] += 2. * nu.pi
elif phi < 0.: phi += 2. * nu.pi
return (R, phi, Z)
def fast_kappa_true(z1, r1, zq, rq, ang, ang_lens, \
th1, phi1, thq, phiq):
rp = abs(r1[:, None] - rq) * sp.cos(ang / 2)
rt = (r1[:, None] + rq) * sp.sin(ang / 2)
rp_lens = abs(r1[:, None] - rq) * sp.cos(ang_lens / 2)
rt_lens = (r1[:, None] + rq) * sp.sin(ang_lens / 2)
w = (rp>=kappa.rp_min) & (rp<=kappa.rp_max) & \
(rt<=kappa.rt_max) & (rt>=kappa.rt_min)
rp = rp[w]
rt = rt[w]
rp_lens = rp_lens[w]
rt_lens = rt_lens[w]
x = (phiq - phi1) * sp.sin(th1)
y = thq - th1
gamma_ang = sp.arctan2(y, x)
#-- getting model and first derivative
xi_model = kappa.xi2d(rt, rp, grid=False)
xi_lens = kappa.xi2d(rt_lens, rp_lens, grid=False)
xip_model = kappa.xi2d(rt, rp, dx=1, grid=False)
R = 1 / (xip_model * rt)
ska = sp.sum((xi_lens - xi_model) / R)
wka = sp.sum(1 / R**2)
gam1 = 4 * sp.cos(2 * gamma_ang) * sp.sum((xi_lens - xi_model) / R)
gam2 = 4 * sp.sin(2 * gamma_ang) * sp.sum((xi_lens - xi_model) / R)
return ska, wka, gam1, gam2
def fast_kappa(z1,r1,w1,d1,zq,rq,wq,ang, \
th1, phi1, thq, phiq):
rp = (r1[:, None] - rq) * sp.cos(ang / 2)
rt = (r1[:, None] + rq) * sp.sin(ang / 2)
we = w1[:, None] * wq
de = d1[:, None]
w = (rp>=kappa.rp_min) & (rp<=kappa.rp_max) & \
(rt<=kappa.rt_max) & (rt>=kappa.rt_min)
rp = rp[w]
rt = rt[w]
we = we[w]
de = de[w]
x = (phiq - phi1) * sp.sin(th1)
y = thq - th1
gamma_ang = sp.arctan2(y, x)
#-- getting model and first derivative
xi_model = kappa.xi2d(rt, rp, grid=False)
xip_model = kappa.xi2d(rt, rp, dx=1, grid=False)
#-- weight of estimator
R = 1 / (xip_model * rt)
ska = sp.sum((de - xi_model) / R * we)
wka = sp.sum(we / R**2)
gam1 = 4 * sp.cos(2 * gamma_ang) * sp.sum((de - xi_model) / R * we)
gam2 = 4 * sp.sin(2 * gamma_ang) * sp.sum((de - xi_model) / R * we)
return ska, wka, gam1, gam2
def ecef2geodetic(x, y, z):
"""Convert ECEF coordinates to geodetic.
J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates \
to geodetic coordinates," IEEE Transactions on Aerospace and \
Electronic Systems, vol. 30, pp. 957-961, 1994."""
a = 6378.137
b = 6356.7523142
esq = 6.69437999014 * 0.001
e1sq = 6.73949674228 * 0.001
# return h in kilo
r = sqrt(x * x + y * y)
Esq = a * a - b * b
F = 54 * b * b * z * z
G = r * r + (1 - esq) * z * z - esq * Esq
C = (esq * esq * F * r * r) / (pow(G, 3))
S = sqrt(1 + C + sqrt(C * C + 2 * C))
P = F / (3 * pow((S + 1 / S + 1), 2) * G * G)
Q = sqrt(1 + 2 * esq * esq * P)
r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - \
P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r)
U = sqrt(pow((r - esq * r_0), 2) + z * z)
V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z)
Z_0 = b * b * z / (a * V)
h = U * (1 - b * b / (a * V))
lat = arctan((z + e1sq * Z_0) / r)
lon = arctan2(y, x)
return degrees(lat), degrees(lon), h
def rect_to_cyl(X,Y,Z):
"""
NAME:
rect_to_cyl
PURPOSE:
convert from rectangular to cylindrical coordinates
INPUT:
X, Y, Z - rectangular coordinates
OUTPUT:
R,phi,z
HISTORY:
2010-09-24 - Written - Bovy (NYU)
"""
R= sc.sqrt(X**2.+Y**2.)
phi= sc.arctan2(Y,X)
if isinstance(phi,nu.ndarray): phi[phi<0.]+= 2.*nu.pi
elif phi < 0.: phi+= 2.*nu.pi
return (R,phi,Z)
def north_direction(lat):
'''get the north direction relative to image positive y coordinate'''
dlatdx = nd.filters.sobel(lat,axis=1,mode='constant',cval=sp.nan) #gradient in x-direction
dlatdy = nd.filters.sobel(lat,axis=0,mode='constant',cval=sp.nan)
ydir = lat[-1,0] -lat[0,0] # check if latitude is ascending or descending in y axis
# same step might have to be done with x direction.
return sp.arctan2(dlatdx,dlatdy*sp.sign(ydir) )*180/sp.pi
def _update_strain(self):
self.e = (self.exy - self.eyx) / 2
self.k = (self.exx + self.eyy) * 1000000. / 2
self.strain = scipy.sqrt((self.exx - self.eyy) * (self.exx - self.eyy) + (self.exy + self.eyx) * (self.exy + self.eyx)) * 1000000.
self.k_max = self.k + self.strain / 2
self.k_min = self.k - self.strain / 2
self.az = scipy.degrees(2 * scipy.arctan2(self.exy + self.eyx, self.eyy - self.exx))
def north_direction(lat):
'''get the north direction relative to image positive y coordinate'''
dlatdx = nd.filters.sobel(lat,axis=1,mode='constant',cval=sp.nan) #gradient in x-direction
dlatdy = nd.filters.sobel(lat,axis=0,mode='constant',cval=sp.nan)
ydir = lat[-1,0] -lat[0,0] # check if latitude is ascending or descending in y axis
# same step might have to be done with x direction.
return sp.arctan2(dlatdx,dlatdy*sp.sign(ydir) )*180/sp.pi
def align_to_world(gnss_position, vectors, motion_time):
"""
Align accelerations to world system (x axis going to east, y to north)
:param gnss_position: 3xn numpy array. positions from gnss data
:param vectors: 3xn numpy array
:param stationary_times: list of tuples
:param angular_positions:
:return: 2 numpy array: 3xn numpy array of rotated accelerations and 4xn angular positions as quaternions
"""
from scipy import sin, cos, arctan2
# get angle of rotation
angle_gnss = np.arctan2(gnss_position[1, motion_time],
gnss_position[0, motion_time])
# TODO pay attention using acceleration
angle_vector = arctan2(vectors[1, motion_time], vectors[0, motion_time])
# rotation_angle = angle_gnss - angle_vector
rotation_angle = angle_gnss
message = "Rotation vector to {} degrees to align to world".format(
np.rad2deg(rotation_angle))
print(message)
new_vectors = vectors.copy()
# rotate vector in xy plane
new_vectors[0] = cos(rotation_angle) * vectors[0] - sin(
rotation_angle) * vectors[1]
new_vectors[1] = sin(rotation_angle) * vectors[0] + cos(
rotation_angle) * vectors[1]
return new_vectors
def thinningEdges(self):
self.gradDir = scipy.arctan2(self.gradDY, self.gradDX)
self.gradCopy = self.grad.copy()
for currRow in range(1, self.width - 1):
for currCol in range(1, self.height - 1):
up = currCol - 1
down = currCol + 1
left = currRow - 1
right = currRow + 1
error = 22.5
if self.gradDir[currRow][currCol] >= 0 and self.gradDir[currRow][currCol] < 0 + error or \
self.gradDir[currRow][currCol] >= 360 and self.gradDir[currRow][currCol] < 360 - error or \
self.gradDir[currRow][currCol] >= 180 - error and self.gradDir[currRow][currCol] < 180 + error :
if self.gradCopy[currRow][currCol] <= self.gradCopy[currRow][down] or \
(self.gradCopy[currRow][currCol] <= self.gradCopy[currRow][up]):
self.grad[currRow][currCol] = 0
elif self.gradDir[currRow][currCol] >= 45 - error and self.gradDir[currRow][currCol] < 45 + error or \
self.gradDir[currRow][currCol] >= 135 - error and self.gradDir[currRow][currCol] < 135 + error:
if (self.gradCopy[currRow][currCol] <= self.gradCopy[left][down]) or \
(self.gradCopy[currRow][currCol] <= self.gradCopy[right][up]):
self.grad[currRow][currCol] = 0
elif self.gradDir[currRow][currCol] >= 90 - error and self.gradDir[currRow][currCol] < 90 + error or \
self.gradDir[currRow][currCol] >= 270 - error and self.gradDir[currRow][currCol] < 270 + error:
if (self.gradCopy[currRow][currCol] <= self.gradCopy[right][currCol]) or \
(self.gradCopy[currRow][currCol] <= self.gradCopy[left][currCol]):
self.grad[currRow][currCol] = 0
else: ## for angles in second and forth quadrant
if (self.gradCopy[currRow][currCol] <= self.gradCopy[right][down]) or \
(self.gradCopy[currRow][currCol] <= self.gradCopy[left][up]):
self.grad[currRow][currCol] = 0
def rotation_matrix_from_cross_prod(a,b):
"""
Returns the rotation matrix which rotates the
vector :samp:`a` onto the the vector :samp:`b`.
:type a: 3 sequence of :obj:`float`
:param a: Vector to be rotated on to :samp:`{b}`.
:type b: 3 sequence of :obj:`float`
:param b: Vector.
:rtype: :obj:`numpy.array`
:return: 3D rotation matrix.
"""
crs = np.cross(a,b)
dotProd = np.dot(a,b)
crsNorm = sp.linalg.norm(crs)
eps = sp.sqrt(sp.finfo(a.dtype).eps)
r = sp.eye(a.size, a.size, dtype=a.dtype)
if (crsNorm > eps):
theta = sp.arctan2(crsNorm, dotProd)
r = axis_angle_to_rotation_matrix(crs, theta)
elif (dotProd < 0):
r = -r
return r
def fast_kappa(z1, r1, w1, d1, z2, r2, w2, d2, ang, same_half_plate, th1,
phi1, th2, phi2):
rp = abs(r1 - r2[:, None]) * sp.cos(ang / 2)
rt = (r1 + r2[:, None]) * sp.sin(ang / 2)
d12 = d1 * d2[:, None]
w12 = w1 * w2[:, None]
w = (rp>=kappa.rp_min) & (rp<=kappa.rp_max) & \
(rt<=kappa.rt_max) & (rt>=kappa.rt_min)
rp = rp[w]
rt = rt[w]
w12 = w12[w]
d12 = d12[w]
x = (phi2 - phi1) * sp.sin(th1)
y = th2 - th1
gamma_ang = sp.arctan2(y, x)
#-- getting model and first derivative
xi_model = kappa.xi2d(rt, rp, grid=False)
xip_model = kappa.xi2d(rt, rp, dx=1, grid=False)
#-- this is the weight of the estimator
R = 1 / (xip_model * rt)
ska = sp.sum((d12 - xi_model) / R * w12)
gam1 = 4 * sp.cos(2 * gamma_ang) * sp.sum(
(d12 - xi_model) / R * w12) # SY
gam2 = 4 * sp.sin(2 * gamma_ang) * sp.sum(
(d12 - xi_model) / R * w12) # SY
wka = sp.sum(w12 / R**2)
nka = 1. * d12.size
return ska, wka, gam1, gam2, nka
def ecef2geodetic(x, y, z):
"""
Convert ecef 2 geodetic
Convert ECEF coordinates to geodetic.
J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
to geodetic coordinates," IEEE Transactions on Aerospace and
Electronic Systems, vol. 30, pp. 957-961, 1994.
"""
a = 6378137
b = 6356752.3142
esq = 0.00669437999013
e1sq = 6.73949674228 * 0.001
r = sqrt(x * x + y * y)
Esq = a * a - b * b
F = 54 * b * b * z * z
G = r * r + (1 - esq) * z * z - esq * Esq
C = (esq * esq * F * r * r) / (pow(G, 3))
S = sqrt(1 + C + sqrt(C * C + 2 * C))
P = F / (3 * pow((S + 1 / S + 1), 2) * G * G)
Q = sqrt(1 + 2 * esq * esq * P)
r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a * (1 + 1.0 / Q) - P *
(1 - esq) * z * z /
(Q * (1 + Q)) - 0.5 * P * r * r)
U = sqrt(pow((r - esq * r_0), 2) + z * z)
V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z)
Z_0 = b * b * z / (a * V)
h = U * (1 - b * b / (a * V))
lat = arctan((z + e1sq * Z_0) / r)
lon = arctan2(y, x)
return degrees(lat), degrees(lon), h
def xyz2longlat(x,y,z):
""" converts cartesian x,y,z coordinates into spherical longitude and latitude """
r = sc.sqrt(x*x + y*y + z*z)
long = sc.arctan2(y,x)
d = sc.sqrt(x*x + y*y)
lat = sc.arcsin(z/r)
return long*deg, lat*deg, r
def cartesian_to_sky(position, wrap=True, degree=True):
"""Transform cartesian coordinates into distance, RA, Dec.
Parameters
----------
position : array of shape (N,3)
position in cartesian coordinates.
wrap : bool, optional
whether to wrap ra into [0,2*pi]
degree : bool, optional
whether RA, Dec are in degree (True) or radian (False).
Returns
-------
dist : array
distance.
ra : array
RA.
dec : array
Dec.
"""
dist = distance(position)
ra = scipy.arctan2(position[:, 1], position[:, 0])
if wrap: ra %= 2. * constants.pi
dec = scipy.arcsin(position[:, 2] / dist)
if degree: return dist, ra / constants.degree, dec / constants.degree
return dist, ra, dec
def two_d_ellipse(self,proj_vars,p,**kwargs):
"""Return the 2d projection as a matplotlib Ellipse object for the given p values
Parameters
----------
proj_vars : array that is 1 for the projection dimension, and 0 other wise
i.e. array([0,0,1,0,1]) will project 5d ellipsoid onto the plane
span by the 3rd and 5th variable.
p : the percent of points contained in the ellipsoid, either a single
value of a list of values i.e. 0.68 or [0.68,0.955],
if p is a list then a list of Ellipse objects will be returned, one for each p value
Keywords
--------
kwargs : keywords to be passed into the matplotlib Ellipse object
Return
------
ells : matplotlib Ellipse object
"""
mu,u,s=self.proj(proj_vars) #get the mean, eigenvectors, and eigenvales for projected array
try: #if a list get the length
l=len(p)
except: #if not then make it a list of length 1
l=1
p=[p]
invp=st.chi.ppf(p,self.dim) #scale it using a chi distribution (see, now we scale it)
angle=rad2deg(arctan2(u[0,1],u[0,0])) #angle the first eignevector makes with the x-axis
ells=[] #list to hold the Ellipse objects
for i in invp:
ells.append(Ellipse(xy=mu,width=s[0]*i*2,height=s[1]*i*2,angle=angle,**kwargs))#make the Ellipse objects, the *2 is needed since Ellipse takes the full axis vector
if l==1: #if only one p values was given return the Ellipse object (not as a list)
return ells[0]
else: #else return the list of Ellipse objects
return ells
def rotation_matrix_from_cross_prod(a, b):
"""
Returns the rotation matrix which rotates the
vector :samp:`a` onto the the vector :samp:`b`.
:type a: 3 sequence of :obj:`float`
:param a: Vector to be rotated on to :samp:`{b}`.
:type b: 3 sequence of :obj:`float`
:param b: Vector.
:rtype: :obj:`numpy.array`
:return: 3D rotation matrix.
"""
crs = np.cross(a, b)
dotProd = np.dot(a, b)
crsNorm = sp.linalg.norm(crs)
eps = sp.sqrt(sp.finfo(a.dtype).eps)
r = sp.eye(a.size, a.size, dtype=a.dtype)
if (crsNorm > eps):
theta = sp.arctan2(crsNorm, dotProd)
r = axis_angle_to_rotation_matrix(crs, theta)
elif (dotProd < 0):
r = -r
return r
def __init__(self, output='out', input='in', \
mag=None, phase=None, coh=None, \
freqlim=[], maglim=[], phaselim=[], \
averaged='not specified', \
seedfreq=-1, seedphase=0,
labels=[], legloc=-1, compin=[]):
self.output = output
self.input = input
if len(compin) > 0:
if mag is None:
self.mag = squeeze(colwise(abs(compin)))
if phase is None:
self.phase = squeeze(colwise(arctan2(imag(compin),real(compin))*180.0/pi))
else:
self.mag = squeeze(mag)
self.phase = squeeze(phase)
self.coh = coh
self.averaged = averaged
self.seedfreq = seedfreq
self.seedphase = seedphase
self.freqlim = freqlim
self.maglim = maglim
self.phaselim = phaselim
self.labels = labels
self.legloc = legloc
def compute_ellipse_params(Sigma, ci=0.95):
"""Compute the parameters of the confidence ellipse for the bivariate
normal distribution with the given covariance matrix.
Parameters
----------
Sigma : 2d array, (2, 2)
Covariance matrix of the bivariate normal.
ci : float or 1d array, optional
Confidence interval(s) to compute. Default is 0.95.
Returns
-------
a : float or 1d array
Major axes for each element in `ci`.
b : float or 1d array
Minor axes for each element in `ci`.
ang : float
Angle of ellipse, in radians.
"""
ci = scipy.atleast_1d(ci)
lam, v = scipy.linalg.eigh(Sigma)
chi2 = [-scipy.log(1.0 - cival) * 2.0 for cival in ci]
a = [2.0 * scipy.sqrt(chi2val * lam[-1]) for chi2val in chi2]
b = [2.0 * scipy.sqrt(chi2val * lam[-2]) for chi2val in chi2]
ang = scipy.arctan2(v[1, -1], v[0, -1])
return a, b, ang
def thinningEdges(self):
self.gradDir = scipy.arctan2(self.gradDY, self.gradDX)
self.gradCopy = self.grad.copy();
for currRow in range(1, self.width-1):
for currCol in range(1, self.height-1):
up = currCol - 1;
down = currCol + 1;
left = currRow - 1;
right = currRow + 1;
error = 22.5
if self.gradDir[currRow][currCol] >= 0 and self.gradDir[currRow][currCol] < 0 + error or \
self.gradDir[currRow][currCol] >= 360 and self.gradDir[currRow][currCol] < 360 - error or \
self.gradDir[currRow][currCol] >= 180 - error and self.gradDir[currRow][currCol] < 180 + error :
if self.gradCopy[currRow][currCol] <= self.gradCopy[currRow][down] or \
(self.gradCopy[currRow][currCol] <= self.gradCopy[currRow][up]):
self.grad[currRow][currCol] = 0
elif self.gradDir[currRow][currCol] >= 45 - error and self.gradDir[currRow][currCol] < 45 + error or \
self.gradDir[currRow][currCol] >= 135 - error and self.gradDir[currRow][currCol] < 135 + error:
if (self.gradCopy[currRow][currCol] <= self.gradCopy[left][down]) or \
(self.gradCopy[currRow][currCol] <= self.gradCopy[right][up]):
self.grad[currRow][currCol] = 0
elif self.gradDir[currRow][currCol] >= 90 - error and self.gradDir[currRow][currCol] < 90 + error or \
self.gradDir[currRow][currCol] >= 270 - error and self.gradDir[currRow][currCol] < 270 + error:
if (self.gradCopy[currRow][currCol] <= self.gradCopy[right][currCol]) or \
(self.gradCopy[currRow][currCol] <= self.gradCopy[left][currCol]):
self.grad[currRow][currCol] = 0
else: ## for angles in second and forth quadrant
if (self.gradCopy[currRow][currCol] <= self.gradCopy[right][down]) or \
(self.gradCopy[currRow][currCol] <= self.gradCopy[left][up]):
self.grad[currRow][currCol] = 0
def extract_hog(img,ROI):
#print("first")
#cellRows = cellCols = 5
#binCount = 4
# BlockRowCells = 3
# BlockColCells = 3
orientations=8
pixels_per_cell=(5, 5)#5,5 - 0.9844
cells_per_block=(3, 3)#3,3
img = resize(img,(50,50))
image = rgb2gray(img)
image = np.atleast_2d(image)
#hist = hog(img,binCount,(cellCols,cellRows),(BlockRowCells,BlockColCells))
#hist = np.divide(hog,np.linalg.norm(hog))
gx = roll(image, 1, axis = 1) - roll(image, -1, axis = 1)
gx[:,0],gx[:,-1] = 0,0;
gy = roll(image, 1, axis = 0) - roll(image, -1, axis = 0)
gy[-1,:],gy[0,:] = 0,0;
matr = np.square(gx) + np.square(gy)
matr = np.sqrt(matr)
orientation = arctan2(gy, (gx + 1e-15)) * (180 / pi) + 90
imx, imy = image.shape
cx, cy = pixels_per_cell
bx, by = cells_per_block
n_cellsx = int(np.floor(imx // cx)) # number of cells in i
n_cellsy = int(np.floor(imy // cy)) # number of cells in j
or_hist = np.zeros((n_cellsx, n_cellsy, orientations))
for i in range(orientations):
condition = orientation < 180 / orientations * (i + 1)
tmp = np.where(condition,orientation, 0)
condition = orientation >= 180 / orientations * i
tmp = np.where(condition,tmp, 0)
cond2 = tmp > 0
temp_mag = np.where(cond2, matr, 0)
or_hist[:,:,i] = uniform_filter(temp_mag, size=(cx, cy))[cx/2::cx, cy/2::cy].T
numbx = (n_cellsx - bx) + 1
numby = (n_cellsy - by) + 1
normb = np.zeros((numbx, numby, bx, by, orientations))
for i in range(numbx):
for j in range(numby):
block = or_hist[i:i + bx, j:j + by, :]
eps = 1e-5
normb[i, j, :] = block / sqrt(block.sum() ** 2 + eps)
return normb.ravel()
def getG(X,Y):
"""
This function calculates the angular components of a state
:param X: X Points
:param Y: Y Points
:return: Arctan(Y/X)
"""
return sp.cos(sp.arctan2(Y, X))
def get_verts(self):
if self.options.mode == "density_graph":
scale = self.options.resolution
offset = self.options.centre
else:
scale = 1
offset = [0, 0]
line = (self.line - offset) / scale
dy = line[1, 0] - line[0, 0]
dx = line[1, 1] - line[0, 1]
wx = self.width / 2 / scale * sp.sin(sp.arctan2(dy, dx))
wy = -self.width / 2 / scale * sp.cos(sp.arctan2(dy, dx))
verts = sp.array([[line[0, 1] - wx, line[0, 0] - wy],
[line[0, 1] + wx, line[0, 0] + wy],
[line[1, 1] + wx, line[1, 0] + wy],
[line[1, 1] - wx, line[1, 0] - wy]])
return verts
def r1(self):
"""returns cylindrical coordinate along second dimension
Returns:
numpy array of cylindrical coordinates in radians
"""
return scipy.arctan2(self.unit[1], self.unit[0])
def r1(self):
"""returns cylindrical coordinate along second dimension
Returns:
numpy array of cylindrical coordinates in radians
"""
return scipy.arctan2(self.unit[1],self.unit[0])
def decode_ee(e1,e2,scale=0.03):
#from: e = (a-b)/(a+b)
#we have: a/b = (1+e)/(1-e)
#below solution for: a+b=scale*2 (=const)
e = sqrt(e1**2+e2**2)
a = (1+e)*scale
b = (1-e)*scale
ang = 0.5*(arctan2(e2,e1))
return e,a,b,ang
def left2right(self, j1_degree, j2_degree):
x = self.length1 * math.cos(
j1_degree * math.pi / 180) + self.length2 * math.cos(
(j1_degree + j2_degree) * math.pi / 180)
y = self.length1 * math.sin(
j1_degree * math.pi / 180) + self.length2 * math.sin(
(j1_degree + j2_degree) * math.pi / 180)
theta_yx = scipy.arctan2(y, x) * 180 / scipy.pi
return (2 * theta_yx - j1_degree), -j2_degree
def decodeMessageSensorUDP(self, msg):
""" This is used to decode message from sensorUDP application from the android market.
The orientation field was first used, but its conventions were unclear.
So now acceleration and magnetic vectors should be used"""
data = msg.split(', ')
if data[0]=='G':
# This is GPS message
time = decimalstr2float(data[2])
latitude_deg = decimalstr2float(data[3])
longitude_deg = decimalstr2float(data[4])
altitude = decimalstr2float(data[5])
hdop = decimalstr2float(data[7]) # Horizontal dilution of precision
vdop = decimalstr2float(data[8]) # Vertical dilution of precision
print time, latitude_deg, longitude_deg, altitude, hdop, vdop
if data[0]=='O':
# \note This is no more used as orientation convention were unclear
# 'O, 146, 1366575961732, 230,1182404, -075,2031250, 001,7968750'
[ u, u, # data not used \
heading_deg, # pointing direction of top of phone \
roll_deg, # around horizontal axis, positive clockwise [-180:180] \
pitch_deg] = decimalstr2float(data[1:]) # around vertical axis [_90:90]
elevation_deg = -sp.rad2deg(sp.arctan2( \
sp.cos(sp.deg2rad(pitch_deg))*sp.cos(sp.deg2rad(roll_deg)), \
sp.sqrt(1+sp.cos(sp.deg2rad(roll_deg))**2*(sp.sin(sp.deg2rad(pitch_deg))**2-1)))) #positive up
inclinaison_deg = pitch_deg #positive clockwise
print heading_deg, roll_deg, pitch_deg, elevation_deg, inclinaison_deg
if data[0] == 'A':
# Accelerometer data
# Index and sign are adjusted to obtain x through the screen, and z down
deltaT = decimalstr2float(data[2])/1000 - self.time_acceleration
if self.filterTimeConstant == 0.0:
alpha = 1
else:
alpha = 1-sp.exp(-deltaT/self.filterTimeConstant)
self.time_acceleration = decimalstr2float(data[2])/1000
self.acceleration_raw[0] = decimalstr2float(data[3])
self.acceleration_raw[1] = decimalstr2float(data[4])
self.acceleration_raw[2] = decimalstr2float(data[5])
# Filter the data
self.acceleration_filtered +=alpha*(sp.array(self.acceleration_raw)-self.acceleration_filtered)
if data[0] == 'M':
# Magnetometer data
# Index and sign are adjusted to obtain x through the screen, and z down
deltaT = decimalstr2float(data[2])/1000-self.time_magnetic
if self.filterTimeConstant == 0.0:
alpha = 1
else:
alpha = 1-sp.exp(-deltaT/self.filterTimeConstant)
self.time_magnetic = decimalstr2float(data[2])/1000
self.magnetic_raw[0] = decimalstr2float(data[3])
self.magnetic_raw[1] = decimalstr2float(data[4])
self.magnetic_raw[2] = -decimalstr2float(data[5])# Adapt to a bug in sensorUDP?
# Filter the data
self.magnetic_filtered += alpha*(sp.array(self.magnetic_raw)-self.magnetic_filtered)
def cart2sphere(coordlist):
X_vec = coordlist[:,0]
Y_vec = coordlist[:,1]
Z_vec = coordlist[:,2]
R_vec = sp.sqrt(X_vec**2+Y_vec**2+Z_vec**2)
Az_vec = np.degrees(sp.arctan2(X_vec,Y_vec))
El_vec = np.degrees(sp.arcsin(Z_vec/R_vec))
sp_coords = sp.array([R_vec,Az_vec,El_vec]).transpose()
return sp_coords
def b(x, y, params):
'''
Returns b as defined by Eq. 4
'''
a0 = params['a0']
x0 = params['x0']
y0 = params['y0']
E = params['E']
return a0 - E + scipy.arctan2((y - y0),(x - x0))
def cart2sphere(coordlist):
r2d = 180.0/sp.pi
d2r = sp.pi/180.0
X_vec = coordlist[:,0]
Y_vec = coordlist[:,1]
Z_vec = coordlist[:,2]
R_vec = sp.sqrt(X_vec**2+Y_vec**2+Z_vec**2)
Az_vec = sp.arctan2(X_vec,Y_vec)*r2d
El_vec = sp.arcsin(Z_vec/R_vec)*r2d
sp_coords = sp.array([R_vec,Az_vec,El_vec]).transpose()
return sp_coords
def compute(self):
if self.has_input("InputMatrix"):
m = self.get_input("InputMatrix")
r = m.Reals().matrix
im = m.Imaginaries().matrix
else:
r = self.get_input("RealMatrix").matrix
im = self.get_input("ImaginaryMatrix").matrix
out = SparseMatrix()
out.matrix = sparse.csc_matrix(scipy.arctan2(im.toarray(),r.toarray()))
self.set_output("Output", out)
def windvec(u= scipy.array([]),\
D=scipy.array([])):
'''
Function to calculate the wind vector from time series of wind
speed and direction.
Parameters:
- u: array of wind speeds [m s-1].
- D: array of wind directions [degrees from North].
Returns:
- uv: Vector wind speed [m s-1].
- Dv: Vector wind direction [degrees from North].
Examples
--------
>>> u = scipy.array([[ 3.],[7.5],[2.1]])
>>> D = scipy.array([[340],[356],[2]])
>>> windvec(u,D)
(4.162354202836905, array([ 353.2118882]))
>>> uv, Dv = windvec(u,D)
>>> uv
4.162354202836905
>>> Dv
array([ 353.2118882])
'''
# Test input array/value
u,D = _arraytest(u,D)
ve = 0.0 # define east component of wind speed
vn = 0.0 # define north component of wind speed
D = D * math.pi / 180.0 # convert wind direction degrees to radians
for i in range(0, len(u)):
ve = ve + u[i] * math.sin(D[i]) # calculate sum east speed components
vn = vn + u[i] * math.cos(D[i]) # calculate sum north speed components
ve = - ve / len(u) # determine average east speed component
vn = - vn / len(u) # determine average north speed component
uv = math.sqrt(ve * ve + vn * vn) # calculate wind speed vector magnitude
# Calculate wind speed vector direction
vdir = scipy.arctan2(ve, vn)
vdir = vdir * 180.0 / math.pi # Convert radians to degrees
if vdir < 180:
Dv = vdir + 180.0
else:
if vdir > 180.0:
Dv = vdir - 180
else:
Dv = vdir
return uv, Dv # uv in m/s, Dv in dgerees from North
def stopRecordingRot(self):
self.wm.accelerometer.unregister_callback(self.recordAccel)
for value in self.currentGestureData:
x, y, z = value[0], value[1], value[2]
rot_angle = int(-(sp.degrees(sp.arctan2(z-512, x-512)) - 90))
self.rotList.append(rot_angle)
print(self.rotList)
self.currentGestureData = []
direction = self.getDirection()
if direction == 1:
self.callback(2)
else:
self.callback(-2)
def EqTolb (ra_deg, dec_deg):
""" Converts equatorial ra, dec, into galactic l,b; from Binney and Merrifield, p. 31
following the method of http://star-www.st-and.ac.uk/~spd3/Teaching/AS3013/programs/radec2lb.f90
NOT QUITE - I use arctan2 method instead"""
ra, dec = (ra_deg*rad), (dec_deg*rad)
# Conversion Code
r = (ra - raGP)
b = sc.arcsin( sc.sin(decGP)*sc.sin(dec) + sc.cos(decGP)*sc.cos(dec)*sc.cos(r) )
t = sc.arctan2((sc.cos(dec)*sc.sin(r)),
(sc.cos(decGP)*sc.sin(dec) - sc.sin(decGP)*sc.cos(dec)*sc.cos(r)) )
l = (lCP - t)
b, l = angle_bounds2((b*deg), (l*deg))
return l, b
def lbToEq (l_deg, b_deg):
""" Converts galactic l,b in to Equatorial ra, dec; from Binney and Merrifield, p. 31;
l, b must be arrays of same shape"""
l, b = (l_deg*rad), (b_deg*rad)
# Conversion Code
t = lCP - l
dec = sc.arcsin(sc.sin(decGP)*sc.sin(b) + sc.cos(decGP)*sc.cos(b)*sc.cos(t) )
r = sc.arctan2( (sc.cos(b)*sc.sin(t)),
( (sc.cos(decGP)*sc.sin(b)) - (sc.sin(decGP)*sc.cos(b)*sc.cos(t))) )
if type(r) != type(arr): r = sc.array([r])
for i in range(len(r)):
r[i] = angle_bounds((r[i] + raGP)*deg)
return r, (dec*deg)
