def state_integrate(self):
   rate = rospy.Rate(100)
   while not rospy.is_shutdown():
     rate.sleep()
     #For integration model
     Q_k = np.eye(9)*.2
     w = np.random.multivariate_normal(np.zeros([1,9])[0], Q_k)
     #For sensor measurement
     R_k = np.eye(3)*.01
     v = np.random.multivariate_normal(np.zeros([1,3])[0], R_k)
     #Predict
     s_k =  np.transpose(np.dot(self.A, self.s))
     P_k = np.add(np.dot(np.dot(self.A, self.P), np.transpose(self.A)), Q_k)
     #Update
     if self.update_unfilt:
       y_k = np.subtract(np.transpose(np.add(self.z, v)), np.dot(self.H, s_k))
       S_K = np.dot(np.dot(self.H, P_k), np.transpose(self.H))
       K = np.dot(P_k, np.dot(np.transpose(self.H), np.pinv(S_K)))
       self.s = np.add(s_k, np.dot(K, y_k))
       self.P = np.dot(np.subtract(np.eye(9), np.dot(K, self.H)), P_k)
       #This could result in a terrible race condition. But will need to test
       self.update_filt = False
     else:
       self.s = s_k
       self.P = P_k

def convert_yuv420_to_rgb_image(y_plane, u_plane, v_plane,
                                w, h,
                                ccm_yuv_to_rgb=DEFAULT_YUV_TO_RGB_CCM,
                                yuv_off=DEFAULT_YUV_OFFSETS):
    """Convert a YUV420 8-bit planar image to an RGB image.

    Args:
        y_plane: The packed 8-bit Y plane.
        u_plane: The packed 8-bit U plane.
        v_plane: The packed 8-bit V plane.
        w: The width of the image.
        h: The height of the image.
        ccm_yuv_to_rgb: (Optional) the 3x3 CCM to convert from YUV to RGB.
        yuv_off: (Optional) offsets to subtract from each of Y,U,V values.

    Returns:
        RGB float-3 image array, with pixel values in [0.0, 1.0].
    """
    y = numpy.subtract(y_plane, yuv_off[0])
    u = numpy.subtract(u_plane, yuv_off[1]).view(numpy.int8)
    v = numpy.subtract(v_plane, yuv_off[2]).view(numpy.int8)
    u = u.reshape(h/2, w/2).repeat(2, axis=1).repeat(2, axis=0)
    v = v.reshape(h/2, w/2).repeat(2, axis=1).repeat(2, axis=0)
    yuv = numpy.dstack([y, u.reshape(w*h), v.reshape(w*h)])
    flt = numpy.empty([h, w, 3], dtype=numpy.float32)
    flt.reshape(w*h*3)[:] = yuv.reshape(h*w*3)[:]
    flt = numpy.dot(flt.reshape(w*h,3), ccm_yuv_to_rgb.T).clip(0, 255)
    rgb = numpy.empty([h, w, 3], dtype=numpy.uint8)
    rgb.reshape(w*h*3)[:] = flt.reshape(w*h*3)[:]
    return rgb.astype(numpy.float32) / 255.0

    def test_ufunc_coercions(self):
        idx = date_range('2011-01-01', periods=3, freq='2D', name='x')

        delta = np.timedelta64(1, 'D')
        for result in [idx + delta, np.add(idx, delta)]:
            assert isinstance(result, DatetimeIndex)
            exp = date_range('2011-01-02', periods=3, freq='2D', name='x')
            tm.assert_index_equal(result, exp)
            assert result.freq == '2D'

        for result in [idx - delta, np.subtract(idx, delta)]:
            assert isinstance(result, DatetimeIndex)
            exp = date_range('2010-12-31', periods=3, freq='2D', name='x')
            tm.assert_index_equal(result, exp)
            assert result.freq == '2D'

        delta = np.array([np.timedelta64(1, 'D'), np.timedelta64(2, 'D'),
                          np.timedelta64(3, 'D')])
        for result in [idx + delta, np.add(idx, delta)]:
            assert isinstance(result, DatetimeIndex)
            exp = DatetimeIndex(['2011-01-02', '2011-01-05', '2011-01-08'],
                                freq='3D', name='x')
            tm.assert_index_equal(result, exp)
            assert result.freq == '3D'

        for result in [idx - delta, np.subtract(idx, delta)]:
            assert isinstance(result, DatetimeIndex)
            exp = DatetimeIndex(['2010-12-31', '2011-01-01', '2011-01-02'],
                                freq='D', name='x')
            tm.assert_index_equal(result, exp)
            assert result.freq == 'D'

