def predict(data,visualize):
if visualize:
window = turtle.Screen()
window.bgcolor('white')
#Boundaries - Gray
box = turtle.Turtle()
box.color('#AAAAAA')
box.penup()
box.goto(240/2-500,300-105/2)
box.pendown()
box.goto(1696/2-500,300-105/2)
box.goto(1696/2-500,300-974/2)
box.goto(240/2-500,300-974/2)
box.goto(240/2-500,300-105/2)
box.penup()
box.hideturtle()
#Center Obstacle - Red
obstacle = turtle.Turtle()
obstacle.color('#AA0000')
obstacle.penup()
obstacle.goto(1000/2-500,300-642/2)
obstacle.pendown()
obstacle.circle(50)
obstacle.penup()
obstacle.hideturtle()
#Measurements - Green
broken_robot = turtle.Turtle()
broken_robot.shape('turtle')
broken_robot.color('green')
broken_robot.resizemode('user')
broken_robot.shapesize(0.25, 0.25, 0.25)
broken_robot.penup()
#Predictions - Red
predict_robot = turtle.Turtle()
predict_robot.shape('turtle')
predict_robot.color('red')
predict_robot.resizemode('user')
predict_robot.shapesize(0.25, 0.25, 0.25)
predict_robot.penup()
OTHER = None
for element in data:
x,y = element
if visualize:
broken_robot.goto(int(x)/2-500, 300-int(y)/2) # flip the y and offset the x
broken_robot.pendown() # or .stamp()
global X
global P
if X is None:
#Initial state {x,y,angle,turningAngle,distance,rotationalVelocity,xAcceleration,yAcceleration}
X = matrix([[x],
[y],
[1.],
[1.],
[1.],
[0.],
[0.],
[0.]])
#Initial Uncertainty
P = matrix([[1000.,0.,0.,0.,0.,0.,0.,0.],
[0.,1000.,0.,0.,0.,0.,0.,0.],
[0.,0.,1000.,0.,0.,0.,0.,0.],
[0.,0.,0.,1000.,0.,0.,0.,0.],
[0.,0.,0.,0.,1000.,0.,0.,0.],
[0.,0.,0.,0.,0.,1000.,0.,0.],
[0.,0.,0.,0.,0.,0.,1000.,0.],
[0.,0.,0.,0.,0.,0.,0.,1000.]])
OTHER = {'lastMeasurement':(x,y),'angle':0,'lastAngle':0,'lastTurningAngle':0,'turningAngle':0,'rotationalVelocity':0,'xAcceleration':0,'yAcceleration':0}
else:
OTHER['distance'] = distance_between((x,y),OTHER['lastMeasurement'])
OTHER['xAcceleration'] = x - OTHER['lastMeasurement'][0]
OTHER['yAcceleration'] = y - OTHER['lastMeasurement'][1]
angle = atan2(y-OTHER['lastMeasurement'][1],x-OTHER['lastMeasurement'][0])
angle = angle_trunc(angle)
OTHER['angle'] = angle
OTHER['turningAngle'] = OTHER['angle'] - OTHER['lastAngle']
angle,didCollide = collision_update(x, y, angle)
if didCollide:
OTHER['rotationalVelocity'] = 0 #Reset our rotationalVelocity on a collision
#OTHER['turningAngle'] = 0
#OTHER['lastTurningAngle'] = 0
else:
OTHER['rotationalVelocity'] = OTHER['turningAngle'] - OTHER['lastTurningAngle']
X,P = kalman_filter((x,y),OTHER,X,P)
OTHER['lastMeasurement'] = (x,y)
OTHER['lastAngle'] = OTHER['angle']
OTHER['lastTurningAngle'] = OTHER['turningAngle']
if visualize:
predict_robot.goto(int(X.value[0][0])/2-500, 300-int(X.value[1][0])/2) # flip the y and offset the x
predict_robot.pendown() # or .stamp()
predictions = []
#Two Second Predictions - Blue
if visualize:
unknown_robot = turtle.Turtle()
unknown_robot.shape('turtle')
unknown_robot.color('blue')
unknown_robot.resizemode('user')
unknown_robot.shapesize(0.25, 0.25, 0.25)
unknown_robot.penup()
for i in range(60):
x = int(X.value[0][0])
y = int(X.value[1][0])
predictions.append((x,y))
OTHER['distance'] = distance_between((x,y),OTHER['lastMeasurement'])
OTHER['xAcceleration'] = x - OTHER['lastMeasurement'][0]
OTHER['yAcceleration'] = y - OTHER['lastMeasurement'][1]
angle = atan2(y-OTHER['lastMeasurement'][1],x-OTHER['lastMeasurement'][0])
angle = angle_trunc(angle)
OTHER['angle'] = angle
OTHER['turningAngle'] = OTHER['angle'] - OTHER['lastAngle']
angle,didCollide = collision_update(x, y, angle)
if didCollide:
OTHER['rotationalVelocity'] = 0 #Reset our rotationalVelocity on a collision
#OTHER['turningAngle'] = 0
#OTHER['lastTurningAngle'] = 0
else:
OTHER['rotationalVelocity'] = OTHER['turningAngle'] - OTHER['lastTurningAngle']
X,P = kalman_filter((x,y),OTHER,X,P)
OTHER['lastMeasurement'] = (x,y)
OTHER['lastAngle'] = OTHER['angle']
OTHER['lastTurningAngle'] = OTHER['turningAngle']
X,P = kalman_filter((x,y),OTHER,X,P)
if visualize:
unknown_robot.goto(int(X.value[0][0])/2-500, 300-int(X.value[1][0])/2) # flip the y and offset the x
unknown_robot.pendown() # or .stamp()
time.sleep(30)
return predictions
def predict(data,visualize):
if visualize:
window = turtle.Screen()
window.bgcolor('white')
#Boundaries - Gray
box = turtle.Turtle()
box.color('#AAAAAA')
box.penup()
box.goto(240/2-500,300-105/2)
box.pendown()
box.goto(1696/2-500,300-105/2)
box.goto(1696/2-500,300-974/2)
box.goto(240/2-500,300-974/2)
box.goto(240/2-500,300-105/2)
box.penup()
box.hideturtle()
#Center Obstacle - Red
obstacle = turtle.Turtle()
obstacle.color('#AA0000')
obstacle.penup()
obstacle.goto(1000/2-500,300-642/2)
obstacle.pendown()
obstacle.circle(50)
obstacle.penup()
obstacle.hideturtle()
#Measurements - Green
broken_robot = turtle.Turtle()
broken_robot.shape('turtle')
broken_robot.color('green')
broken_robot.resizemode('user')
broken_robot.shapesize(0.25, 0.25, 0.25)
broken_robot.penup()
#Predictions - Red
predict_robot = turtle.Turtle()
predict_robot.shape('turtle')
predict_robot.color('red')
predict_robot.resizemode('user')
predict_robot.shapesize(0.25, 0.25, 0.25)
predict_robot.penup()
OTHER = None
for element in data:
x,y = element
if visualize:
broken_robot.goto(int(x)/2-500, 300-int(y)/2) # flip the y and offset the x
broken_robot.pendown() # or .stamp()
global X
global P
global historicalDeque
global historicalDequeSize
global historicalMinimum
if X is None:
#Initial state {x,y,xVelocity,yVelocity}
X = matrix([[x],
[y],
[1.],
[1.],
[1.]])
#Initial Uncertainty
P = matrix([[1000.,0.,0.,0.,0.],
[0.,1000.,0.,0.,0.],
[0.,0.,1000.,0.,0.],
[0.,0.,0.,1000.,0.],
[0.,0.,0.,0.,1000.]])
OTHER = {'lastMeasurement':(x,y)}
else:
#Calculates the difference between prediction and measurement
errorPercentX = float((x-X.value[0][0])/x)
errorPercentY = float((y-X.value[1][0])/y)
errorPercentage = (abs(errorPercentX) + abs(errorPercentY))*100
angle = atan2(y-OTHER['lastMeasurement'][1],x-OTHER['lastMeasurement'][0])
angle = angle_trunc(angle)
x,y,angle,didCollide = collision_update(x, y, angle)
OTHER['lastAngle'] = angle
OTHER['turningAngle'] = angle - OTHER['lastAngle']
#OTHER['lastAngle'] = angle
OTHER['distance'] = distance_between((x,y),OTHER['lastMeasurement'])
X,P = kalman_filter((x,y),OTHER,X,P)
OTHER['lastMeasurement'] = (x,y)
if didCollide:#Reset historical
historicalDeque = deque(maxlen=historicalDequeSize)
else:
normalizedVector = (OTHER['distance']*cos(OTHER['lastAngle'] + OTHER['turningAngle']),OTHER['distance']*sin(OTHER['lastAngle'] + OTHER['turningAngle']))
#normalizedVector = (cos(OTHER['lastAngle'] + OTHER['turningAngle']),sin(OTHER['lastAngle'] + OTHER['turningAngle']))
if len(historicalDeque) == historicalDequeSize:
historicalDeque.popleft()
historicalDeque.append(normalizedVector)
if visualize:
predict_robot.goto(int(X.value[0][0])/2-500, 300-int(X.value[1][0])/2) # flip the y and offset the x
predict_robot.pendown() # or .stamp()
predictions = []
#Two Second Predictions - Blue
#print historicalDeque
if visualize:
unknown_robot = turtle.Turtle()
unknown_robot.shape('turtle')
unknown_robot.color('blue')
unknown_robot.resizemode('user')
unknown_robot.shapesize(0.25, 0.25, 0.25)
unknown_robot.penup()
#print historicalDeque
for i in range(60):
#print 'deque: ',historicalDeque
x = int(X.value[0][0])
y = int(X.value[1][0])
predictions.append((x,y))
if len(historicalDeque) > historicalMinimum:#If we have history, use it for our prediction
xVector = 0
yVector = 0
for vector in historicalDeque:
xVector += vector[0]
yVector += vector[1]
xVector = xVector/len(historicalDeque)
yVector = yVector/len(historicalDeque)
angle = atan2(yVector,xVector)
#print 'Historical Vector Angle: ', angle*180/pi
else:
angle = atan2(y-OTHER['lastMeasurement'][1],x-OTHER['lastMeasurement'][0])
angle = angle_trunc(angle)
x,y,angle,didCollide = collision_update(x, y, angle)
OTHER['lastAngle'] = angle
OTHER['turningAngle'] = angle - OTHER['lastAngle']
#OTHER['distance'] = distance_between((x,y),OTHER['lastMeasurement'])
vectorDistance = distance_between((x,y),(x+xVector,y+yVector))
OTHER['distance'] = vectorDistance
X,P = kalman_filter((x,y),OTHER,X,P)
OTHER['lastMeasurement'] = (x,y)
if didCollide:#Reset historical
historicalDeque = deque(maxlen=historicalDequeSize)
else:
normalizedVector = (OTHER['distance']*cos(OTHER['lastAngle'] + OTHER['turningAngle']),OTHER['distance']*sin(OTHER['lastAngle'] + OTHER['turningAngle']))
#normalizedVector = (cos(OTHER['lastAngle'] + OTHER['turningAngle']),sin(OTHER['lastAngle'] + OTHER['turningAngle']))
if len(historicalDeque) == historicalDequeSize:
historicalDeque.popleft()
historicalDeque.append(normalizedVector)
if visualize:
unknown_robot.goto(int(X.value[0][0])/2-500, 300-int(X.value[1][0])/2) # flip the y and offset the x
unknown_robot.pendown()
#time.sleep(30)
return predictions
