import numpy as np import collections as c from scipy.stats import norm from matplotlib import animation from tempfile import NamedTemporaryFile from IPython.display import HTML import math import matplotlib.mlab as mlab def parametric(x_func, y_func, t_values, grid): fig = plt.figure() scat = plt.scatter(x, y, c=c, s=100) ax = plt.axes(*zip(grid[0], grid[1])) path = ax.scatter([], []) def init(): path.set_offsets([[]]) return path, def animate(i): t = t_values[i] x = x_func(t) y = y_func(t) scat.set_array(data[i]) return scat, return animation.FuncAnimation(fig, animate, init_func=init, frames=len(t_values), interval=20, blit=True, fargs=(scat,)) def display_animation(anim): plt.close(anim._fig) return HTML(anim_to_html(anim)) def square(x): return x**2 def probability(x, mean, stddev): return mlab.normpdf(x,mean,stddev) times = np.arange(0, 10, .4) # time goes from 0 sec to 10 sec in increments of 0.1 start = vector(4, 1) # the target starts at 8 meters to the right, and 5 meters up from the origin velocity = vector(-0.1, 0.7) # the target moves 0.2 meters to the left and 0.5 meters up in 1 sec def target_position(time): return start + velocity * time def x(t): return getX(target_position(t)) def y(t): return getY(target_position(t)) # TODO: make this more elegant mean = distance.euclidean(own_position, target) # this is where I want the data centered data = normal(mean, standard_deviation, sample_size) # data is a normal/standard distribution data = data.round(int(-log10(interval))) # round to the nearest interval, defined above # Next, count up each thing so that {distance_1: number of light rays for distance 1, distance_2: number of light rays for distance 2} return Counter(data)

Next, I should try to triangulate position for a single target, using simple triangles and circles. First, I need a peak extractor. A peak is where one value is higher than those adjacent to it. Each peak is part of a larger event, where the data is strictly increasing to the left, and strictly decreasing to the right. Each peak can be weighted by the length of the event it is a part of. From this we can produce a bar chart with distance on the x, and how long the event is on the y. We will only bother plotting an x and its associated y if it is a peak. To get a single number, we generate a weighted average. Since the data was generated while looking at a point, that was 5.6 meters away, if this works it should detect a peak in the returning light rays at 5.6. def get_peaks(data): data = sort(data) # data[x][0] is the distance, data[x][1] is the weight, or amplitude peaks = {} event_length = 0 current_peak = None prior_state = 'increasing' for distance in range(len(data) - 1): if data[distance][1] <= data[distance + 1][1]: # incerasing if prior_state == 'increasing': # continuation, status quo event_length += 1 # increment by one if prior_state == 'decreasing': # used to be decreasing, now increasing # start of a new event, so save old one and reset peaks[current_peak] = event_length # save old one event_length = 0 # reset # now set prior state to current state, for next time prior_state = 'increasing' if data[distance][1] >= data[distance + 1][1]: if prior_state == 'decreasing': # continuation, status quo event_length += 1 if prior_state == 'increasing': # used to be increasing, now decreasing # This must be a peak current_peak = data[distance][0] # now set prior state to current state, for next time prior_state = 'decreasing' return peaks peaks = get_peaks(test_data) bar_chart1(peaks, width=interval * 4) print ('Average: {average_peak!s}'.format(average_peak=weighted_average(peaks))) To make the classical algorithm work for multiple targets, I would need a way to decompose the sum of several Gaussian functions into their substituents. Imagine being able to see the green bell curve (the data coming back from the radar), and be able to split it into its substituent parts (the blue and yellow curve). This would be like what the Fast Fourier Transform does, except operating on Gaussian bell-curves instead of Sine waves t = np.arange(0., 5., 0.1) bell = lambda mean, stddev, t: exp(-(t - mean)**2 / (2*stddev)**2) / sqrt(2 * pi * mean**2) + 0.05 a = bell(2, 0.5, t) b = bell(4, 0.5, t) c = (a + b) plt.plot(t, a, 'b', t, b, 'y', t, c, 'g--')

Global variables: interval, grid,

Import things for graphing...



In [2]:

    
#%pylab inline
from operator import itemgetter
#from JSAnimation import IPython_display
from matplotlib import animation
from collections import Counter, OrderedDict
from scipy.spatial import distance
import matplotlib.patches as patches









    



