def obsModel(s):
if numObservations < 10:
return dist.mixture(dist.deltaDist(ideal[s]),
dist.uniformDist(range(numObservations)),
float(numObservations) / 15.)
return dist.mixture(
dist.triangleDist(ideal[s],
int(numObservations / 6.0) + 2),
dist.uniformDist(range(numObservations)), 0.8)
def setup():
global ideal
ideal = idealReadings.computeIdealReadings(WORLD_FILE, xMin, xMax, robotY, numStates, numObservations)
if not (hasattr(robot,'g') and robot.g.winfo_exists()):
robot.g = graph.Grapher(ideal)
robot.nS = numStates
if robot.nS != numStates:
robot.g.destroy()
robot.g = graph.Grapher(ideal)
robot.nS = numStates
robot.estimator = markov.StateEstimator(dist.uniformDist(range(numStates)), transModel, obsModel)
robot.g.updateObsGraph([0 for s in xrange(numStates)])
robot.g.updateBeliefGraph([robot.estimator.belief.prob(s) for s in xrange(numStates)])
def setup():
global ideal
ideal = idealReadings.computeIdealReadings(WORLD_FILE, xMin, xMax, robotY,
numStates, numObservations)
if not (hasattr(robot, 'g') and robot.g.winfo_exists()):
robot.g = graph.Grapher(ideal)
robot.nS = numStates
if robot.nS != numStates:
robot.g.destroy()
robot.g = graph.Grapher(ideal)
robot.nS = numStates
robot.estimator = markov.StateEstimator(dist.uniformDist(range(numStates)),
transModel, obsModel)
robot.g.updateObsGraph([0 for s in xrange(numStates)])
robot.g.updateBeliefGraph(
[robot.estimator.belief.prob(s) for s in xrange(numStates)])
def setup():
global ideal, confident, parkingSpot, targetTheta, targetX
ideal = idealReadings.computeIdealReadings(WORLD_FILE, xMin, xMax, robotY,
numStates, numObservations)
parkingSpot = getParkingSpot(ideal)
targetX = None
targetTheta = math.pi / 2
confident = False
if not (hasattr(robot, 'g') and robot.g.winfo_exists()):
robot.g = graph.Grapher(ideal)
robot.nS = numStates
if robot.nS != numStates:
robot.g.destroy()
robot.g = graph.Grapher(ideal)
robot.nS = numStates
robot.estimator = markov.StateEstimator(dist.uniformDist(range(numStates)),
transModel, obsModel)
robot.g.updateObsGraph([0 for s in xrange(numStates)])
robot.g.updateBeliefGraph(
[robot.estimator.belief.prob(s) for s in xrange(numStates)])
robot.distances = []
def teleportModel(state):
right_d = dist.deltaDist(min(state+1, numStates-1))
random_d = dist.uniformDist(range(numStates))
return dist.mixture(right_d, random_d, 0.7)
def obsModelA(state):
zero_dist = dist.uniformDist([0])
return dist.mixture(dist.deltaDist(ideal[state]), zero_dist, 0.7)
return dist.mixture(right_d, random_d, 0.7)
def resetModel(state):
right_d = dist.deltaDist(min(state+1, numStates-1))
zero_d = dist.deltaDist(0)
return dist.mixture(right_d, zero_d, 0.7)
def teleportModel2(state):
current_d = dist.deltaDist(state)
random_d = dist.uniformDist(range(numStates))
return dist.mixture(current_d, random_d, 0.7)
def resetModel2(state):
current_d = dist.deltaDist(state)
zero_d = dist.deltaDist(0)
return dist.mixture(current_d, zero_d, 0.7)
## STARTING DISTRIBUTIONS
uniformPrior = dist.uniformDist(range(len(ideal)))
alwaysLeftPrior = dist.DDist({0:1.0})
## SIMULATION CODE
def simulate(transDist, obsDist):
testSE = StateEstimator(uniformPrior, transDist, obsDist)
testHMM = HMM(alwaysLeftPrior, transDist, obsDist)
Simulator(testHMM, testSE, ideal).simulate()
simulate(moveRightModel, obsModelB)
def teleportModel(state):
nominal = moveRightModel(state)
return dist.mixture(dist.uniformDist(range(numStates)), nominal, 0.3)
inp.odometry.theta = util.fixAnglePlusMinusPi(inp.odometry.theta)
if inp.odometry.x>targetX+.05 and abs(inp.odometry.theta)<math.pi/4:
io.Action(fvel=-0.2,rvel=0).execute() #drive backwards if necessary
elif inp.odometry.x<targetX and abs(inp.odometry.theta)<math.pi/4:
io.Action(fvel=0.2,rvel=0).execute() #drive to desired x location
elif inp.odometry.theta<targetTheta-.05:
io.Action(fvel=0,rvel=0.2).execute() #rotate
elif inp.sonars[3]>.3:
io.Action(fvel=0.2,rvel=0).execute() #drive into spot
else:
io.Action(fvel=0,rvel=0).execute() #congratulate yourself (or call insurance company)
def brainStop():
pass
def shutdown():
pass
if __name__ == '__main__':
prior = dist.uniformDist(range(200))
se = StateEstimator(prior,transModel,obsModel)
se.reset()
for i in xrange(len(C)):
ix,s,obs = C[i] #timestep, state, observation
se.update(obs)
loc,done = confidentLocation(se.belief)
if done:
break
estimatedState = loc
realState = s
def teleportModel2(state):
current_d = dist.deltaDist(state)
random_d = dist.uniformDist(range(numStates))
return dist.mixture(current_d, random_d, 0.7)
def teleportModel(state):
right_d = dist.deltaDist(min(state + 1, numStates - 1))
random_d = dist.uniformDist(range(numStates))
return dist.mixture(right_d, random_d, 0.7)
def obsModelA(state):
zero_dist = dist.uniformDist([0])
return dist.mixture(dist.deltaDist(ideal[state]), zero_dist, 0.7)
return dist.mixture(right_d, zero_d, 0.7)
def teleportModel2(state):
current_d = dist.deltaDist(state)
random_d = dist.uniformDist(range(numStates))
return dist.mixture(current_d, random_d, 0.7)
def resetModel2(state):
current_d = dist.deltaDist(state)
zero_d = dist.deltaDist(0)
return dist.mixture(current_d, zero_d, 0.7)
## STARTING DISTRIBUTIONS
uniformPrior = dist.uniformDist(range(len(ideal)))
alwaysLeftPrior = dist.DDist({0: 1.0})
## SIMULATION CODE
def simulate(transDist, obsDist):
testSE = StateEstimator(uniformPrior, transDist, obsDist)
testHMM = HMM(alwaysLeftPrior, transDist, obsDist)
Simulator(testHMM, testSE, ideal).simulate()
simulate(moveRightModel, obsModelB)
def uniformObsModel(s):
return dist.uniformDist(range(numObservations))
def teleportModel2(state):
nominal = dist.deltaDist(state)
return dist.mixture(dist.uniformDist(range(numStates)), nominal, 0.3)
def teleportModel2(state):
current_d = dist.deltaDist(state)
random_d = dist.uniformDist(range(numStates))
return dist.mixture(current_d, random_d, 0.7)
io.Action(fvel=0.2, rvel=0).execute() #drive to desired x location
elif inp.odometry.theta < targetTheta - .05:
io.Action(fvel=0, rvel=0.2).execute() #rotate
elif inp.sonars[3] > .3:
io.Action(fvel=0.2, rvel=0).execute() #drive into spot
else:
io.Action(fvel=0, rvel=0).execute(
) #congratulate yourself (or call insurance company)
def brainStop():
pass
def shutdown():
pass
if __name__ == '__main__':
prior = dist.uniformDist(range(200))
se = StateEstimator(prior, transModel, obsModel)
se.reset()
for i in xrange(len(C)):
ix, s, obs = C[i] #timestep, state, observation
se.update(obs)
loc, done = confidentLocation(se.belief)
if done:
break
estimatedState = loc
realState = s
def obsModelB(state):
return dist.mixture(dist.deltaDist(ideal[state]),dist.uniformDist(range(10)),0.7)
def teleportModel(state):
nominal = moveRightModel(state)
return dist.mixture(dist.uniformDist(range(numStates)), nominal,0.3)
def teleportModel2(state):
nominal = dist.deltaDist(state)
return dist.mixture(dist.uniformDist(range(numStates)), nominal,0.3)
def obsModel(s):
if numObservations < 10:
return dist.mixture(dist.deltaDist(ideal[s]), dist.uniformDist(range(numObservations)), float(numObservations) / 15.)
return dist.mixture(dist.triangleDist(ideal[s], int(numObservations / 6.0) + 2), dist.uniformDist(range(numObservations)), 0.8)
def obsModelB(state):
return dist.mixture(dist.deltaDist(ideal[state]),
dist.uniformDist(range(10)), 0.7)