def normalize_layout(l):
    """Make sure all the spots in a layout are where you can click.

    Returns a copy of the layout with all spot coordinates are
    normalized to within (0.0, 0.98).

    """
    xs = []
    ys = []
    ks = []
    for (k, (x, y)) in l.items():
        xs.append(x)
        ys.append(y)
        ks.append(k)
    minx = np.min(xs)
    maxx = np.max(xs)
    try:
        xco = 0.98 / (maxx - minx)
        xnorm = np.multiply(np.subtract(xs, [minx] * len(xs)), xco)
    except ZeroDivisionError:
        xnorm = np.array([0.5] * len(xs))
    miny = np.min(ys)
    maxy = np.max(ys)
    try:
        yco = 0.98 / (maxy - miny)
        ynorm = np.multiply(np.subtract(ys, [miny] * len(ys)), yco)
    except ZeroDivisionError:
        ynorm = np.array([0.5] * len(ys))
    return dict(zip(ks, zip(map(float, xnorm), map(float, ynorm))))

def resolve_collision(m):
    # Calculate relative velocity
    rv = numpy.subtract(m.b.velocity, m.a.velocity)

    # Calculate relative velocity in terms of the normal direction
    velocity_along_normal = numpy.dot(rv, m.normal)

    # Do not resolve if velocities are separating
    if velocity_along_normal > 0:
        # print("Separating:", velocity_along_normal)
        # print("  Normal:  ", m.normal)
        # print("  Vel:     ", m.b.velocity, m.a.velocity)
        return False

    # Calculate restitution
    e = min(m.a.restitution, m.a.restitution)

    # Calculate impulse scalar
    j = -(1 + e) * velocity_along_normal
    j /= 1 / m.a.mass + 1 / m.b.mass

    # Apply impulse
    impulse = numpy.multiply(j, m.normal)

    # print("Before: ", m.a.velocity, m.b.velocity)
    m.a.velocity = numpy.subtract(m.a.velocity,
                                  numpy.multiply(1 / m.a.mass, impulse))
    m.b.velocity = numpy.add(m.b.velocity,
                             numpy.multiply(1 / m.b.mass, impulse))
    # print("After:  ", m.a.velocity, m.b.velocity)
    # print("  Normal:  ", m.normal)

    return True

    def test_parr_ops_errors(self, ng, box_with_array):
        idx = PeriodIndex(['2011-01', '2011-02', '2011-03', '2011-04'],
                          freq='M', name='idx')
        obj = tm.box_expected(idx, box_with_array)

        msg = r"unsupported operand type\(s\)"
        with pytest.raises(TypeError, match=msg):
            obj + ng

        with pytest.raises(TypeError):
            # error message differs between PY2 and 3
            ng + obj

        with pytest.raises(TypeError, match=msg):
            obj - ng

        with pytest.raises(TypeError):
            np.add(obj, ng)

        with pytest.raises(TypeError):
            np.add(ng, obj)

        with pytest.raises(TypeError):
            np.subtract(obj, ng)

        with pytest.raises(TypeError):
            np.subtract(ng, obj)

    def __init__(self, indices, vertices, normals, texcoords, material):
        """A triangle should not be created manually."""

        self.vertices = vertices
        """A (3, 3) float array for points in the triangle"""
        self.normals = normals
        """A (3, 3) float array with the normals for points in the triangle.
        If the triangle didn't have normals, they will be computed."""
        self.texcoords = texcoords
        """A tuple with (3, 2) float arrays with the texture coordinates
          for the points in the triangle"""
        self.material = material
        """If coming from an unbound :class:`collada.triangleset.TriangleSet`, contains a
          string with the material symbol. If coming from a bound
          :class:`collada.triangleset.BoundTriangleSet`, contains the actual
          :class:`collada.material.Effect` the triangle is bound to."""
        self.indices = indices
        """A (3, 3) int array with vertex indexes in the vertex array"""

        if self.normals is None:
            # generate normals
            vec1 = numpy.subtract(vertices[0], vertices[1])
            vec2 = numpy.subtract(vertices[2], vertices[0])
            vec3 = toUnitVec(numpy.cross(toUnitVec(vec2), toUnitVec(vec1)))
            self.normals = numpy.array([vec3, vec3, vec3])

