00001
00002
00003 import numpy as np, math
00004 import cPickle as pkl
00005 import yaml
00006 import scipy.stats as stats
00007
00008 class RfidModel:
00009 def __init__( self, yaml_fname ):
00010
00011
00012
00013
00014
00015
00016
00017 f = open( yaml_fname )
00018 yaml_config = yaml.load( f )
00019 f.close()
00020
00021
00022
00023
00024 f = open( yaml_config['use_combined'].replace('.pkl','_MODEL.pkl'), 'r' )
00025 self.model = pkl.load( f )
00026 f.close()
00027
00028 self.default_detect = 0.0
00029 self.default_rssi = 1.0
00030
00031 self.num = 1
00032
00033
00034
00035
00036 def lookup( self, x, y ):
00037
00038
00039
00040 xind = np.sum( x > self.model['xedges'] ) - 1
00041 yind = np.sum( y > self.model['yedges'] ) - 1
00042
00043
00044 if xind < 0 or xind >= len(self.model['xedges'])-1 or yind < 0 or yind >= len(self.model['yedges'])-1:
00045 return ( None, None, None )
00046
00047
00048 return ( self.model['detect_model'][yind,xind],
00049 self.model['rssi_model'][yind,xind],
00050 self.model['stddev_model'][yind,xind] )
00051
00052 def sample( self, x, y ):
00053
00054 detect, rssi, stddev = self.lookup( x, y )
00055 if (not detect) or (not rssi) or (not stddev):
00056 return -1
00057
00058 detected = np.random.uniform() <= detect
00059 if not detected:
00060 return -1
00061
00062 return np.clip( np.random.normal( loc=rssi, scale=stddev ),
00063 np.min( self.model['rssi_model'] ),
00064 np.max( self.model['rssi_model'] ))
00065
00066
00067 def prob( self, x, y, measurement ):
00068
00069 detect, rssi, stddev = self.lookup( x, y )
00070 if (not detect) or (not rssi) or (not stddev):
00071 return self.default_detect, self.default_rssi
00072
00073 rv = stats.norm( loc = rssi, scale = stddev )
00074 return detect, rv.pdf( measurement )
00075
00076
00077 def weight( self, measurement, particle ):
00078
00079 detect_prob, rssi_prob = self.prob( particle[0], particle[1], measurement )
00080 return detect_prob * rssi_prob
00081
00082
00083
00084 def weight_set( self, measurement, particle_set ):
00085 self.num += 1
00086 if self.num % 10 == 0:
00087 print '\tProcessing sample %d' % self.num
00088
00089 xy = particle_set[:,0:2].T
00090
00091
00092 bins_ind_x = np.sum( xy[0][:,np.newaxis] > self.model['xedges'][:-1], axis = 1 ) - 1
00093 bins_ind_y = np.sum( xy[1][:,np.newaxis] > self.model['yedges'][:-1], axis = 1 ) - 1
00094
00095
00096
00097 ind_x_less = np.where( xy[0] < self.model['xedges'][0] )[0]
00098 ind_x_more = np.where( xy[0] > self.model['xedges'][-1] )[0]
00099 ind_y_less = np.where( xy[1] < self.model['yedges'][0] )[0]
00100 ind_y_more = np.where( xy[1] > self.model['yedges'][-1] )[0]
00101
00102
00103 detect = self.model['detect_model'][bins_ind_y,bins_ind_x]
00104 rssi = self.model['rssi_model'][bins_ind_y,bins_ind_x]
00105 stddev = self.model['stddev_model'][bins_ind_y,bins_ind_x]
00106
00107
00108 uniform_detect = 0.2
00109
00110 detect_prob = detect.filled( 0.0 )
00111 detect_prob += uniform_detect
00112
00113 detect_prob[ ind_x_less ] = uniform_detect
00114 detect_prob[ ind_x_more ] = uniform_detect
00115 detect_prob[ ind_y_less ] = uniform_detect
00116 detect_prob[ ind_y_more ] = uniform_detect
00117
00118 detect_prob = np.clip( detect_prob, 0.0, 1.0 )
00119
00120
00121
00122 uniform_rssi = 0.2
00123
00124 rssi_prob = 1.0 / np.sqrt(2.0 * np.pi * np.power(stddev,2.0))
00125 rssi_prob *= np.exp( -1.0 * np.power( measurement - rssi, 2.0 ) / (2.0*np.power(stddev,2.0)))
00126
00127 rssi_prob = rssi_prob.filled( 0.0 )
00128 rssi_prob += uniform_rssi
00129
00130 rssi_prob[ ind_x_less ] = uniform_rssi
00131 rssi_prob[ ind_x_more ] = uniform_rssi
00132 rssi_prob[ ind_y_less ] = uniform_rssi
00133 rssi_prob[ ind_y_more ] = uniform_rssi
00134
00135 rssi_prob = np.clip( rssi_prob, 0.0, 1.0 )
00136
00137
00138
00139
00140 weight = detect_prob * rssi_prob
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150 particle_set[:,2] *= weight
00151
00152
00153
00154
00155 particle_set[:,2] /= np.sum( particle_set[:,2] )
00156
00157 return np.copy( particle_set )
00158
00159 yaml_fname = '/home/travis/svn/robot1/src/projects/rfid_datacapture/src/rfid_datacapture/rad_pattern_cap/rad_plot_shoulder_table_both_SpectrMedBot.yaml'
00160
00161
00162
00163
00164 class NoMotion:
00165 def __init__( self ):
00166 print 'Starting NoMotion.'
00167
00168 def predict_set( self, control, p_set ):
00169 return p_set
00170
00171
00172
00173 if __name__ == '__main__':
00174 import time
00175
00176 X,Y = np.meshgrid( np.arange(-10,10,0.1), np.arange(-10,10,0.1))
00177 xyw = np.row_stack([ X.flatten(), Y.flatten(), np.ones( X.shape ).flatten() ]).T
00178 def test1( rm, measurement, particles ):
00179 t0 = time.time()
00180
00181 w = np.array([ rm.weight( measurement, p[0:2] ) for p in particles ])
00182 rv = np.column_stack([ particles[:,0:2], w ])
00183
00184 td = time.time() - t0
00185 return rv, td
00186
00187 def test2( rm, measurement, particles ):
00188 t0 = time.time()
00189
00190 rv = rm.weight_set( measurement, particles )
00191
00192 td = time.time() - t0
00193 return rv, td
00194
00195 rm = RfidModel( yaml_fname )
00196
00197 print 'Starting 1'
00198 r1,t1 = test1( rm, 80, xyw )
00199 print 'Done 1 in %2.2f sec.' % t1
00200
00201 print 'Starting 2'
00202 r2,t2 = test2( rm, 80, xyw )
00203 print 'Done 2 in %2.2f sec.' % t2
00204
00205 print 'Speedup: %3.2fx' % (t1 / t2)