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])