def do_intersect(p1, p2, q1, q2):
	# s1 = p1 + tr, r = p2 - p1
	# s2 = q1 + us, s = q2 - q1
	r = np.subtract(p2, p1)
	s = np.subtract(q2, q1)
	rxs = np.cross(r, s)
	# if r x s = 0, s1 and s2 are parallel or colinear.
	if rxs == 0:
		# if parallel, (p - q) x r != 0
		if np.cross(np.subtract(p1, q1), r) != 0:
			return False
		else:
			# project onto x- and y-axis and check for overlap.
			return ((q1[0] >= p1[0] and q1[0] <= p2[0] or
				q2[0] >= p1[0] and q2[0] <= p2[0] or
				p1[0] >= q1[0] and p1[0] <= q2[0] or
				p2[0] >= q1[0] and p2[0] <= q2[0]) and
				(q1[1] >= p1[1] and q1[1] <= p2[1] or
					q2[1] >= p1[1] and q2[1] <= p2[1] or
					p1[1] >= q1[1] and p1[1] <= q2[1] or
					p2[1] >= q1[1] and p2[1] <= q2[1]));
	# s1 and s2 intersect where s1 = s2 where 0 <= t <= 1 and 0 <= u <= 1
	#   (p + tr) x s = (q + us) x s
	# p x s + tr x s = q x s + us x s
	#         tr x s = q x s - p x s
	#              t = (q - p) x s / r x s
	t = np.cross(np.subtract(q1, p1), s) / rxs
	u = np.cross(np.subtract(q1, p1), r) / rxs
	if 0 <= t and 1 >= t and 0 <= u and 1 >= u:
		return True
	else:
		return False

def getGammaAngle(appf,cAtom,oAtom,hAtom):
    # first determine the nAtom
    aminoGroup = appf.select('resnum ' + str(cAtom.getResnum()))
    for at in aminoGroup:
        if(at.getName() == 'N'):
            nAtom = at
        # get coordinates
    cCoords = cAtom.getCoords()
    oCoords = oAtom.getCoords()
    hCoords = hAtom.getCoords()
    nCoords = nAtom.getCoords()
    # get necessary vectors
    oc = np.subtract(oCoords,cCoords)
    nc = np.subtract(nCoords,cCoords)
    ho = np.subtract(hCoords,oCoords)
    n1 = np.cross(oc,nc)
    n1_unit = np.divide(n1,np.linalg.norm(n1))
    # get projection of H-O in O-C direction
    oc_unit = np.divide(oc,np.linalg.norm(oc))
    #print oc_unit
    hproj = np.dot(ho,oc_unit)
    # get projection of H-O onto N-C-O plane
    out = np.dot(ho,n1_unit)
    n2 = np.cross(np.multiply(n1_unit,out),oc)
    #print n2
    ho_ip = np.subtract(ho,np.multiply(n1_unit,out))
    test = np.dot(n2,ho_ip)
    #print test
    ang = hproj/np.linalg.norm(ho_ip)
    ang = math.acos(ang)
    ang = ang*180/math.pi
    #if(test < 0):
    #    ang = ang * -1
    return ang

