def iterate(self, measurement, control ):
if measurement:
self.lost = False
velocity = control
# known are speeds, convert to absolute movements
nao_movement_x = velocity[0]
nao_movement_y = velocity[1]
nao_movement_t = velocity[2]
# step forward using control vector u
self.u[0][0] = nao_movement_x
self.u[1][0] = nao_movement_y
muBelief = self.mu
# ________ PREDICTION ________
# rotate using nao_movements theta
t = nao_movement_t
rotationmatrix = [[ math.cos( t ), -math.sin(t) ],[ math.sin(t), math.cos(t) ]]
# Predict new measurement based on nao movement.
# Nao moves x,y towards the ball
muBelief = matrix.subtract( muBelief , self.u )
#print muBelief
# Nao rotates theta
muBelief = matrix.mult( rotationmatrix, muBelief )
# add noise to motion
muBelief = sample( muBelief, self.Sigma, 2)
# covariance matrix update
SigmaBelief = matrix.plus( self.Sigma , self.R)
# ________ CORRECTION _________
if measurement:
self.z[0][0] = measurement[0]
self.z[1][0] = measurement[1]
# Since C = [1,0;0,1], drop it
s = matrix.inverse2D( matrix.plus( SigmaBelief, self.Q) )
K = matrix.mult(SigmaBelief, s )
self.mu = matrix.plus( muBelief, matrix.mult(K, matrix.subtract(self.z , muBelief)) )
self.Sigma = matrix.mult(matrix.subtract( self.I, K ), SigmaBelief)
else:
# if no ball is found, use the predicted state!
self.mu = muBelief
self.Sigma = SigmaBelief
self.ballPos = self.mu[0][0], self.mu[1][0]
def iterate(self, measurement, control):
now = time.time()
# timestamps matter. IMPORTANT: Do not use if first iteration has not been set.
if self.firstCall:
self.firstCall = False
timeTaken = time.time() - self.timeStamp
# major screwup if this happens
if timeTaken > 1.0:
print 'Interval was way too long: ', timeTaken, 'seconds.'
timeTaken = 0.0
self.timeStamp = time.time()
#velocity = motProxy.getRobotVelocity()
velocity = control
# known are speeds, convert to absolute movements
nao_movement_x = velocity[0] * timeTaken
nao_movement_y = velocity[1] * timeTaken
nao_movement_t = velocity[2] * timeTaken
#print timeTaken
# step forward using control vector u
self.u[0][0] = nao_movement_x
self.u[1][0] = nao_movement_y
print 'Increment control ', self.u
muBelief = self.mu
# ________ PREDICTION ________
# rotate using nao_movements theta
t = nao_movement_t
rotationmatrix = [[ math.cos( t ), -math.sin(t) ],[ math.sin(t), math.cos(t) ]]
# Predict new measurement based on nao movement.
# Nao moves x,y towards the ball
muBelief = matrix.subtract( muBelief , self.u )
#print muBelief
# Nao rotates theta
muBelief = matrix.mult( rotationmatrix, muBelief )
# add noise to motion
muBelief = sample( muBelief, self.Sigma, 2)
# covariance matrix update
SigmaBelief = matrix.plus( self.Sigma , self.R)
# ________ CORRECTION _________
if measurement:
self.z[0][0] = measurement[0]
self.z[1][0] = measurement[1]
# Since C = [1,0;0,1], drop it
s = matrix.inverse2D( matrix.plus( SigmaBelief, self.Q) )
K = matrix.mult(SigmaBelief, s )
self.mu = matrix.plus( muBelief, matrix.mult(K, matrix.subtract(self.z , muBelief)) )
self.Sigma = matrix.mult(matrix.subtract( self.I, K ), SigmaBelief)
else:
# if no ball is found, use the predicted state!
self.mu = muBelief
self.Sigma = SigmaBelief
#print 'Mu:',self.mu
#print 'Sigma: '
#matrix.show(self.Sigma)
#print ''
return (self.mu[0][0], self.mu[1][0])
