def goalie():
goaliePose()
while True:
track.findRedBall()
redballtracker.startTracker()
if counter == 0: #This is for the first time to define redballposition since it will be updated after redballposition2
redballposition = redballtracker.getPosition()
counter == 1
redballposition2 = redballposition #redballposition2 stores the old position of the ball and we get the difference of them
print "redballposition2 :", redballposition2
while count < 100:
redballtracker.setWholeBodyOn(True)
redballposition2 = redballposition
redballposition = redballtracker.getPosition() #Updates the position of the ball
print "redballposition:", redballposition
distance = math.sqrt(math.pow(redballposition[0],2) + math.pow(redballposition[1],2) + math.pow(redballposition[2],2))
print "distance:" , distance
velocity = math.sqrt(math.pow((redballposition[0] - redballposition2[0]), 2))/ .1
print "velocity:" , velocity, "count" , count
if velocity >= .2 and redballposition[1] > .1 and redballposition[0] < 2:
redballtracker.stopTracker()
dive.diveLeftGoal()
goaliePose()
redballtracker.startTracker()
count = 100
elif velocity >= .2 and redballposition[1] < -.1 and redballposition[0] < 2:
redballtracker.stopTracker()
dive.diveRightGoal()
goaliePose()
redballtracker.startTracker()
count = 100
elif velocity == 0: #If the ball has not moved the count goes up by one and if reaches 100 looks for ball again.
count += 1
count = 0
counter = 0
def goalieImproved(option):
redballposition=track.findRedBall()
print redballposition
redballtracker.startTracker()
if option == 1:
walk.walkClockwise()
while redballposition[1]<-.31:
redballposition= redballtracker.getPosition()
print "Walking!", redballposition[1]
redballtracker.stopTracker()
walk.stop()
else:
walk.walkCounterclockwise()
while redballposition[1] > .31:
redballposition= redballtracker.getPosition()
print "counter", redballposition[1]
redballtracker.stopTracker()
walk.stop()
goaliePose()
def run(self):
kalmanstop.clear() #clears the flag that seting the event does.
trackstop.clear()
#initializing the vectors that store the variables.
global xkalman, ykalman, xvelocity, yvelocity #these variables are made global, so we can call on them
#Whenever we want.
xvelocity = [0]
yvelocity = [0]
T = [0] #Time elapsed
xsensed=[1]
ysensed=[0]
xkalman=[1]
ykalman=[1]
#setting the constants
t=0.01; #change in time
r=1;
r1=1;
G = numpy.matrix([[math.pow(t,2)/2],[math.pow(t,2)/2],[t],[t]])
A = numpy.matrix([[1,0,t,0],[0,1,0,t],[0,0,1,0],[0,0,0,1]])
R = numpy.matrix('1,0;0,1')*math.pow(r,2);
H= numpy.matrix('1,0,0,0;0,1,0,0')
Q = G*numpy.transpose(G)*math.pow(r1,2)
Pk_1 = numpy.matrix([[10,0,0,0],[0,10,0,0],[0,0,10,0],[0,0,0,10]]);
xhk_1=numpy.matrix([[1],[0],[1],[2]]);
#setting initial conditions
xb=0.1
yb=0.1
aj=0 #the time in loop
#now we look for and find the ball
redballposition=track.findRedBall(trackstop)
redballtracker.startTracker() #you have to start the tracker after using the find_red_ball function
while(not kalmanstop.is_set()):
redballposition=redballtracker.getPosition();
zk= H*numpy.matrix([[redballposition[0]],[redballposition[1]],[xb],[yb]]) ;
xhbk = A*xhk_1
Pbk = A * Pk_1 * numpy.transpose(A) + Q;
Kk = Pbk*numpy.transpose(H)*numpy.linalg.inv((H * Pbk * numpy.transpose(H)) + R);
xhk = xhbk + Kk*(zk-H*xhbk);
Pk = (numpy.matrix([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]) - Kk*H)*Pbk;
Pk_1=Pk;
xhk_1=xhk;
xb=xhk[2][0] #updates the x velocity
yb=xhk[3][0] #updates the y velocity
#all this appends the new found positons/velocity to their respective vectors.
xvelocity.append(xb)
yvelocity.append(yb)
xsensed.append(zk[0][0])
ysensed.append(zk[1][0])
xkalman.append(xhk[0][0])
ykalman.append(xhk[1][0])
aj=aj+t
T.append(aj)
redballtracker.stopTracker()
kalmanstop.clear()
trackstop.clear()
print 'done'
def map():
config.stiffnessOn()
config.poseInit()
redballposition = track.findRedBall()
mapLocation(localization.getNaoLocation(),(redballposition[0],redballposition[1]))
def kalmanFilter():
#initializing the vectors that store the variables.
xvelocity = [0]
yvelocity = [0]
T = [0] #Time elapsed
xsensed=[1]
ysensed=[0]
xkalman=[1]
ykalman=[1]
#setting the constants
t=0.01; #change in time
r=1;
r1=1;
G = numpy.matrix([[math.pow(t,2)/2],[math.pow(t,2)/2],[t],[t]])
A = numpy.matrix([[1,0,t,0],[0,1,0,t],[0,0,1,0],[0,0,0,1]])
R = numpy.matrix('1,0;0,1')*math.pow(r,2);
H= numpy.matrix('1,0,0,0;0,1,0,0')
Q = G*numpy.transpose(G)*math.pow(r1,2)
Pk_1 = numpy.matrix([[10,0,0,0],[0,10,0,0],[0,0,10,0],[0,0,0,10]]);
xhk_1=numpy.matrix([[1],[0],[1],[2]]);
#setting initial conditions
xb=0.1
yb=0.1
aj=0 #the time in loop
k=1
iterations=500 #the number of iterations you want
#now we look for and find the ball
redballposition=track.findRedBall()
redballtracker.startTracker() #you have to start the tracker after using the find_red_ball function
while k<iterations:
redballposition=redballtracker.getPosition()
zk= H*numpy.matrix([[redballposition[0]],[redballposition[1]],[xb],[yb]]) ;
xhbk = A*xhk_1
Pbk = A * Pk_1 * numpy.transpose(A) + Q;
Kk = Pbk*numpy.transpose(H)*numpy.linalg.inv((H * Pbk * numpy.transpose(H)) + R)
xhk = xhbk + Kk*(zk-H*xhbk);
print xhk[0][0] #prints the kalman estimate of the x position
Pk = (numpy.matrix([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]) - Kk*H)*Pbk;
Pk_1=Pk
xhk_1=xhk
xb=xhk[2][0] #updates the x velocity
yb=xhk[3][0] #updates the y velocity
xvelocity.append(xb)
yvelocity.append(yb)
xsensed.append(zk[0][0])
ysensed.append(zk[1][0])
xkalman.append(xhk[0][0])
ykalman.append(xhk[1][0])
aj=aj+t
T.append(aj)
k=k+1
redballtracker.stopTracker()
texttospeechProxy.say("done")
#plots the sensed value the kalman estimated position and velocity. (in y)
pyplot.plot(T,ysensed,'r',T,ykalman,'g',T,yvelocity,'b--')
pyplot.legend(('Sensed Position', 'Kalman Estimate', 'Velocity'))
pyplot.xlabel('Itterations')
pyplot.ylabel('value')
pyplot.show()