def clearShot(p1, p2, worldLines, worldPoints, agent):
	### YOUR CODE GOES BELOW HERE ###
	
	def minDistance(point):
		best = INFINITY
		for line in worldLines:
			current = minimumDistance(line, point)
			if current < best:
				best = current
		return best
	
	# Insurance check to avoid divide by zero error
	if distance(p1, p2) < EPSILON:
		return True
	# Fetch agent's max radius
	radius = agent.getMaxRadius()
	# Find the deltas in x and y, and scale them based on length of agent's max radius
	(dx, dy) = numpy.multiply(numpy.subtract(p2, p1), radius / distance(p1, p2))
	# Swap x and y and flip sign of one for perpendicular translation vector
	p = (dy, -dx)
	# Check edges of agent line of travel for collisions; add line if no collision
	if rayTraceWorld(numpy.add(p1, p), numpy.add(p2, p), worldLines) == None:
		if rayTraceWorld(numpy.subtract(p1, p), numpy.subtract(p2, p), worldLines) == None:
			if minDistance(p1) > radius and minDistance(p2) > radius:
				return True
	
	### YOUR CODE GOES ABOVE HERE ###
	return False

 def __call__(self, values, clip=True, out=None):
     values = _prepare(values, clip=clip, out=out)
     np.multiply(values, np.log(self.exp + 1.), out=values)
     np.exp(values, out=values)
     np.subtract(values, 1., out=values)
     np.true_divide(values, self.exp, out=values)
     return values

                def dispatch_request(self, subject):

                    assert subject['method'] == 'train' or subject['method'] == 'accuracy'
                    if subject['method'] == 'train':
                        params = subject['params']
                        net = loads_net(params)
                        o_weights = np.copy(net.weights)
                        o_biases = np.copy(net.biases)
                        #mini_batch_size = 10 #TODO maybe allow this to come from params 
                        net.SGD(training_data, 1, 10, 0.5, evaluation_data=test_data, monitor_evaluation_accuracy=True)
                        delt_weights = np.subtract(net.weights, o_weights)
                        delt_biases = np.subtract(net.biases, o_biases)
                        send_net = {}
                        send_net['delt_weights'] = delt_weights
                        send_net['delt_biases'] = delt_biases
                        print('sending updates')
                        # send the deltas back to the server
                        #res = self.parent.request("updates", wait_for_response=True)
                        #print('res: ' + repr(res))
                        self.parent.request('updates', params=send_net, wait_for_response = False)
                        return 'round done'
                    elif subject['method'] == 'accuracy':
                        params = subject['params']
                        net = loads_net(params)
                        return net.accuracy(test_data)

    def createMesh(self):

        info = "\n\nChannel mesh:\n"

        eID = 1
        for pID in range(len(self.proMesh)-1):
            pID += 1
            for nID in range(len(self.proMesh[pID])-1):
                a1 = self.proMesh[pID][nID]
                a2 = self.proMesh[pID][nID+1]
                b1 = self.proMesh[pID+1][nID]
                b2 = self.proMesh[pID+1][nID+1]

                d1 = np.linalg.norm(np.subtract(self.nodMesh[b1], self.nodMesh[a2]))
                d2 = np.linalg.norm(np.subtract(self.nodMesh[b2], self.nodMesh[a1]))

                if d1 < d2:
                    self.mesh[eID] = [a1, a2, b1]
                    eID += 1
                    self.mesh[eID] = [b1, a2, b2]
                    eID += 1
                else:
                    self.mesh[eID] = [b1, a1, b2]
                    eID += 1
                    self.mesh[eID] = [b2, a1, a2]
                    eID += 1

        info += " - Nodes:\t{0}\n".format(len(self.nodMesh))
        info += " - Elements:\t{0}\n".format(eID-1)

        return info

