Changeset 104


Ignore:
Timestamp:
Jun 26, 2012, 2:02:31 PM (5 years ago)
Author:
jdbonior21
Message:

The multitone_small_v1.0.py works finally! The particle filter needs to be fine tuned to try and smooth out the tracking functionality.

Files:
2 added
4 edited

Legend:

Unmodified
Added
Removed
  • data_collection/finite_acquisition_experiments/finite_test_case.py

    r102 r104  
    55import matplotlib.pyplot as plt
    66
    7 usrp = uhd.usrp_source(device_addr="addr0=192.168.10.109, addr1=192.168.10.110, addr2=192.168.10.112, ", stream_args=uhd.stream_args(cpu_format="fc32",channels=range(3),),)
     7CENTER = 1e9
     8
     9usrp = uhd.usrp_source(device_addr="addr0=192.168.10.110, addr1=192.168.10.104, addr2=192.168.10.105, ", stream_args=uhd.stream_args(cpu_format="fc32",channels=range(3),),)
    810usrp.set_clock_source("external", 0)
    911usrp.set_time_source("external", 0)
     
    1315usrp.set_time_source("external", 2)
    1416usrp.set_time_unknown_pps(uhd.time_spec())
    15 usrp.set_samp_rate(200000)
    16 usrp.set_center_freq(1000000000, 0)
     17usrp.set_samp_rate(204918)
     18usrp.set_center_freq(CENTER, 0)
    1719usrp.set_gain(50, 0)
    1820usrp.set_antenna("RX2", 0)
    19 usrp.set_center_freq(1000000000, 1)
     21usrp.set_center_freq(CENTER, 1)
    2022usrp.set_gain(50, 1)
    2123usrp.set_antenna("RX2", 1)
    22 usrp.set_center_freq(1000000000, 2)
     24usrp.set_center_freq(CENTER, 2)
    2325usrp.set_gain(50, 2)
    2426usrp.set_antenna("RX2", 2)
    2527
    26 temp = usrp.finite_acquisition_v(2048000)
     28temp = usrp.finite_acquisition_v(2048)
    2729
    2830a = numpy.array(temp[0])
     
    5254plt.plot(abs(fft_c))
    5355plt.show()
     56
  • misc/simple_tx/2_tx_from_datfile.py

    r102 r104  
    2828                self.transmitter_B.set_gain(40, 0)
    2929                self.transmitter_B.set_antenna("TX/RX", 0)
    30                 self.file_source_B = gr.file_source(gr.sizeof_gr_complex*1, "ofdm_symbol_old_B.dat", True)
     30                self.file_source_B = gr.file_source(gr.sizeof_gr_complex*1, "ofdm_symbol_B.dat", True)
    3131
    3232                self.transmitter_A = uhd.usrp_sink(device_addr="addr=192.168.10.125", io_type=uhd.io_type.COMPLEX_FLOAT32, num_channels=1,)
     
    3535                self.transmitter_A.set_gain(40, 0)
    3636                self.transmitter_A.set_antenna("TX/RX", 0)
    37                 self.file_source_A = gr.file_source(gr.sizeof_gr_complex*1, "ofdm_symbol_old_A.dat", True)
     37                self.file_source_A = gr.file_source(gr.sizeof_gr_complex*1, "ofdm_symbol_A.dat", True)
    3838
    3939                ##################################################
  • sensors/variance_based_detection/multitone_small_v1.0.py

    r102 r104  
    33from gnuradio import uhd
    44from numpy import fft as npfft
     5import matplotlib.pyplot as plt
    56import numpy
     7from datetime import datetime
    68from collections import deque
    79import numpy
     
    911import random
    1012import math
    11 import time
    12 
    13 RX = [112, 113, 114, 115, 116, 117, 118, 119, 109, 110]
    14 LOCX = [-15, -15, -15, -5, 5, 15, 15, 15, 5, -5]
    15 LOCY = [-15, 0, 15, 15, 15, 15, 0, -15, -15, -15]
    16 TONE_INDEX = [300, 350, 1680, 1780]
    17 CORNERS = [-15, -15, 15, 15]
     13
     14RX = [112, 113, 114, 115, 116, 117, 118, 119, 109, 110, 104, 105]
     15LOCX = [-9, -9, -9, -3, 3, 9, 9, 9, 3, -3, 3, -3]
     16LOCY = [-6, 0, 6, 6, 6, 6, 0, -6, -6, -6, 0, 0]
     17#RX = [112, 113, 115, 116, 117, 118, 119, 109, 110, 104, 105]
     18#LOCX = [-9, -9, -3, 3, 9, 9, 9, 3, -3, 3, -3]
     19#LOCY = [-6, 0, 6, 6, 6, 0, -6, -6, -6, 0, 0]
     20#TONE_INDEX = [300, 350, 1680, 1780]
     21TONE_INDEX = [398, 909, 1422, 1934]
     22#TONE_INDEX = [398]
     23CORNERS = [-10, -10, 10, 10]
     24w = [[0.375, 0.375, 0.125, 0.125], #112
     25     [0.250, 0.375, 0.250, 0.125], #113
     26     [0.125, 0.375, 0.375, 0.125], #114
     27     [0.125, 0.250, 0.375, 0.250], #115
     28     [0.125, 0.250, 0.375, 0.250], #116
     29     [0.125, 0.125, 0.375, 0.375], #117
     30     [0.250, 0.125, 0.250, 0.375], #118
     31     [0.375, 0.125, 0.125, 0.375], #119
     32     [0.375, 0.250, 0.125, 0.250], #109
     33     [0.375, 0.250, 0.125, 0.250], #110
     34     [0.250, 0.250, 0.250, 0.250], #104
     35     [0.250, 0.250, 0.250, 0.250]] #105
    1836
    1937SAMP_RATE = 204918
    2038CENTER = 1000000000
    2139ANTENNA = "RX2"
    22 GAIN = 50
     40GAIN = 20
    2341PARTICLE_COUNT = 2000    # Total number of particles
    24 THRESHOLD = 0.15
    25 MEM = 10
     42THRESHOLD = 1e-5
     43MEM = 20
     44MEM_MEAN = 100
    2645
    2746#-----------------Some utility functions------------------#
     
    3857#-----------------Set up the GUI Environment--------------#
    3958print "Configuring display environment..."
    40 turtle.title("Clement Hall 402")
    41 #turtle.bgpic("room_layout.gif")
     59turtle.title("Clement Hall 402 Tracking Area")
    4260turtle.setworldcoordinates(CORNERS[0], CORNERS[1], CORNERS[2], CORNERS[3])
    4361mean_icon = turtle.Turtle()
    4462mean_icon.shape("circle")
    4563mean_icon.color("blue")
    46 est_icon = turtle.Turtle()
    47 est_icon.shape("circle")
    48 est_icon.color("red")
    49 est_icon.up()
    50 est_icon.setposition(-15, -15)
    51 est_icon.stamp()
    52 est_icon.setposition(-15, 0)
    53 est_icon.stamp()
    54 est_icon.setposition(-15, 15)
    55 est_icon.stamp()
    56 est_icon.setposition(-5, 15)
    57 est_icon.stamp()
    58 est_icon.setposition(5, 15)
    59 est_icon.stamp()
    60 est_icon.setposition(15, 15)
    61 est_icon.stamp()
    62 est_icon.setposition(15, 0)
    63 est_icon.stamp()
    64 est_icon.setposition(15, -15)
    65 est_icon.stamp()
    66 est_icon.setposition(5, -15)
    67 est_icon.stamp()
    68 est_icon.setposition(-5, -15)
    69 est_icon.stamp()
     64usrp_icon = turtle.Turtle()
     65usrp_icon.shape("square")
     66usrp_icon.color("black")
     67usrp_icon.up()
     68usrp_icon.setposition(-9, -6)
     69usrp_icon.stamp()
     70usrp_icon.setposition(-9, 0)
     71usrp_icon.stamp()
     72usrp_icon.setposition(-9, 6)
     73usrp_icon.stamp()
     74usrp_icon.setposition(-3, 6)
     75usrp_icon.stamp()
     76usrp_icon.setposition(3, 6)
     77usrp_icon.stamp()
     78usrp_icon.setposition(9, 6)
     79usrp_icon.stamp()
     80usrp_icon.setposition(9, 0)
     81usrp_icon.stamp()
     82usrp_icon.setposition(9, -6)
     83usrp_icon.stamp()
     84usrp_icon.setposition(3, -6)
     85usrp_icon.stamp()
     86usrp_icon.setposition(-3, -6)
     87usrp_icon.stamp()
     88usrp_icon.setposition(3, 0)
     89usrp_icon.stamp()
     90usrp_icon.setposition(-3, 0)
     91usrp_icon.stamp()
    7092print "complete.\n"
    7193
     
    95117print "\ncomplete.\n"
    96118print "Pausing for initialization period... \n"
    97 junk = usrp.finite_acquisition_v(10000)
     119junk = usrp.finite_acquisition_v(200000)
    98120
    99121#-----------------Initialize the Particles----------------#
     
    107129        exec('tone_%d_A = deque([],MEM)' % addr)
    108130        exec('tone_%d_B = deque([],MEM)' % addr)
    109         #exec('tone_%d_C = deque([],MEM)' % addr)
    110         #exec('tone_%d_D = deque([],MEM)' % addr)
     131        exec('tone_%d_C = deque([],MEM)' % addr)
     132        exec('tone_%d_D = deque([],MEM)' % addr)
     133        #exec('mu_%d_A = deque([],MEM)' % addr)
     134        #exec('mu_%d_B = deque([],MEM)' % addr)
     135        #exec('mu_%d_C = deque([],MEM)' % addr)
     136        #exec('mu_%d_D = deque([],MEM)' % addr)
    111137
    112138for _ in range(0, MEM):
     
    114140        n = 0
    115141        for addr in RX:
    116                 exec('fft_%d = npfft.fft(temp[%d], 2048)' % (addr, n))
     142                exec('fft_%d = npfft.fft(temp[%d], 2048)/2048' % (addr, n))
    117143                exec('tone_%d_A.append(abs(fft_%d[TONE_INDEX[0]]))' % (addr, addr))
    118                 exec('tone_%d_B.append(abs(fft_%d[TONE_INDEX[2]]))' % (addr, addr))
    119                 #exec('tone_%d_C.append(abs(fft_%d[TONE_INDEX[2]]))' % (addr, addr))
    120                 #exec('tone_%d_D.append(abs(fft_%d[TONE_INDEX[3]]))' % (addr, addr))
     144                exec('tone_%d_B.append(abs(fft_%d[TONE_INDEX[1]]))' % (addr, addr))
     145                exec('tone_%d_C.append(abs(fft_%d[TONE_INDEX[2]]))' % (addr, addr))
     146                exec('tone_%d_D.append(abs(fft_%d[TONE_INDEX[3]]))' % (addr, addr))
    121147                exec('mu_%d_A = abs(fft_%d[TONE_INDEX[0]])' % (addr, addr))
    122148                exec('mu_%d_B = abs(fft_%d[TONE_INDEX[1]])' % (addr, addr))
    123                 #exec('mu_%d_C = 0.0' % addr)
    124                 #exec('mu_%d_D = 0.0' % addr)
     149                exec('mu_%d_C = abs(fft_%d[TONE_INDEX[2]])' % (addr, addr))
     150                exec('mu_%d_D = abs(fft_%d[TONE_INDEX[3]])' % (addr, addr))
    125151                n += 1
    126152
     153print "\ncomplete.\n"
    127154print "Setup complete, beginning processing.... \n"
    128155print "Press Ctrl-C to quit\n"
     
    143170        for addr in RX:
    144171                exec('raw_%d = temp[%d]' % (addr, n))
    145                 exec('fft_%d = npfft.fft(raw_%d, 2048)' % (addr, addr))
     172                exec('fft_%d = npfft.fft(raw_%d, 2048)/2048' % (addr, addr))
    146173                exec('tone_%d_A.append(abs(fft_%d[TONE_INDEX[0]]))' % (addr, addr))
    147174                exec('tone_%d_B.append(abs(fft_%d[TONE_INDEX[1]]))' % (addr, addr))
    148                 #exec('tone_%d_C.append(abs(fft_%d[TONE_INDEX[2]]))' % (addr, addr))
    149                 #exec('tone_%d_D.append(abs(fft_%d[TONE_INDEX[3]]))' % (addr, addr))
    150                 #exec('var_%d_A = numpy.var(tone_%d_A) / numpy.mean(tone_%d_A)' % (addr, addr, addr))
    151                 #exec('var_%d_B = numpy.var(tone_%d_B) / numpy.mean(tone_%d_B)' % (addr, addr, addr))
    152                 #exec('var_%d_A = numpy.var(tone_%d_A)' % (addr, addr))
    153                 #exec('var_%d_B = numpy.var(tone_%d_B)' % (addr, addr))
    154                 #exec('var_%d_C = numpy.var(tone_%d_C) / numpy.mean(tone_%d_C)' % (addr, addr, addr))
    155                 #exec('var_%d_D = numpy.var(tone_%d_D) / numpy.mean(tone_%d_D)' % (addr, addr, addr))
     175                exec('tone_%d_C.append(abs(fft_%d[TONE_INDEX[2]]))' % (addr, addr))
     176                exec('tone_%d_D.append(abs(fft_%d[TONE_INDEX[3]]))' % (addr, addr))
    156177                exec('mu_%d_A = mean(abs(fft_%d[TONE_INDEX[0]]), mu_%d_A, b)' % (addr, addr, addr))
    157178                exec('mu_%d_B = mean(abs(fft_%d[TONE_INDEX[1]]), mu_%d_B, b)' % (addr, addr, addr))
    158                 #exec('mu_%d_C = mean(abs(fft_%d[TONE_INDEX[2]]), mu_%d_C, b)' % (addr, addr, addr))
    159                 #exec('mu_%d_D = mean(abs(fft_%d[TONE_INDEX[3]]), mu_%d_D, b)' % (addr, addr, addr))                           
     179                exec('mu_%d_C = mean(abs(fft_%d[TONE_INDEX[2]]), mu_%d_C, b)' % (addr, addr, addr))
     180                exec('mu_%d_D = mean(abs(fft_%d[TONE_INDEX[3]]), mu_%d_D, b)' % (addr, addr, addr))                             
    160181                exec('var_%d_A = numpy.var(tone_%d_A)/mu_%d_A' % (addr, addr, addr))
    161182                exec('var_%d_B = numpy.var(tone_%d_B)/mu_%d_B' % (addr, addr, addr))
    162                 #exec('var_%d_C = numpy.var(tone_%d_C)/mu_%d_C' % (addr, addr, addr))
    163                 #exec('var_%d_D = numpy.var(tone_%d_D)/mu_%d_D' % (addr, addr, addr))
    164                 #exec('total_%d = var_%d_A + var_%d_B + var_%d_C + var_%d_D' % (addr, addr, addr, addr, addr))
    165                 #exec('total_%d = (var_%d_A + var_%d_B + var_%d_C + var_%d_D)/4' % (addr, addr, addr, addr, addr))
    166                 exec('total_%d = var_%d_A + var_%d_B' % (addr, addr, addr))
     183                exec('var_%d_C = numpy.var(tone_%d_C)/mu_%d_C' % (addr, addr, addr))
     184                exec('var_%d_D = numpy.var(tone_%d_D)/mu_%d_D' % (addr, addr, addr))
     185                exec('total_%d = (var_%d_A * w[n][0] + var_%d_B * w[n][1] + var_%d_C * w[n][2] + var_%d_D * w[n][3])' % (addr, addr, addr, addr, addr))
     186                exec("print total_%d" % addr)
    167187                exec('if total_%d >= THRESHOLD:\n    temp_x = total_%d * LOCX[%d]\n    temp_y = total_%d * LOCY[%d]\nelse:\n    total_%d = 0\n    temp_x = 0\n    temp_y = 0' % (addr, addr, n, addr, n, addr))
    168188                exec('total_weight += total_%d' % addr)
     
    171191                n += 1
    172192
    173         #print numpy.var(tone_114_A)   
    174         #print mu_114_A
    175         #print var_110_A,   mu_110_A
    176         #print "total_114: %10.8f" % total_114
    177193        b += 1
    178        
     194
     195        #print total_weight     
     196
    179197        if total_weight != 0:
    180198                x_est = sum(x_vec) / total_weight
    181199                y_est = sum(y_vec) / total_weight
    182                 #print "The estimated location is: %8.4f, %8.4f" % (x_est, y_est)
    183                 #print total_weight
    184                 #print total_112, total_114, total_116, total_118
    185 
    186                 #--------------------Particle Filter----------------------#
    187                 new_x = x_est
    188                 new_y = y_est
     200
     201        #--------------------Particle Filter----------------------#
     202        new_x = x_est
     203        new_y = y_est
    189204       
    190                 # Show current state and calculate weights using gaussian kernel
    191                 for k in range(0, PARTICLE_COUNT):
    192                         d = math.sqrt(math.pow((particles[k][0] - new_x), 2) + math.pow((particles[k][1] - new_y), 2)) / 100
    193                         particles[k][2] = alt_gauss(d)
    194                 # Normalise weights
    195                 nu = sum([particles[i][2] for i in range(len(particles))])
    196                 if nu:
    197                         for k in range(len(particles)):
    198                                 particles[k][2] = particles[k][2] / nu
    199                
    200                 # Try to find current best estimate for display
    201                 m_x = sum([particles[i][0]*particles[i][2] for i in range(len(particles))]) / sum([particles[i][2] for i in range(0, len(particles))])
    202                 m_y = sum([particles[i][1]*particles[i][2] for i in range(len(particles))]) / sum([particles[i][2] for i in range(0, len(particles))])
    203 
    204                 # Print the filtered target estimate to the screen
    205                 #y_out = m_y * 2.0405 - 422
    206                 #x_out = m_x * 1.6732 - 295  # Conversion factors to make this fit the actual room dimensions
    207                 y_out = m_y
    208                 x_out = m_x
    209                 #x_est_display = new_x * 1.6732 - 295
    210                 #y_est_display = new_y * 2.0405 - 422
    211                 #print "Real Location:     %.4f, %.4f" %        (x_est, y_est)
    212                 #print "Filtered Location: %.4f, %.4f" % (x_out, y_out)
    213                 #print "The target is at location: %f, %f" % (x_est_display, y_est_display)
    214                 #print "The target is at location: %8.4f, %8.4f" % (x_est, y_est)
    215 
    216                 mean_icon.setposition(x_out, y_out)
    217                 #est_icon.setposition(x_est_display, y_est_display)
    218                 #est_icon.setposition(x_est, y_est)
    219 
    220                 # ---------- Shuffle particles ----------
    221                 for k in range(0, PARTICLE_COUNT):
    222                         #if particles[k][2] < 0.005:
    223                         if particles[k][2] < 0.05:
    224                                 particles[k] = [random.uniform(CORNERS[0], CORNERS[2]), random.uniform(CORNERS[1], CORNERS[3]), 1.0]
    225 
    226                 dx = m_x - old_m_x
    227                 dy = m_y - old_m_y
    228                 old_m_x = m_x
    229                 old_m_y = m_y
     205        # Show current state and calculate weights using gaussian kernel
     206        for k in range(0, PARTICLE_COUNT):
     207                d = math.sqrt(math.pow((particles[k][0] - new_x), 2) + math.pow((particles[k][1] - new_y), 2)) / 100
     208                particles[k][2] = alt_gauss(d)
     209        # Normalise weights
     210        nu = sum([particles[i][2] for i in range(len(particles))])
     211        if nu:
     212                for k in range(len(particles)):
     213                        particles[k][2] = particles[k][2] / nu
    230214       
    231                 # Move particles according to my belief of movement (this may
    232                 # be different than the real movement, but it's all I got)
    233                 for k in range(0, PARTICLE_COUNT):
    234                         particles[k][0] += dx
    235                         particles[k][1] += dy
    236 
    237 
     215        # Try to find current best estimate for display
     216        m_x = sum([particles[i][0]*particles[i][2] for i in range(len(particles))]) / sum([particles[i][2] for i in range(0, len(particles))])
     217        m_y = sum([particles[i][1]*particles[i][2] for i in range(len(particles))]) / sum([particles[i][2] for i in range(0, len(particles))])
     218
     219        # Print the filtered target estimate to the screen
     220        y_out = m_y
     221        x_out = m_x
     222        #print "Real Location:     %.4f, %.4f" %        (x_est, y_est)
     223        #print "Filtered Location: %.4f, %.4f" % (x_out, y_out)
     224        #print "The target is at location: %f, %f" % (x_est_display, y_est_display)
     225        #print "The target is at location: %8.4f, %8.4f" % (x_est, y_est)
     226
     227        mean_icon.setposition(x_out, y_out)
     228
     229        # ---------- Shuffle particles ----------
     230        for k in range(0, PARTICLE_COUNT):
     231                #if particles[k][2] < 0.005:
     232                if particles[k][2] < 0.05:
     233                        particles[k] = [random.uniform(CORNERS[0], CORNERS[2]), random.uniform(CORNERS[1], CORNERS[3]), 1.0]
     234
     235        dx = m_x - old_m_x
     236        dy = m_y - old_m_y
     237        old_m_x = m_x
     238        old_m_y = m_y
     239        # Move particles according to my belief of movement (this may
     240        # be different than the real movement, but it's all I got)
     241        for k in range(0, PARTICLE_COUNT):
     242                particles[k][0] += dx
     243                particles[k][1] += dy
  • sensors/variance_based_detection/transmitters.py

    r102 r104  
    1515                self.gain = gain = 40
    1616
    17                 self.tx_A = uhd.usrp_sink(device_addr="addr=192.168.10.109", io_type=uhd.io_type.COMPLEX_FLOAT32, num_channels=1,)
     17                self.tx_A = uhd.usrp_sink(device_addr="addr=192.168.10.102", io_type=uhd.io_type.COMPLEX_FLOAT32, num_channels=1,)
    1818                self.tx_A.set_samp_rate(samp_rate)
    1919                self.tx_A.set_center_freq(carrier, 0)
     
    2222                self.file_source_A = gr.file_source(gr.sizeof_gr_complex*1, "ofdm_symbol_A.dat", True)
    2323
    24                 self.tx_B = uhd.usrp_sink(device_addr="addr=192.168.10.110", io_type=uhd.io_type.COMPLEX_FLOAT32, num_channels=1,)
     24                self.tx_B = uhd.usrp_sink(device_addr="addr=192.168.10.103", io_type=uhd.io_type.COMPLEX_FLOAT32, num_channels=1,)
    2525                self.tx_B.set_samp_rate(samp_rate)
    2626                self.tx_B.set_center_freq(carrier, 0)
Note: See TracChangeset for help on using the changeset viewer.