def process_position(self, odom):
odom.header.frame_id = 'map'
self.original_odom.publish(odom)
# print('add noise')
# noise = np.random.normal(0,1)
## noise params ##
# pose.pose.position.[x,y] 2
# pose.pose.orientation.[x,y,z,w] (heading) 3
# twist.twist.linear.[x] 4
# twist.twist.angular.[z] 5
x = odom.pose.pose.position.x
y = odom.pose.pose.position.y
v = abs(odom.twist.twist.linear.x)
w = abs(odom.twist.twist.angular.z)
odom.pose.pose.position.x += noise(0,self.position_variation) # 1
odom.pose.pose.position.y += noise(0,self.position_variation) # 2
if odom.pose.pose.position.z: # if it is not 0
odom.pose.pose.position.z += noise(0,self.position_variation)
heading = quaternion_to_heading(odom.pose.pose.orientation)
heading += noise(0,self.heading_variation+.05*w+.01*v)
odom.pose.pose.orientation = heading_to_quaternion(heading) # 3
odom.twist.twist.linear.x += noise(0,self.linear_vel+.05*v+.01*w) # 4
if odom.twist.twist.linear.y: # if it is not 0
odom.twist.twist.linear.y += noise(0,self.linear_vel)
if odom.twist.twist.linear.z: # if it is not 0
odom.twist.twist.linear.z += noise(0,self.linear_vel)
odom.twist.twist.angular.z += noise(0,self.angular_vel+.05*w+.01*v) # 5
if odom.twist.twist.angular.x: # if it is not 0
odom.twist.twist.angular.x += noise(0,self.angular_vel)
if odom.twist.twist.angular.y: # if it is not 0
odom.twist.twist.angular.y += noise(0,self.angular_vel)
self.noisy_odom.publish(odom)
def setUp(self):
self.fd = np.asarray(load_image(funcfile))
self.fi = FmriImageList.from_image(load_image(funcfile))
# I think it makes more sense to use fd instead of fi for GLM
# purposes -- reduces some noticeable overhead in creating the
# array from FmriImageList
# create a design matrix, model and contrast matrix
self.design = noise((self.fd.shape[0],3))
self.model = ols_model(self.design)
self.cmatrix = np.array([[1,0,0],[0,1,0]])
def animate(i):
global x,y,vx,vy,dr,j
vx_temp = []
vy_temp = []
dr = .5
for j in range(n):
con = ((x<x[j]+dr) & (x>x[j]-dr) & (y<y[j]+dr) & (y>y[j]-dr))#it indicates if there is someone near or not
if sum(con)!=0:
vx_temp.append(sum(con*vx)/sum(con)+(noise()-.5)*3)
vy_temp.append(sum(con*vy)/sum(con)+(noise()-.5)*3)
else:
vx_temp.append(vx)
vy_temp.append(vy)
vx = np.array(vx_temp.copy())
vy = np.array(vy_temp.copy())
x = (x+vx*dt)%w
y = (y+vy*dt)%w
line.set_xdata(x)
line.set_ydata(y)
return line,
warnings.resetwarnings()
# Module globals
FIMG = load_image(funcfile)
# Put time on first axis
FIMG = img_rollaxis(FIMG, "t")
FDATA = FIMG.get_data()
FIL = FmriImageList.from_image(FIMG)
# I think it makes more sense to use FDATA instead of FIL for GLM
# purposes -- reduces some noticeable overhead in creating the
# array from FmriImageList
# create a design matrix, model and contrast matrix
DESIGN = noise((FDATA.shape[0], 3))
MODEL = OLSModel(DESIGN)
CMATRIX = np.array([[1, 0, 0], [0, 1, 0]])
# two prototypical functions in a GLM analysis
def fit(input):
return MODEL.fit(input).resid
def contrast(results):
return results.Fcontrast(CMATRIX)
# generators
def result_generator(datag):
for i, fdata in datag:
def sinus_model(X, variance):
'''Sinus function plus noise'''
return np.sin(2 * np.pi * X) + noise(0,np.sqrt(variance),X.shape)
warnings.resetwarnings()
# Module globals
FIMG = load_image(funcfile)
# Put time on first axis
FIMG = rollimg(FIMG, 't')
FDATA = FIMG.get_data()
FIL = FmriImageList.from_image(FIMG)
# I think it makes more sense to use FDATA instead of FIL for GLM
# purposes -- reduces some noticeable overhead in creating the
# array from FmriImageList
# create a design matrix, model and contrast matrix
DESIGN = noise((FDATA.shape[0], 3))
MODEL = OLSModel(DESIGN)
CMATRIX = np.array([[1, 0, 0], [0, 1, 0]])
# two prototypical functions in a GLM analysis
def fit(input):
return MODEL.fit(input).resid
def contrast(results):
return results.Fcontrast(CMATRIX)
# generators
def result_generator(datag):
def linear_model(X, variance, w0=-0.3, w1=0.5):
'''Linear function plus noise'''
return w0 + w1 * X + noise(0, np.sqrt(variance), X.shape)