Changeset 91


Ignore:
Timestamp:
May 30, 2012, 2:26:49 PM (6 years ago)
Author:
jdbonior21
Message:

created a tracking toolbox for use with the variance based detection experiments.

Location:
sensors
Files:
2 added
4 edited

Legend:

Unmodified
Added
Removed
  • sensors/tracking_filters/particle_filter_test_case/particle_standalone.py

    r89 r91  
    1515
    1616
    17 path_x = range(-100, 300, 20)
    18 y_a = range(-200, 0, 20)
    19 y_b = range(0, -200, -20)
     17x_a = range(-100, 100, 40)
     18x_b = range(100, -300, -40)
     19x_c = range(-300, -100, 40)
     20path_x = x_a + x_b + x_c
     21y_a = range(-300, 100, 40)
     22y_b = range(100, -300, -40)
    2023path_y = y_a + y_b
    2124
     
    2528# Some utility functions
    2629
    27 def add_noise(level, *coords):
    28         return [x + random.uniform(-level, level) for x in coords]
    29 
    30 def add_little_noise(*coords):
    31         return add_noise(0.02, *coords)
    32 
    33 def add_some_noise(*coords):
    34         return add_noise(0.25, *coords)
    35 
    36 def random_loc():
    37         x = random.uniform(-500, 500)
    38         y = random.uniform(-500, 500)
    39         return x, y
    40 
    4130# This is just a gaussian kernel I pulled out of my hat, to transform
    4231# values near to robbie's measurement => 1, further away => 0
     
    4534def alt_gauss(d):
    4635        g = math.e ** -(d ** 2 / (2 * sigma2))
    47         print "g = %f" % g
     36        #print "g = %f" % g
    4837        return g
    4938
     
    6453old_m_y = 0
    6554
     55time.sleep(5)
     56
    6657for x, y in zip(path_x, path_y):
    67         new_x = x + random.gauss(0, 2.5)
    68         new_y = y + random.gauss(0, 2.5)
     58        #new_x = x + random.uniform(-20, 20)
     59        #new_y = y + random.uniform(-20, 20)
     60        new_x = x + random.gauss(0, 20)
     61        new_y = y + random.gauss(0, 20)
     62
    6963
    7064        # ---------- Show current state and calculate weights using gaussian kernel ----------
     
    7670        for k in range(0, PARTICLE_COUNT):
    7771                d = math.sqrt(math.pow((particles[k][0] - new_x), 2) + math.pow((particles[k][1] - new_y), 2)) / 100
    78                 print "d = %f" % d
     72                #print "d = %f" % d
    7973                particles[k][2] = alt_gauss(d)
    80                 print "The weight is: %f" % particles[k][2]
     74                #print "The weight is: %f" % particles[k][2]
    8175        # Normalise weights
    8276        nu = sum([particles[i][2] for i in range(len(particles))])
    83         print "nu = %f" % nu   
     77        #print "nu = %f" % nu   
    8478        if nu:
    8579                for k in range(len(particles)):
     
    9185        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))])
    9286
    93         print m_x, m_y
     87        #print m_x, m_y
    9488
    9589        # Display the Mean Value
     
    9791        turtle.setposition(m_x, m_y)
    9892        turtle.shape("circle")
     93        turtle.stamp()
     94        turtle.update()
     95
     96        # Display the estimate
     97        turtle.color("red")
     98        turtle.setposition(new_x, new_y)
     99        turtle.shape("classic")
    99100        turtle.stamp()
    100101        turtle.update()
     
    128129
    129130        time.sleep(1)
     131time.sleep(5)
  • sensors/tracking_filters/tracking.py

    r87 r91  
    2222from gnuradio import gr
    2323import gnuradio.extras
     24from __future__ import absolute_import
     25import numpy
     26import random
     27import math
     28import bisect
     29import turtle
     30import time
    2431
    2532
    26 def add_noise(level, *coords):
    27     return [x + random.uniform(-level, level) for x in coords]
     33PARTICLE_COUNT = 2000    # Total number of particles
    2834
    29 def add_little_noise(*coords):
    30     return add_noise(0.02, *coords)
     35# ------------------------------------------------------------------------
     36# Some utility functions
    3137
    32 def add_some_noise(*coords):
    33     return add_noise(0.1, *coords)
     38sigma2 = 0.5 ** 2
     39def alt_gauss(d):
     40        g = math.e ** -(d ** 2 / (2 * sigma2))
     41        #print "g = %f" % g
     42        return g
    3443
    35 sigma2 = 0.9 ** 2
    36 def gauss_weight(d):
    37     g = math.e ** -(d ** 2 / (2 * sigma2))
    38     return g
    39        
    40 def distance(a, b, c, d):
    41         g = math.sqrt(math.pow(math.fabs(a - c), 2) + math.pow(math.fabs(b - d), 2))
    42         return g
    43        
    4444class particle_filter(gr.block):
    4545        """
    46         Particle filter, uses 2000 particles by default
     46        Particle filter for moving target tracking.
    4747        """
    48         def __init__(self, N_particles=2000):
    49        
    50                 self.N_particles = N_particles
    51                 self.old_loc = [0, 0]
    52        
    53                 gr.sync_block.__init__(
     48        def __init__(self):
     49                gr.block.__init__(
    5450                        self,
    5551                        name = "particle_filter",
    56                         in_sig = [(numpy.float32, 2)],
    57                         out_sig = [(numpy.float32, N_particles), (numpy.float32, N_particles)]
     52                        in_sig = [numpy.float32, numpy.float32],
     53                        out_sig = [numpy.float32]
    5854                )
     55                # Create the world
     56                turtle.title("This is a test")
     57                turtle.setworldcoordinates(-500, -500, 500, 500)
     58                turtle.bgpic("room_layout.gif")
     59                turtle.up()
     60
     61                # initial distribution assigns each particle an equal probability
     62                particles = []
     63                for _ in range(0, PARTICLE_COUNT):
     64                        new_p = [random.uniform(-500, 500), random.uniform(-500, 500), 1.0]
     65                        particles.append(new_p)
     66
     67                one_px = float(turtle.window_width()) / float(1000) / 2
     68                old_m_x = 0
     69                old_m_y = 0
     70
     71        def work(self, input_items, output_items):
     72                # input_items[0] should be the x value and input_items[1] should be the y value
     73                new_x = input_items[0]
     74                new_y = input_items[1]
     75
     76                # ---------- Show current state and calculate weights using gaussian kernel ----------
     77                turtle.hideturtle()       
     78                turtle.shape("classic")
     79                draw_cnt = 0
     80                px = {}
     81                for k in range(0, PARTICLE_COUNT):
     82                        d = math.sqrt(math.pow((particles[k][0] - new_x), 2) + math.pow((particles[k][1] - new_y), 2)) / 100
     83                        particles[k][2] = alt_gauss(d)
     84                # Normalise weights
     85                nu = sum([particles[i][2] for i in range(len(particles))])
     86                if nu:
     87                        for k in range(len(particles)):
     88                                particles[k][2] = particles[k][2] / nu
    5989       
    60         #       Create initial particle locations
    61         for _ in range(0)
    62                 particles = particle_tools.random_place(10, 10)
    63        
    64         def work(self, input_items, output_items):
     90                # ---------- Try to find current best estimate for display ----------
     91                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))])
     92                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))])
    6593
    66                 #       Calculate particle errors and update weights
    67                 for p in particles:
    68                         d = distance(p.xy, input_items[0][:])
    69                         p.weight = gauss_weight(d)
     94                # Display the Mean Value
     95                turtle.color("blue")
     96                turtle.setposition(m_x, m_y)
     97                turtle.shape("circle")
     98                turtle.stamp()
     99                turtle.update()
    70100
    71                 #       Normalize weights
    72                 b = sum(p.w for p in particles)
    73                 if b:
    74                         for p in particles:
    75                                 p.w = p.w / b
    76                
    77                 # create a weighted distribution, for fast picking
    78                 dist = WeightedDistribution(particles)
     101                # Display the estimate
     102                turtle.color("red")
     103                turtle.setposition(new_x, new_y)
     104                turtle.shape("classic")
     105                turtle.stamp()
     106                turtle.update()
    79107
    80                 for _ in particles:
    81                         p = dist.pick()
    82                         if p is None:  # No pick b/c all totally improbable
    83                                 new_particle = p.random_place(10, 10)
    84                         else:
    85                                 new_particle = Particle(p.x, p.y, noisy=True)
    86                         new_particles.append(new_particle)
     108                # ---------- Shuffle particles ----------
     109                for k in range(0, PARTICLE_COUNT):
     110                        if particles[k][2] < 0.005:
     111                                particles[k] = [random.uniform(-500, 500), random.uniform(-500, 500), 1.0]
    87112
    88                 particles = new_particles
    89                
    90                 #       Calculate heading, speed
    91                 x_move = mean_loc[0] - old_loc[0]
    92                 y_move = mean_loc[1] - old_loc[1]
    93                
    94                 #       Update particles and locations
    95                 old_loc = mean_loc
    96                 i = 0
    97                 for p in particles:
    98                         p.move_by(x_move, y_move)
    99                         output[0][i][0], output[0][i][1] = p.xy
    100        
     113                old_x = new_x
     114                old_y = new_y
     115                dx = m_x - old_m_x
     116                dy = m_y - old_m_y
     117                old_m_x = m_x
     118                old_m_y = m_y
     119
     120                # Move particles according to my belief of movement (this may
     121                # be different than the real movement, but it's all I got)
     122                for k in range(0, PARTICLE_COUNT):
     123                        particles[k][0] += dx
     124                        particles[k][1] += dy
     125
     126                output_items[0][:] = 0 # Just because I don't know what to do with it yet
     127
     128                # This is the end of the block
    101129                return len(output_items[0])
    102130
    103 # --------------------------------------------------------------
    104                
    105 class particle_tools(object):
    106         def __init__(self, x, y, noisy=False):
    107                 if noisy:
    108                         x, y = add_some_noise(x, y)
    109         self.x = x
    110         self.y = y
    111         self.w = w
    112 
    113         def __repr__(self):
    114                 return "(%f, %f, w=%f)" % (self.x, self.y, self.w)
    115 
    116         @property
    117         def xy(self):
    118                 return self.x, self.y
    119 
    120         def random_place(self, width, height):
    121                 x = random.uniform(0, width)
    122                 y = random.uniform(0, height)
    123                 return x, y
    124 
    125         def move_by(self, x, y):
    126                 self.x += x
    127                 self.y += y
    128        
    129 # --------------------------------------------------------------
    130        
    131 class WeightedDistribution(object):
    132     def __init__(self, state):
    133         accum = 0.0
    134         self.state = [p for p in state if p.w > 0]
    135         self.distribution = []
    136         for x in self.state:
    137             accum += x.w
    138             self.distribution.append(accum)
    139 
    140     def pick(self):
    141         try:
    142             return self.state[bisect.bisect_left(self.distribution, random.uniform(0, 1))]
    143         except IndexError:
    144             # Happens when all particles are improbable w=0
    145             return None
    146        
    147131#---------------------------------------------------------------
    148132#       Kalman Filter
    149133#---------------------------------------------------------------
     134
     135class kalman(gr.block):
     136        """
     137        kalman filtering for target localization.
     138        """
     139        def __init__(self):
     140                gr.block.__init__(
     141                        self,
     142                        name = "kalman",
     143                        in_sig = [numpy.float32],
     144                        out_sig = [numpy.float32]
     145                )
     146
     147        def work(self, input_items, output_items):
     148                global XHATMINUS
     149                global PMINUS
     150                global XHAT
     151                global X_PREV
     152                global V
     153                global P
     154
     155                in_data = input_items[0]
     156                #print type(in_data)
     157                #print 'in_data = %f' % in_data[0]
     158               
     159                input = in_data[0]
     160                if not math.isnan(float(in_data)):
     161                        # time update
     162                        XHATMINUS = XHAT + V
     163                        print 'XHATMINUS = %f' % XHATMINUS
     164                        PMINUS = P + Q
     165
     166                        # measurement update
     167                        K = PMINUS / ( PMINUS + R )
     168                        XHAT = XHATMINUS + K * (in_data - XHATMINUS)
     169                        temp = float(in_data) - float(XHATMINUS)
     170                        #print 'temp = %f' % temp
     171                #       print type(in_data)
     172                        #print type(XHATMINUS)
     173                        #print type(temp)
     174                        print 'XHAT = %f' % XHAT
     175                        P = (1 - K) * PMINUS
     176                        V = XHAT - X_PREV
     177                        X_PREV = XHAT
     178
     179                output_items[0][:] = XHAT
     180
     181                #if not math.isnan(output_items[0][:]):
     182                #       now = datetime.now()
     183                #       print "%.4f at %s" % (output_items[0][:], now.isoformat(' '))
     184                #       output_items[0][:] = 0
     185
     186                #print output_items[0][:]
     187                return len(output_items[0])
  • sensors/variance_based_detection/multitone_var_v1.4.py

    r84 r91  
    1111from gnuradio import gr
    1212from gnuradio import uhd
     13import gnuradio.extras
    1314import numpy
    1415from datetime import datetime
     
    1718from gnuradio import window
    1819import time
    19 import tracking
     20from stats import variance
     21from tracking import particle_filter
    2022
    2123SAMP_RATE = 262144
     
    2729MEAN_SIZE = 10000
    2830FFT_SIZE = 512
    29 THRESH = -200
    30 RX = [111, 112, 113, 114, 115, 116, 117, 118]
    31 TX = [110]
    32 LOCX = [1, 2, 3, 4, 5, 6, 7, 8]
    33 LOCY = [1, 2, 3, 4, 5, 6, 7, 8]
     31THRESH = -100
     32RX = [111, 112, 113]
     33TX = []
     34LOCX = [1, 2, 3]
     35LOCY = [1, 2, 3]
    3436
    3537Q = 1e-5 # process variance
     
    4244P = 0.120
    4345
    44 class tone_filter(gr.sync_block):
     46class tone_filter(gr.block):
    4547        """
    4648        Separation of desired tonal components.
    4749        """
    4850        def __init__(self):
    49                 gr.sync_block.__init__(
     51                gr.block.__init__(
    5052                        self,
    5153                        name = "tone_filter",
     
    6062
    6163                return len(output_items[0])
    62                        
    63 #class variance(gr.sync_block):
    64 #       """
    65 #       Calculation of variance.
    66 #       """
    67 #       def __init__(self):
    68 #               gr.sync_block.__init__(
    69 #                       self,
    70 #                       name = "variance",
    71 #                       in_sig = [(numpy.float32, VAR_SIZE)],
    72 #                       out_sig = [numpy.float32]
    73 #               )
    74 #
    75 #       def work(self, input_items, output_items):
    76 #               output_items[0][:] = (numpy.var(input_items[0]))
    77 #               return len(output_items[0])
    78 
    79 
    80 class location(gr.sync_block):
     64
     65class location(gr.block):
    8166        def __init__(self):
    8267                in_sig_list = []
    8368                for i in range(len(RX)):
    8469                        in_sig_list.append(numpy.float32)
    85                 gr.sync_block.__init__(
     70                gr.block.__init__(
    8671                        self,
    8772                        name = "location",
    8873                        in_sig = in_sig_list[:],       
    89                         out_sig = [(numpy.float32, 2)]
     74                        out_sig = [numpy.float32, numpy.float32]
    9075                )
    9176
     
    9580                        temp.append(input_items[i])
    9681                temp = numpy.array(temp)
    97                 temp2 = temp/numpy.sum(temp)
    98                 output_items[0][0] = numpy.sum(LOCX*temp2.T)
    99                 output_items[0][1] = numpy.sum(LOCY*temp2.T)
    100                
    101                 if not math.isnan(output_items[0][:]):
     82                if not (numpy.sum(temp) == 0):
     83                        temp2 = temp/numpy.sum(temp)
     84                        output_items[0] = [numpy.sum(LOCX*temp2.T)]
     85                        output_items[1] = [numpy.sum(LOCY*temp2.T)]
    10286                        now = datetime.now()
    103                         print "%.4f at %s" % (output_items[0][:], now.isoformat(' '))
    104 
     87                        print "%.4f, %.4f at %s" % (output_items[0][0], output_items[1][0], now.isoformat(' '))
     88                else:
     89                        output_items[0] = ["no change"]
     90                        output_items[1] = ["no change"]
    10591                return len(output_items[0])
    106 
    107 class kalman(gr.sync_block):
    108         """
    109         kalman filtering for target localization.
    110         """
    111         def __init__(self):
    112                 gr.sync_block.__init__(
    113                         self,
    114                         name = "kalman",
    115                         in_sig = [numpy.float32],
    116                         out_sig = [numpy.float32]
    117                 )
    118 
    119         def work(self, input_items, output_items):
    120                 global XHATMINUS
    121                 global PMINUS
    122                 global XHAT
    123                 global X_PREV
    124                 global V
    125                 global P
    126 
    127                 in_data = input_items[0]
    128                 #print type(in_data)
    129                 #print 'in_data = %f' % in_data[0]
    130                
    131                 input = in_data[0]
    132                 if not math.isnan(float(in_data)):
    133                         # time update
    134                         XHATMINUS = XHAT + V
    135                         print 'XHATMINUS = %f' % XHATMINUS
    136                         PMINUS = P + Q
    137 
    138                         # measurement update
    139                         K = PMINUS / ( PMINUS + R )
    140                         XHAT = XHATMINUS + K * (in_data - XHATMINUS)
    141                         temp = float(in_data) - float(XHATMINUS)
    142                         #print 'temp = %f' % temp
    143                 #       print type(in_data)
    144                         #print type(XHATMINUS)
    145                         #print type(temp)
    146                         print 'XHAT = %f' % XHAT
    147                         P = (1 - K) * PMINUS
    148                         V = XHAT - X_PREV
    149                         X_PREV = XHAT
    150 
    151                 output_items[0][:] = XHAT
    152 
    153                 #if not math.isnan(output_items[0][:]):
    154                 #       now = datetime.now()
    155                 #       print "%.4f at %s" % (output_items[0][:], now.isoformat(' '))
    156                 #       output_items[0][:] = 0
    157 
    158                 #print output_items[0][:]
    159                 return len(output_items[0])
    160 
    16192
    16293class var_detection(gr.top_block):
     
    203134                        exec('self.gr_complex_to_mag_%d = gr.complex_to_mag(FFT_SIZE)' % Y)
    204135                       
    205                         exec('self.variance_%d_0 = variance()' % Y)
    206                         exec('self.variance_%d_1 = variance()' % Y)
    207                         exec('self.variance_%d_2 = variance()' % Y)
     136                        exec('self.variance_%d_0 = variance(VAR_SIZE)' % Y)
     137                        exec('self.variance_%d_1 = variance(VAR_SIZE)' % Y)
     138                        exec('self.variance_%d_2 = variance(VAR_SIZE)' % Y)
    208139                        exec('self.gr_moving_average_%d_0 = gr.moving_average_ff(MEAN_SIZE, 0.0001, 40000)' % Y)
    209140                        exec('self.gr_moving_average_%d_1 = gr.moving_average_ff(MEAN_SIZE, 0.0001, 40000)' % Y)
     
    217148                self.particle = particle_filter()
    218149                #self.kalman = kalman()
    219                 self.sink_0 = gr.null_sink(gr.sizeof_float*1, 2000)
    220                 self.sink_1 = gr.null_sink(gr.sizeof_float*1, 2000)
     150                self.sink_0 = gr.null_sink(gr.sizeof_float*1)
     151                #self.sink_1 = gr.null_sink(gr.sizeof_float*1)
    221152
    222153                ##################################################
     
    255186
    256187                self.connect((self.location, 0), (self.particle, 0))
    257                 self.connect((self.particle, 0), (self.sink_0, 0)
    258                 self.connect((self.particle, 1), (self.sink_1, 0)
    259                 #self.connect((self.location, 0), (self.null_sink_0, 0))
     188                self.connect((self.location, 1), (self.particle, 1))
     189                self.connect((self.particle, 0), (self.sink_0, 0))
     190                #self.connect((self.location, 0), (self.sink_0, 0))
     191                #self.connect((self.location, 1), (self.sink_1, 0))
    260192
    261193        def get_samp_rate(self):
  • sensors/variance_based_detection/stats.py

    r84 r91  
    66#-----------------------------------------------------------------
    77
    8 class variance(gr.sync_block):
     8from gnuradio import gr
     9import gnuradio.extras
     10import numpy
     11
     12class variance(gr.block):
    913        """
    1014        Calculation of variance.
    1115        """
    12         def __init__(self):
    13                 gr.sync_block.__init__(
     16        def __init__(self, VAR_SIZE):
     17                gr.block.__init__(
    1418                        self,
    1519                        name = "variance",
Note: See TracChangeset for help on using the changeset viewer.