def hObs(s, obj, heading, err):
print 'called'
initn, inite = s.objects[obj].centroid
subn, sube = s.objects['sub'].centroid
prevd = toPolar(initn - subn, inite - sube)[0]
newn, newe = toCartesian(prevd, heading)
deltan, deltae = newn - initn, newe - inite
expHeading = toPolar(initn - subn, inite - sube)[1]
# Input rejection.
#if abs(heading - expHeading) / expHeading > AUTOREJFRAC: return False
# Bayesian update.
print s.objects['sub'].centroid
uf = lambda n, e: gaussian(heading, err)(toPolar(n - s.objects['sub'].centroid[0], e - s.objects['sub'].centroid[1])[0])
print 'adding...', s.objects[obj].events
s.objects[obj].events.append(Event(uf, curUT()))
print s.objects[obj].events
# Reverse Bayesian error estimation.
#huf = lambda he: gaussian(heading - expHeading, err)(he)
#s.herr.events.append(Event(huf, curUT(), 1))
# Covariance
s.doCovariance(s.objects[obj], deltan, deltae, prevd * err * pi/180) # Approximate conversion of heading error to position error using sin(x) ~ x in radians.
def dObs(s, obj, distance, err):
initn, inite = s.objects[obj].centroid
subn, sube = s.objects['sub'].centroid
prevh = toPolar(initn - subn, inite - sube)[1]
newn, newe = toCartesian(distance, prevh)
deltan, deltae = newn - initn, newe - inite
expDistance = toPolar(initn - subn, inite - sube)[0]
# Input rejection.
if abs(distance - expDistance) > AUTOREJFRAC: return False
# Bayesian update.
uf = lambda n, e: gaussian(distance, err)(toPolar(n - s.objects['sub'].centroid[0], e - s.objects['sub'].centroid[0])[0])
s.objects[obj].events.append(Event(uf, curUT(), 2))
# Reverse Bayesian error estimation.
duf = lambda de: gaussian(distance / expDistance, err / expDistance)(de)
s.derr.events.append(Event(duf, curUT(), 1))
# Covariance
s.doCovariance(s.objects[obj], deltan, deltae, err) # Distance error is close enough to position error.
def move(s, dN, dE, err):
tpmap = n.ones((GRIDSIZE, GRIDSIZE))
ncentroid = (s.objects['sub'].centroid[0] + dN, s.objects['sub'].centroid[1] + dE)
for r in range(GRIDSIZE):
for c in range(GRIDSIZE):
ar = int(round(r - dN / GRIDSCALE))
ac = int(round(c - dE / GRIDSCALE))
try: tpmap[r][c] = s.objects['sub'].pmap[ar][ac]
except: tpmap[r][c] = gaussian(0, s.objects['sub'].error)(toPolar(ncentroid[0] - r*GRIDSCALE, ncentroid[1] - c*GRIDSCALE)[0])
tpmap /= tpmap.sum()
s.objects['sub'].pmap = tpmap
s.objects['sub'].rcCentroid()
def getDistance(obj):
return toPolar(obj.centroid[0] - objects['sub'].centroid[0], obj.centroid[1] - objects['sub'].centroid[1])[0]
def getHeading(obj):
return toPolar(obj.centroid[0] - objects['sub'].centroid[0], obj.centroid[1] - objects['sub'].centroid[1])[1]