def parabola_fit(filter_cost_path, cost_path, start, end):
  '''
  Returns parabolic fit of filtered cost path, as well as residuals
  :parameters:
    - filter_cost_path : numpy.ndarray
      The cost path median filtered (truncated to best fit parabolic fit)
    - cost_path : numpy.ndarray
      Original cost path, vector of values of cells accessed in alignment path
    - start : int
      Index to start truncated form of cost_path at to best aligned with the filtered form
    - end : int
      Index to end truncated form of cost_path at to best aligned with the filtered form

  :returns:
    - parab : numpy.ndarray
      Parabola of best fit
    - residuals_filt : numpy.ndarray
      Residuals created by subtracting filtered cost path and parab
    - res_original : numpy.ndarray
      Residuals created by subtracting original cost path and parab
  '''
  x = np.arange(start = 0, stop = filter_cost_path.shape[0])
  p = np.polyfit(x =x, y =filter_cost_path, deg = 2)
  # build residuals because apparently numpy just gives the sum of them, and actual parabola because why not
  parab = p[2]+p[1]*x+p[0]*x**2

  # residuals = np.zeros(x.shape)
  residuals_filt = np.subtract(filter_cost_path, parab)
  res_original = np.subtract(cost_path[start:end], parab)
  # for i in xrange(residuals.shape[0]):
  #   residuals[i] = cost_path[i]-parab[i]
  return parab, residuals_filt, res_original

    def _Gram(self, X):
        if X is self.X:
            if self.Gs_train is None:
                kernel_scalar = rbf_kernel(self.X, gamma=self.gamma)[:, :,
                                                                     newaxis,
                                                                     newaxis]
                delta = subtract(X.T[:, newaxis, :], self.X.T[:, :, newaxis])
                self.Gs_train = asarray(transpose(
                    2 * self.gamma * kernel_scalar *
                    (2 * self.gamma * (delta[:, newaxis, :, :] *
                                       delta[newaxis, :, :, :]).transpose(
                        (3, 2, 0, 1)) +
                        ((self.p - 1) - 2 * self.gamma *
                         _norm_axis_0(delta)[:, :, newaxis, newaxis]**2) *
                        eye(self.p)[newaxis, newaxis, :, :]), (0, 2, 1, 3)
                )).reshape((self.p * X.shape[0], self.p * self.X.shape[0]))
            return self.Gs_train

        kernel_scalar = rbf_kernel(X, self.X, gamma=self.gamma)[:, :,
                                                                newaxis,
                                                                newaxis]
        delta = subtract(X.T[:, newaxis, :], self.X.T[:, :, newaxis])
        return asarray(transpose(
            2 * self.gamma * kernel_scalar *
            (2 * self.gamma * (delta[:, newaxis, :, :] *
                               delta[newaxis, :, :, :]).transpose(
                (3, 2, 0, 1)) +
                ((self.p - 1) - 2 * self.gamma *
                 _norm_axis_0(delta).T[:, :, newaxis, newaxis]**2) *
                eye(self.p)[newaxis, newaxis, :, :]), (0, 2, 1, 3)
        )).reshape((self.p * X.shape[0], self.p * self.X.shape[0]))

def compute_factors(signal_dict_by_tf_1, signal_dict_by_tf_2):
    keys = signal_dict_by_tf_1.keys()

    signal_1 = np.zeros(len(keys))
    signal_2 = np.zeros(len(keys))

    for idx, key in enumerate(keys):
        signal_1[idx] = sum(signal_dict_by_tf_1[key])
        signal_2[idx] = sum(signal_dict_by_tf_2[key])

    # Take log
    log_tc1 = np.log(signal_1)
    log_tc2 = np.log(signal_2)

    # Average
    average_log_tc = np.add(log_tc1, log_tc2) / 2

    # Filter
    filter_log_tc1 = log_tc1[~np.isnan(log_tc1)]
    filter_log_tc2 = log_tc2[~np.isnan(log_tc2)]
    filter_log_tc = average_log_tc[~np.isnan(average_log_tc)]

    # Subtract
    sub_tc1 = np.subtract(filter_log_tc1, filter_log_tc)
    sub_tc2 = np.subtract(filter_log_tc2, filter_log_tc)

    median_tc1 = np.median(sub_tc1)
    median_tc2 = np.median(sub_tc2)

    factor1 = np.exp(median_tc1)
    factor2 = np.exp(median_tc2)

    return factor1, factor2