Welcome to pylab, a matplotlib-based Python environment [backend: module://IPython.zmq.pylab.backend_inline].
For more information, type 'help(pylab)'.

Make some simple geometric functions for readability...



In [2]:

    
getX = lambda point: point[0]
getY = lambda point: point[1]
#sort = lambda dictionary: sorted(dictionary.items(), key=itemgetter(0))
in_rectangle = lambda point, grid: getX(grid[0]) < getX(point) < getX(grid[1]) and \
                                   getY(grid[0]) < getY(point) < getY(grid[1])
grid_array = lambda: zeros(shape=((getX(grid[1]) - getX(grid[0])) / res, (getY(grid[1]) - getY(grid[0])) / res), dtype=float64)

And some data processing stuff as well...



In [1]:

    
normalize = lambda data: [float(datum) / sum(data) for datum in data]
weighted_average = lambda data: average(distances, weights=data)
nearest = lambda value: min(int(value / interval), int(max_distance / interval) - 1)



In [1]:

    
def time_point(x_func, y_func, t_values):
    fig = plt.figure()
    ax = plt.axes(xlim=zip(*grid)[0], ylim=zip(*grid)[1])
    path = ax.scatter([], [])
    def init():
        path.set_offsets([[]])
        return path,
    def animate(i):
        t = t_values[i]
        x = x_func(t)
        y = y_func(t)
        path.set_offsets([[x, y]])
        return path,
    return animation.FuncAnimation(fig, animate, init_func=init, frames=len(t_values), interval=20, blit=True)

def bar_chart1(data):
    plt.figure()
    plt.title('Signal strength by distance')
    plt.xlabel('distance (in meters)'.format(interval))
    plt.ylabel('signal strength (normalized to 1)')
    plt.xlim(0, max_distance)
    plt.bar(distances, data, width=interval, linewidth=0)
    plt.show()

def visualize_circles(circles, points, ccolors, pcolors):
    plt.figure()
    ax = plt.axes(xlim=zip(*grid)[0], ylim=zip(*grid)[1], aspect='equal')
    for (center, radius), color in zip(circles, ccolors):
        ax.add_patch(patches.Circle(center, radius, fill=False, ls='solid',
                     lw=1.0, edgecolor=color))
    for point, color in zip(points, pcolors):
        ax.add_patch(patches.Circle(point, 0.1, fill=False, ls='solid',
                     lw=4.0, edgecolor=color))
    plt.show()

def heatmap1(data):
    plt.figure(figsize=(10, 2))
    ax1 = plt.subplot(211)
    plt.title('Signal strength by distance')
    plt.xlim(0, max_distance)
    plt.bar(distances, data, width=interval, linewidth=0)
    
    height = 10
    ax2 = plt.subplot(212, sharex=ax1)
    d = [data] * height
    ax2.imshow(d, interpolation='none', extent=[0, max_distance, 0, height])
    plt.show()

def heatmap2(f, title):
    Z = grid_array()
    for x in range(len(Z)):
        for y in range(len(Z[0])):
            Z[x][y] = f(x * res, y * res)
    plt.figure()
    ax = plt.axes(aspect='equal')
    ax.imshow(Z.T, origin='lower', interpolation='none',
              extent=[grid[0][0], grid[1][0], grid[0][1], grid[1][1]])
    plt.title(title)
    plt.show()

def heatmap3(a, b, titlea, titleb, targets):
    Za = grid_array()
    Zb = grid_array()
    for x in range(len(Za)):
        for y in range(len(Za[0])):
            Za[x][y] = a(x * res, y * res)
            Zb[x][y] = b(x * res, y * res)
    plt.figure(1, figsize=(9, 9))
    
    axa = plt.subplot(121, aspect='equal')
    axa.imshow(Za.T, origin='lower', interpolation='none', #cmap='Greys',
              extent=[grid[0][0], grid[1][0], grid[0][1], grid[1][1]])
    axa.set_xticks(numpy.arange(grid[0][0], grid[1][0], 1))
    axa.set_yticks(numpy.arange(grid[0][1], grid[1][1], 1))
    axa.plot(targets.T[0], targets.T[1], 'gx', markersize=25.0)
    plt.grid()
    plt.title(titlea)
    
    axb = plt.subplot(122, aspect='equal')
    axb.imshow(Zb.T, origin='lower', interpolation='none', #cmap='Greys',
              extent=[grid[0][0], grid[1][0], grid[0][1], grid[1][1]])
    axb.set_xticks(numpy.arange(grid[0][0], grid[1][0], 1))
    axb.set_yticks(numpy.arange(grid[0][1], grid[1][1], 1))
    axb.plot(targets.T[0], targets.T[1], 'wx', markersize=25.0)
    plt.grid()
    plt.title(titleb)