def calc_spectral_kurtosis(data, win_size=2048):
    """
    Spectral skewness formula from:
    Thoman, Chris. Model-Based Classification Of Speech Audio. ProQuest, 2009.

    Parameters
    ----------
    data: audio array in mono

    win_size: analysis block size in samples

    Returns
    -------
    The spectral skewness for each window
    """
    # Get M[n]
    magnitudes = np.abs(compute_stft(data, win_size))

    # Get SCt and turn it into a matrix for easy calculation
    sc = calc_spectral_centroid(data, win_size)
    sc_matrix = np.tile(sc, (magnitudes.shape[0], 1))

    # Get F[n] and matricize it
    fk = generate_fft_bins(win_size)
    fk_matrix = np.transpose(np.tile(fk, (magnitudes.shape[1], 1)))

    # Get SPt
    ss = calc_spectral_spread(data, win_size)

    # Create the numerator and denominators
    numerator = np.sum(np.multiply(np.power(np.subtract(fk_matrix, sc_matrix), 4), magnitudes), axis=0)
    denominator = np.multiply(np.power(ss, 4), np.sum(magnitudes, axis=0))

    # Kurtosis incoming
    return np.subtract(np.divide(numerator, denominator), 3)

def face_surface_2D (mesh, pos, fid, return_barycenter = False) :
	"""Compute surface of a polygonal convex face
	
	:Parameters:
	 - `mesh` (:class:`openalea.container.Topomesh`)
	 - `pos` (dict of (pid|array) ) - geometrical position of points in space
	 - `fid` (fid) - id of the face to consider
	 - `return_barycenter` (bool) - tells wether the function will return
	                                the barycenter of the face too
	
	:Returns Type: float or (float,array)
	"""
	bary = centroid(mesh,pos,2,fid)
	
	#compute triangle for each edge
	surface = 0.
	for eid in mesh.borders(2,fid) :
		pid1,pid2 = mesh.borders(1,eid)
		surface += abs(cross(subtract(pos[pid1],bary),
		                     subtract(pos[pid2],bary) ) )
	
	#return
	if return_barycenter :
		return surface / 2.,bary
	else :
		return surface / 2.

    def test_pi_ops_nat(self):
        idx = PeriodIndex(['2011-01', '2011-02', 'NaT', '2011-04'],
                          freq='M', name='idx')
        expected = PeriodIndex(['2011-03', '2011-04', 'NaT', '2011-06'],
                               freq='M', name='idx')

        self._check(idx, lambda x: x + 2, expected)
        self._check(idx, lambda x: 2 + x, expected)
        self._check(idx, lambda x: np.add(x, 2), expected)

        self._check(idx + 2, lambda x: x - 2, idx)
        self._check(idx + 2, lambda x: np.subtract(x, 2), idx)

        # freq with mult
        idx = PeriodIndex(['2011-01', '2011-02', 'NaT', '2011-04'],
                          freq='2M', name='idx')
        expected = PeriodIndex(['2011-07', '2011-08', 'NaT', '2011-10'],
                               freq='2M', name='idx')

        self._check(idx, lambda x: x + 3, expected)
        self._check(idx, lambda x: 3 + x, expected)
        self._check(idx, lambda x: np.add(x, 3), expected)

        self._check(idx + 3, lambda x: x - 3, idx)
        self._check(idx + 3, lambda x: np.subtract(x, 3), idx)

def point_to_point_azimuth(point0, point1, out=None):
    """Azimuth of vector that joins two points.

    Parameters
    ----------
    (y0, x0) : tuple of array_like
    (y1, x1) : tuple of array_like
    out : array_like, optional
        An array to store the output. Must be the same shape as the output would
        have.

    Returns
    -------
    azimuth : array_like
        Azimuth of vector joining points; if *out* is provided, *v* will be
        equal to *out*.

    Examples
    --------
    >>> from landlab.grid.unstructured.base import point_to_point_azimuth
    >>> point_to_point_azimuth((0, 0), (1, 0))
    array([ 0.])
    >>> point_to_point_azimuth([(0, 1), (0, 1)], (1, 0))
    array([  0., -90.])
    >>> point_to_point_azimuth([(0, 1, 0), (0, 1, 2)], [(1, 1, 2), (0, 0, 4)])
    array([  0., -90.,  45.])
    """
    azimuth_in_rads = point_to_point_angle(point0, point1, out=out)
    if out is None:
        return (np.pi * .5 - azimuth_in_rads) * 180. / np.pi
    else:
        np.subtract(np.pi * .5, azimuth_in_rads, out=out)
        return np.multiply(out, 180. / np.pi, out=out)