def main(): measurement = np.zeros((2,1)) # Measurement vector state = np.zeros((2,1)) # Initial state vector [x,y,vx,vy] pose = Pose2D() kalman = Kalman() navdata = navdata_info() rospy.init_node('filtered_tag_data') pub_kalman = rospy.Publisher('/kalman', Pose2D, queue_size=100) rate = rospy.Rate(200) # 200Hz while not rospy.is_shutdown(): # measurement[0, 0] = navdata.norm_tag_x # measurement[1, 0] = navdata.norm_tag_y # measurement[2, 0] = navdata.tag_vx # measurement[3, 0] = navdata.tag_vy measurement[0, 0] = navdata.tag_x measurement[1, 0] = navdata.tag_y kalman.state_callback() kalman.measurement_callback(measurement) state = kalman.x pose.x = state[0, 0] pose.y = state[1, 0] pub_kalman.publish(pose) rate.sleep()
def __init__(self): self.depthSub = rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.depth_callback) self.pose = rospy.Subscriber('/pose', PoseStamped, self.coordinate_callback) self.d1 = None # Distance from robot to landmark 1. self.d2 = None # Distance from robot to landmark 2. self.odom_initial_x = None self.odom_initial_y = None self.linear_velocity = 0 self.angular_velocity = 0 self.pose_x = 0 self.pose_y = 0 self.angular_displacement = 0 self.delta_t = 0 # Change in time between pose messages. self.delta_d = 0 # Change in distance between pose messages. self.delta_angle = 0 self.current_time = rospy.get_time() # Initial State: [x, y, phi, linear velocity] self.starting_state = np.array([[START_X, START_Y, 0, 0]]).reshape(4, 1) # Convert to 4x1 vector. # Initial velocity: [linear velocity, angular velocity] self.control_state = np.array([[0, 0]]).reshape(2, 1) self.filter = Kalman(self.starting_state, self.control_state) rospy.sleep(2)
def doKalman(self, X, Y): self.Kfilter = Kalman(X, Y) Mu, Sigma = self.Kfilter.runKalman(self.bzrc) enemyX = Mu.item((0, 0)) enemyY = Mu.item((3, 0)) distance = Point(self.tank.x, self.tank.y).distance(Point(enemyX, enemyY)) if distance > 450: return pred = (distance / float(self.constants['shotspeed'])) * (1.0 / t) pred = int(pred + 1) MuPred, garbage = self.Kfilter.predictiveKalman(Mu, Sigma, pred) targetX, targetY = MuPred.item((0, 0)), MuPred.item((3, 0)) deltaX, deltaY = self.getDeltaXY(self.tank, Point(targetX, targetY)) theta = math.atan2(deltaY, deltaX) theta = self.normalize_angle(theta - self.tank.angle) #distance = Point(tank.x, tank.y).distance(Point(targetX,targetY)) if distance <= 390 and (theta < 0.2 and theta > -0.2): shoot = True else: shoot = False self.kalmanCommand(deltaX, deltaY, theta, shoot, self.tank)
def doKalman(self,X,Y): self.Kfilter = Kalman(X,Y) Mu,Sigma = self.Kfilter.runKalman(self.bzrc) enemyX = Mu.item((0,0)) enemyY = Mu.item((3,0)) distance = Point(self.tank.x, self.tank.y).distance(Point(enemyX,enemyY)) if distance > 450: return pred = (distance/float(self.constants['shotspeed']))*(1.0/t) pred = int(pred+1) MuPred,garbage = self.Kfilter.predictiveKalman(Mu,Sigma,pred) targetX,targetY = MuPred.item((0,0)),MuPred.item((3,0)) deltaX, deltaY = self.getDeltaXY(self.tank,Point(targetX,targetY)) theta = math.atan2(deltaY,deltaX) theta = self.normalize_angle(theta-self.tank.angle) #distance = Point(tank.x, tank.y).distance(Point(targetX,targetY)) if distance <= 390 and (theta < 0.2 and theta > -0.2): shoot = True else: shoot = False self.kalmanCommand(deltaX, deltaY, theta, shoot, self.tank)
def __init__(self, historyKal, dimKal, historyBayes, distType=None): # First check Kalman related parameters if historyKal < 1 or dimKal < 1: print('Hybrid module reporting: ') raise ValueError( 'Improper value entered for history length and/or dimension of observation matrix (Kalman)...' ) if dimKal < 2: print('Hybrid module reporting: ') print( 'Caution! Low accuracy due to the size of the observation matrix. (Affecting mostly Kalman)' ) # Now check for Bayes related parameters if historyBayes < 1: print('Hybrid module reporting: ') raise ValueError('History length is too small (Bayes)...') # Final check to ensure smooth operations if historyBayes < historyKal: print('Hybrid module reporting: ') raise ValueError( 'History length for Bayesian system must be greater or equal to that of Kalman. ' ) # Check if the distribution is supported if distType is None: correctionType = 'none' # Initialise type of data to be handled/corrected (will be detected) else: if distType.lower() in distAllowed.keys(): correctionType = distAllowed[distType.lower( )] # Initialise type of data to be handled/corrected (user defined) else: print('Hybrid module reporting: ') raise ValueError( 'Distribution not currently implemented for the system.') # Now ready to use the input (could defer to classes but thought better to do it here as well) self.KF = Kalman(historyKal, dimKal) self.BS = Bayes(historyBayes, correctionType)
def __init__(self, unique_name, refresh_rate): self.unique_name = unique_name self.source_frame = "/map" self.target_frame = "/{}/base_footprint".format(self.unique_name) self.state = Kalman(1, 0.2) self.route = [] self.last_route_timestamp = rospy.Time().now() self.route_timeout = rospy.Duration.from_sec(5) # use only every X point in path, this safes alot of computing # power, good values may change with different configs self.route_skip_points = 25 self.refresh_rate = refresh_rate self.tflistener = tf.TransformListener() self.subscriber = rospy.Subscriber( "{}/move_base/DWAPlannerROS/global_plan".format(self.unique_name), Path, self.handle_robot_path, queue_size=1)
def main(): measurement = np.zeros((2, 1)) # Measurement vector state = np.zeros((2, 1)) # Initial state vector [x,y,vx,vy] pose = Pose2D() kalman = Kalman() navdata = navdata_info() rospy.init_node('filtered_tag_data') pub_kalman = rospy.Publisher('/kalman', Pose2D, queue_size=100) rate = rospy.Rate(200) # 200Hz while not rospy.is_shutdown(): # measurement[0, 0] = navdata.norm_tag_x # measurement[1, 0] = navdata.norm_tag_y # measurement[2, 0] = navdata.tag_vx # measurement[3, 0] = navdata.tag_vy measurement[0, 0] = navdata.tag_x measurement[1, 0] = navdata.tag_y kalman.state_callback() kalman.measurement_callback(measurement) state = kalman.x pose.x = state[0, 0] pose.y = state[1, 0] pub_kalman.publish(pose) rate.sleep()
def main(): measurement = np.zeros((2,1)) # Measurement vector state = np.zeros((2,1)) # Initial state vector [x,y,vx,vy] pose = Pose2D() kalman = Kalman() navdata = navdata_info() # conversions x_conversion = 0.64 y_conversion = 0.36 rospy.init_node('filtered_tag_data') pub_kalman = rospy.Publisher('/filtered_tag_pose', Pose2D, queue_size=100) rate = rospy.Rate(200) # 200Hz while not rospy.is_shutdown(): measurement[0, 0] = navdata.tag_x * x_conversion measurement[1, 0] = navdata.tag_y * y_conversion kalman.state_callback() kalman.measurement_callback(measurement) state = kalman.x pose.x = state[0, 0] pose.y = state[1, 0] pub_kalman.publish(pose) rate.sleep()
class Controller: def __init__(self, Kp, Ki, n, A, B, C, Q, R): self.Kp = Kp self.Ki = Ki self.u = 0.0 self.err = 0.0 self.kalman = Kalman(n, A, B, C, Q, R) def compute_next_u(self, y_err): (x_out, y_out) = self.kalman.compute_kalman(self.u, y_err) self.err = (self.err - y_out)[0, 0] self.u = (self.Ki * self.err - self.Kp * x_out)[0, 0] return self.u
def init_kalman(self): deltaT = 1 / self.fps print(deltaT) F = np.matrix([[1, 0, deltaT, 0], [0, 1, 0, deltaT], [0, 0, 1, 0], [0, 0, 0, 1]]) B = np.matrix([[deltaT**2 / 2, 0], [0, deltaT**2 / 2], [deltaT, 0], [0, deltaT]]) H = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) model_noise = 1 measurement_noise = 0.1 R = measurement_noise**2 * np.matlib.identity(4) / deltaT Q = model_noise**2 * np.matlib.identity(4) * deltaT init_mu = np.matrix([[0.], [0.]]) # This is the acceleration estimate init_P = np.matlib.identity(4) init_x = np.matrix([[0], [0], [0], [0]]) new_kalman = Kalman(F, B, Q, H, R, init_x, init_mu, init_P, deltaT) return new_kalman, F, B, Q, H, R, init_mu, init_P, deltaT
class RobotState(object): def __init__(self, unique_name, refresh_rate): self.unique_name = unique_name self.source_frame = "/map" self.target_frame = "/{}/base_footprint".format(self.unique_name) self.state = Kalman(1, 0.2) self.route = [] self.last_route_timestamp = rospy.Time().now() self.route_timeout = rospy.Duration.from_sec(5) # use only every X point in path, this safes alot of computing # power, good values may change with different configs self.route_skip_points = 25 self.refresh_rate = refresh_rate self.tflistener = tf.TransformListener() self.subscriber = rospy.Subscriber( "{}/move_base/DWAPlannerROS/global_plan".format(self.unique_name), Path, self.handle_robot_path, queue_size=1) def handle_robot_path(self, msg): """ receive routes and save them transformed to map_frame """ path = [] i = 0 for p in msg.poses: i += 1 if i % self.route_skip_points == 0: if not (isnan(p.pose.position.x) or isnan(p.pose.position.y) or isnan(p.pose.position.z) or \ isnan(p.pose.orientation.x) or isnan(p.pose.orientation.y) or isnan(p.pose.orientation.z) or isnan(p.pose.orientation.w)): path.append( self.tflistener.transformPose(self.source_frame, p)) else: rospy.logerr( "{}: Error in robot path for robot {}".format( strftime("%d.%m.%Y %H:%M:%S", localtime()), self.unique_name)) return False #path = msg.poses # copy poses untransformed print path self.route = path self.last_route_timestamp = rospy.Time().now() def build_msg(self): # Get the guesed position try: #(trans,rot) = listener.lookupTransform(source_frame, target_frame.format(robot), rospy.Time.now()) (trans, rot) = self.tflistener.lookupTransform(self.source_frame, self.target_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: rospy.logerr("{}: Error own position for robot {}".format( strftime("%d.%m.%Y %H:%M:%S", localtime()), self.unique_name)) rospy.loginfo(e) return False # Feed Kalman filter with raw values raw_x, raw_y = trans[:2] raw_orientation = euler_from_quaternion(rot)[2] if not (isnan(raw_x) or isnan(raw_y) or isnan(raw_orientation)): self.state.update([raw_x, raw_y, raw_orientation], 1.0 / self.refresh_rate) # Get position, orientation and velocitys from Kalman filter x, y, yaw = self.state.position().tolist()[0] vx, vy, vyaw = self.state.velocity().tolist()[0] # Build and publish message msg = virtual_obstacles.msg.moving_object_msg() msg.unique_name = self.unique_name msg.pose.x = x msg.pose.y = y msg.pose.theta = yaw msg.velocity.linear.x = vx msg.velocity.linear.y = vy msg.velocity.angular.z = vyaw if (rospy.Time().now() - self.last_route_timestamp) <= self.route_timeout: msg.route = self.route else: msg.route = [] return msg
class HybridKB(object): def __init__(self, historyKal, dimKal, historyBayes, distType=None): # First check Kalman related parameters if historyKal < 1 or dimKal < 1: print('Hybrid module reporting: ') raise ValueError( 'Improper value entered for history length and/or dimension of observation matrix (Kalman)...' ) if dimKal < 2: print('Hybrid module reporting: ') print( 'Caution! Low accuracy due to the size of the observation matrix. (Affecting mostly Kalman)' ) # Now check for Bayes related parameters if historyBayes < 1: print('Hybrid module reporting: ') raise ValueError('History length is too small (Bayes)...') # Final check to ensure smooth operations if historyBayes < historyKal: print('Hybrid module reporting: ') raise ValueError( 'History length for Bayesian system must be greater or equal to that of Kalman. ' ) # Check if the distribution is supported if distType is None: correctionType = 'none' # Initialise type of data to be handled/corrected (will be detected) else: if distType.lower() in distAllowed.keys(): correctionType = distAllowed[distType.lower( )] # Initialise type of data to be handled/corrected (user defined) else: print('Hybrid module reporting: ') raise ValueError( 'Distribution not currently implemented for the system.') # Now ready to use the input (could defer to classes but thought better to do it here as well) self.KF = Kalman(historyKal, dimKal) self.BS = Bayes(historyBayes, correctionType) def trainMe(self, obs, model, retarget=False): """ Master method to control the initial training of the system. """ # Ensure it's working with numpy arrays myObs = np.array(obs) myModel = np.array(model) # Check if shapes match if myObs.shape != myModel.shape: raise TypeError('Training set does not have conforming shapes.') # Train each module self.KF.trainMe(myObs, myModel) self.BS.trainMe(myObs, myModel, retarget=retarget) def adjustForecast(self, model, buff=20.0): """ Method to control the correction of the forecast value. """ tempVal = self.KF.adjustForecast(model, buff=buff) ret = self.BS.adjustForecast(tempVal, buff=buff) return ret
[280], # x' [3000], # y [120] # y' ]) initial_covar = numpy.array([ [400, 0, 0, 0], # x [0, 25, 0, 0], # x' [0, 0, 0, 0], # y [0, 0, 0, 0] # y' ]) acceleration = numpy.array([[2], [0]]) dt = 1 kfilter = Kalman(initial_state, initial_covar) observation = numpy.array([ [4260], # x [282], # x' [0], # y [0] # y' ]) kfilter.estimate(observation, acceleration, dt) observation = numpy.array([ [4550], # x [285], # x' [0], # y [0] # y' ])
def main(): s = Strapdown() K = Kalman() rot_mean = toVector(0., 0., 0.) acc_mean = toVector(0., 0., 0.) mag_mean = toVector(0., 0., 0.) # read sensors filePath = "data\\adafruit10DOF\\10min_calib_360.csv" d = CSVImporter(filePath, columns=range(0, 10), skip_header=7, hasTime=True) accelArray, rotationArray, magneticArray = convArray2IMU(d.values) # variable sample rate test time = d.values[:, 0] diff = ([x - time[i - 1] for i, x in enumerate(time)][1:]) diff.insert(0, 0.0134981505504) # realtime 3D plot fig = plt.figure() ax = fig.add_subplot(111, projection='3d') plt.ion() start = datetime.now() phi_list = [] theta_list = [] psi_list = [] for i in range(1, int(47999)): #62247 dt = diff[i] # for 10 - 15 minutes if not s.isInitialized: rot_mean = runningAverage(rot_mean, rotationArray[:, i], 1 / i) acc_mean = runningAverage(acc_mean, accelArray[:, i], 1 / i) mag_mean = runningAverage(mag_mean, magneticArray[:, i], 1 / i) if i >= 45500: #100*60*7.5 : s.Initialze(acc_mean, mag_mean) gyroBias = rot_mean #toVector(0.019686476,-0.014544547,0.002910090) print('STRAPDOWN INITIALIZED with %i values\n' % (i, )) print('Initial bearing\n', s.getOrientation() * 180 / pi) print('Initial velocity\n', s.getVelocity()) print('Initial position\n', s.getPosition()) print('Initial gyro Bias\n', gyroBias * 180 / pi) else: if i % 10 == 0: # plot area plt.cla() fig.texts = [] plot3DFrame(s, ax) plt.draw() plt.pause(0.01) phi, theta, psi = rad2deg(s.getOrientation()) phi_list.append(phi) theta_list.append(theta) psi_list.append(psi) acceleration = accelArray[:, i] rotationRate = rotationArray[:, i] - gyroBias magneticField = magneticArray[:, i] s.quaternion.update(rotationRate, dt) s.velocity.update(acceleration, s.quaternion, dt) s.position.update(s.velocity, dt) K.timeUpdate(s.quaternion, dt) # if i%10 == 0: # K.measurementUpdate(acceleration, magneticField, s.quaternion) # # bearingOld = s.getOrientation() # errorQuat = Quaternion(K.bearingError) # s.quaternion *= errorQuat # #s.quaternion.update(K.bearingError/DT) #angle = rate*DT # # print(s.quaternion.values) # bearingNew = s.getOrientation() # # print("Differenz zwischen neuer und alter Lage \n",rad2deg(bearingNew-bearingOld)) # gyroBias = gyroBias+K.gyroBias # K.resetState() print("Differenz zwischen x Bias End-Start: %E" % rad2deg(gyroBias[0] - rot_mean[0])) print("Differenz zwischen y Bias End-Start: %E" % rad2deg(gyroBias[1] - rot_mean[1])) print("Differenz zwischen z Bias End-Start: %E\n" % rad2deg(gyroBias[2] - rot_mean[2])) print("RMS_dphi = %f deg" % rad2deg(sqrt(K.P[0, 0]))) print("RMS_dthe = %f deg" % rad2deg(sqrt(K.P[1, 1]))) print("RMS_dpsi = %f deg" % rad2deg(sqrt(K.P[2, 2]))) print("RMS_bx = %f deg/s" % rad2deg(sqrt(K.P[3, 3]))) print("RMS_by = %f deg/s" % rad2deg(sqrt(K.P[4, 4]))) print("RMS_bz = %f deg/s\n" % rad2deg(sqrt(K.P[5, 5]))) print('final orientation\n', s.getOrientation() * 180 / pi) end = datetime.now() diff = end - start print('Time needed = ', diff.total_seconds()) print("RMS_phi = %f deg, max = %f, min = %f\n" % (std(phi_list), max(phi_list), min(phi_list))) print("RMS_theta = %f deg, max = %f, min = %f\n" % (std(theta_list), max(theta_list), min(theta_list))) print("RMS_psi = %f deg, max = %f, min = %f\n" % (std(psi_list), max(psi_list), min(psi_list))) plt.show()
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Revision $Id$ ## Simple talker demo that listens to std_msgs/Strings published ## to the 'chatter' topic from Kalman import Kalman from std_msgs.msg import Int64 import numpy as np K1 = Kalman(10, 0.05, 0.00001) import rospy from my_pack.msg import NumArray def callback(data): #rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.data) result = K1.Kalman_continue(np.array(data.numArray).tolist()) pub = rospy.Publisher('Distance', Int64, queue_size=5) pub.publish(result[0]) pub1 = rospy.Publisher('rawdata', Int64, queue_size=5) pub1.publish(result[1]) #print "Final Value ",result
class SuperUberAgent(object): def __init__(self, bzrc,tank,job): self.updatecounts = 0 self.tank = tank self.bzrc = bzrc self.fields = fields.Calculations() self.throttle = .15 self.commands = [] self.constants = self.bzrc.get_constants() self.job = job self.base = bzrc.get_bases()[self.getFlagIndex()] c1 = Point(self.base.corner1_x,self.base.corner1_y) c3 = Point(self.base.corner3_x,self.base.corner3_y) line = Line(c1,c3) self.base = line.getMidpoint() flags = bzrc.get_flags() for flag in flags: if flag.color == self.constants['team']: self.flag = flag self.obstacles = [] #the .07125 comes from the 4 Ls world where 7.125% of the world was occupied self.time = time.time() self.locationList = [] self.oldlocation = [] self.startTime = time.time() self.mytanks = self.bzrc.get_mytanks() def angle(self,tank,target): angle = math.atan2((target.y-tank.y),(target.x-tank.x)) return angle def getDeltaXY(self, tank, enemy): theta = self.angle(tank, enemy) distance = Point(tank.x, tank.y).distance(Point(enemy.x,enemy.y)) deltaX = distance*math.cos(theta) deltaY = distance*math.sin(theta) return deltaX, deltaY def doKalman(self,X,Y): self.Kfilter = Kalman(X,Y) Mu,Sigma = self.Kfilter.runKalman(self.bzrc) enemyX = Mu.item((0,0)) enemyY = Mu.item((3,0)) distance = Point(self.tank.x, self.tank.y).distance(Point(enemyX,enemyY)) if distance > 450: return pred = (distance/float(self.constants['shotspeed']))*(1.0/t) pred = int(pred+1) MuPred,garbage = self.Kfilter.predictiveKalman(Mu,Sigma,pred) targetX,targetY = MuPred.item((0,0)),MuPred.item((3,0)) deltaX, deltaY = self.getDeltaXY(self.tank,Point(targetX,targetY)) theta = math.atan2(deltaY,deltaX) theta = self.normalize_angle(theta-self.tank.angle) #distance = Point(tank.x, tank.y).distance(Point(targetX,targetY)) if distance <= 390 and (theta < 0.2 and theta > -0.2): shoot = True else: shoot = False self.kalmanCommand(deltaX, deltaY, theta, shoot, self.tank) def update(self,tank): self.tank = tank def updateObstacles(self,grid): self.updatecounts += 1 if(self.updatecounts > 100): return else: self.obstacles = grid def getFlagIndex(self): flags = self.bzrc.get_flags() i = 0 for flag in flags: if flag.color == self.constants['team']: self.enemyFlag = (i + 1) % len(flags) return i self.enemyFlag = i i += 1 return def flagSafe(self,i): flags = self.bzrc.get_flags() if abs(flags[i].x - self.flag.x) > 10 or abs(flags[i].y-self.flag.y) > 10: self.flag = flags[i] return False else: self.flag = flags[i] return True def tick(self): if (((time.time()) - self.startTime) > 100): self.job = "a" if (self.job == "d"): i = self.getFlagIndex() if(self.flagSafe(i)): delx,dely = self.fields.getTangentialField2(self.tank,self.flag, 100,2.5,"CCW") delx2,dely2 = self.fields.getAttractiveField(self.tank, self.flag, 200, 2.5) delx2 = delx2*.2 dely2 = dely2*.2 delx = delx+delx2 dely = dely + dely2 else: delx,dely = self.fields.getAttractiveField(self.tank, self.flag, 800, 2.5) self.sendCommand(delx, dely, self.tank) if (self.job == "a"): flags = self.bzrc.get_flags() totalX=0 totalY = 0 for obstacle in self.obstacles: deltaX,deltaY = self.fields.getTangentialField2(self.tank,obstacle, 100, 10, "CW") totalX += deltaX totalY += deltaY if(self.tank.flag == "-"): delx,dely = self.fields.getTangentialField2(self.tank,self.flag, 100,2.5,"CCW") totalX += delx totalY += dely delx2,dely2 = self.fields.getAttractiveField(self.tank, flags[self.enemyFlag], 800, 2.5) totalX += delx2 totalY += dely2 else: delx,dely = self.fields.getAttractiveField(self.tank, self.base, 800, 40) totalX += delx totalY += dely # do offense self.sendCommand(totalX, totalY, self.tank) def goToPoint(self,tank,target): totalX = 0 totalY = 0 deltaX,deltaY = self.fields.getAttractiveField(tank, target, 800, 5) totalX += deltaX totalY += deltaY totalX += random.randrange(-300,300) totalY += random.randrange(-300,300) self.sendCommand(totalY, totalX, tank) def normalize_angle(self, angle): """Make any angle be between +/- pi.""" angle -= 2 * math.pi * int (angle / (2 * math.pi)) if angle <= -math.pi: angle += 2 * math.pi elif angle > math.pi: angle -= 2 * math.pi return angle def sendCommand(self,totalX,totalY,tank): othertanks = self.bzrc.get_othertanks() me = Point(tank.x,tank.y) cur = 100000 mins = 100000 enemy = Point(0,0) for other in othertanks: cur = Point(other.x,other.y).distance(me) if (cur < mins): mins = cur enemy = other if mins <= 200: self.doKalman(enemy.x, enemy.y) else: theta = math.atan2(totalY,totalX) theta = self.normalize_angle(theta-tank.angle) speed = math.sqrt(totalY**2+totalX**2) self.commands = [] command = Command(tank.index,.15*speed,.85*theta,False) self.commands.append(command) self.bzrc.do_commands(self.commands) self.commands = [] def kalmanCommand(self,totalX,totalY,theta,shoot,tank): p = Point(tank.x,tank.y) for obstacle in self.obstacles: if(p.distance(obstacle) < 50): deltaX,deltaY = self.fields.getTangentialField2(self.tank,obstacle, 100, 40,"CW") totalX = deltaX totalY = deltaY theta = math.atan2(totalY,totalX) theta = self.normalize_angle(theta-tank.angle) speed = math.sqrt(totalY**2+totalX**2) self.commands = [] command = Command(tank.index,.15*speed,1.15*theta,True) self.commands.append(command) self.bzrc.do_commands(self.commands) self.commands = []
from Kalman import Kalman import smbus import time import math kalmanX = Kalman() kalmanY = Kalman() RestrictPitch= True radToDeg = 57.2957786 kalmanAngleX = 0 kalmanAngleY = 0 #Bazı MPU6050 Registerları ve Registerların Adresleri PWR_MGMT_1 = 0x6B SMPLRT_DIV = 0x19 CONFIG = 0x1A GYRO_CONFIG = 0x1B INT_ENABLE = 0x38 ACCEL_XOUT_H = 0x3B #ACCEL_XOUT_L = 0x3C ACCEL_YOUT_H = 0x3D #ACCEL_YOUT_L = 0x3E ACCEL_ZOUT_H = 0x3F #ACCEL_ZOUT_L = 0x40 GYRO_XOUT_H = 0x43 #GYRO_XOUT_L = 0x44 GYRO_YOUT_H = 0x45 #GYRO_YOUT_L = 0x46 GYRO_ZOUT_H = 0x47 #GYRO_ZOUT_L = 0x48 #Read the gyro and acceleromater values from MPU6050 def MPU_Init(): #write to sample rate register
class SuperUberAgent(object): def __init__(self, bzrc, tank, job): self.updatecounts = 0 self.tank = tank self.bzrc = bzrc self.fields = fields.Calculations() self.throttle = .15 self.commands = [] self.constants = self.bzrc.get_constants() self.job = job self.base = bzrc.get_bases()[self.getFlagIndex()] c1 = Point(self.base.corner1_x, self.base.corner1_y) c3 = Point(self.base.corner3_x, self.base.corner3_y) line = Line(c1, c3) self.base = line.getMidpoint() flags = bzrc.get_flags() for flag in flags: if flag.color == self.constants['team']: self.flag = flag self.obstacles = [] #the .07125 comes from the 4 Ls world where 7.125% of the world was occupied self.time = time.time() self.locationList = [] self.oldlocation = [] self.startTime = time.time() self.mytanks = self.bzrc.get_mytanks() def angle(self, tank, target): angle = math.atan2((target.y - tank.y), (target.x - tank.x)) return angle def getDeltaXY(self, tank, enemy): theta = self.angle(tank, enemy) distance = Point(tank.x, tank.y).distance(Point(enemy.x, enemy.y)) deltaX = distance * math.cos(theta) deltaY = distance * math.sin(theta) return deltaX, deltaY def doKalman(self, X, Y): self.Kfilter = Kalman(X, Y) Mu, Sigma = self.Kfilter.runKalman(self.bzrc) enemyX = Mu.item((0, 0)) enemyY = Mu.item((3, 0)) distance = Point(self.tank.x, self.tank.y).distance(Point(enemyX, enemyY)) if distance > 450: return pred = (distance / float(self.constants['shotspeed'])) * (1.0 / t) pred = int(pred + 1) MuPred, garbage = self.Kfilter.predictiveKalman(Mu, Sigma, pred) targetX, targetY = MuPred.item((0, 0)), MuPred.item((3, 0)) deltaX, deltaY = self.getDeltaXY(self.tank, Point(targetX, targetY)) theta = math.atan2(deltaY, deltaX) theta = self.normalize_angle(theta - self.tank.angle) #distance = Point(tank.x, tank.y).distance(Point(targetX,targetY)) if distance <= 390 and (theta < 0.2 and theta > -0.2): shoot = True else: shoot = False self.kalmanCommand(deltaX, deltaY, theta, shoot, self.tank) def update(self, tank): self.tank = tank def updateObstacles(self, grid): self.updatecounts += 1 if (self.updatecounts > 100): return else: self.obstacles = grid def getFlagIndex(self): flags = self.bzrc.get_flags() i = 0 for flag in flags: if flag.color == self.constants['team']: self.enemyFlag = (i + 1) % len(flags) return i self.enemyFlag = i i += 1 return def flagSafe(self, i): flags = self.bzrc.get_flags() if abs(flags[i].x - self.flag.x) > 10 or abs(flags[i].y - self.flag.y) > 10: self.flag = flags[i] return False else: self.flag = flags[i] return True def tick(self): if (((time.time()) - self.startTime) > 100): self.job = "a" if (self.job == "d"): i = self.getFlagIndex() if (self.flagSafe(i)): delx, dely = self.fields.getTangentialField2( self.tank, self.flag, 100, 2.5, "CCW") delx2, dely2 = self.fields.getAttractiveField( self.tank, self.flag, 200, 2.5) delx2 = delx2 * .2 dely2 = dely2 * .2 delx = delx + delx2 dely = dely + dely2 else: delx, dely = self.fields.getAttractiveField( self.tank, self.flag, 800, 2.5) self.sendCommand(delx, dely, self.tank) if (self.job == "a"): flags = self.bzrc.get_flags() totalX = 0 totalY = 0 for obstacle in self.obstacles: deltaX, deltaY = self.fields.getTangentialField2( self.tank, obstacle, 100, 10, "CW") totalX += deltaX totalY += deltaY if (self.tank.flag == "-"): delx, dely = self.fields.getTangentialField2( self.tank, self.flag, 100, 2.5, "CCW") totalX += delx totalY += dely delx2, dely2 = self.fields.getAttractiveField( self.tank, flags[self.enemyFlag], 800, 2.5) totalX += delx2 totalY += dely2 else: delx, dely = self.fields.getAttractiveField( self.tank, self.base, 800, 40) totalX += delx totalY += dely # do offense self.sendCommand(totalX, totalY, self.tank) def goToPoint(self, tank, target): totalX = 0 totalY = 0 deltaX, deltaY = self.fields.getAttractiveField(tank, target, 800, 5) totalX += deltaX totalY += deltaY totalX += random.randrange(-300, 300) totalY += random.randrange(-300, 300) self.sendCommand(totalY, totalX, tank) def normalize_angle(self, angle): """Make any angle be between +/- pi.""" angle -= 2 * math.pi * int(angle / (2 * math.pi)) if angle <= -math.pi: angle += 2 * math.pi elif angle > math.pi: angle -= 2 * math.pi return angle def sendCommand(self, totalX, totalY, tank): othertanks = self.bzrc.get_othertanks() me = Point(tank.x, tank.y) cur = 100000 mins = 100000 enemy = Point(0, 0) for other in othertanks: cur = Point(other.x, other.y).distance(me) if (cur < mins): mins = cur enemy = other if mins <= 200: self.doKalman(enemy.x, enemy.y) else: theta = math.atan2(totalY, totalX) theta = self.normalize_angle(theta - tank.angle) speed = math.sqrt(totalY**2 + totalX**2) self.commands = [] command = Command(tank.index, .15 * speed, .85 * theta, False) self.commands.append(command) self.bzrc.do_commands(self.commands) self.commands = [] def kalmanCommand(self, totalX, totalY, theta, shoot, tank): p = Point(tank.x, tank.y) for obstacle in self.obstacles: if (p.distance(obstacle) < 50): deltaX, deltaY = self.fields.getTangentialField2( self.tank, obstacle, 100, 40, "CW") totalX = deltaX totalY = deltaY theta = math.atan2(totalY, totalX) theta = self.normalize_angle(theta - tank.angle) speed = math.sqrt(totalY**2 + totalX**2) self.commands = [] command = Command(tank.index, .15 * speed, 1.15 * theta, True) self.commands.append(command) self.bzrc.do_commands(self.commands) self.commands = []
# Estimating DSGE by Maximum Likelihood in Python # Author: Michal Miktus # Date: 20.03.2019 # Import libraries from LinApp_Solve import LinApp_Solve import matplotlib.pyplot as plt import seaborn as sn import pandas as pd import numpy as np import pickle from numpy import vstack, hstack, zeros, eye from Linear_Time_Iteration import Linear_Time_Iteration # %matplotlib inline # Set printing options np.set_printoptions(precision=3, suppress=True, linewidth=120) pd.set_option( 'float_format', lambda x: '%.3g' % x, ) # -------------------------------------------------------------------------- # -- Define the model # -------------------------------------------------------------------------- # VARIABLES [M,3] cell array: Variable name (case sensitive) ~ Variable type ~ Description # Variable type: 'X' ... Endogenous state variable # 'Y' ... Endogenous other (jump) variable # 'Z' ... Exogenous state variable # 'U' ... Innovation to exogenous state variable # '' ... Skip variable variable_symbols = [ r'$e_a$', r'$e_I$', r'$e_b$', r'$e_L$', r'$e_G$', r'$e_pi$', r'$pi$',
class Robot: def __init__(self): self.depthSub = rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.depth_callback) self.pose = rospy.Subscriber('/pose', PoseStamped, self.coordinate_callback) self.d1 = None # Distance from robot to landmark 1. self.d2 = None # Distance from robot to landmark 2. self.odom_initial_x = None self.odom_initial_y = None self.linear_velocity = 0 self.angular_velocity = 0 self.pose_x = 0 self.pose_y = 0 self.angular_displacement = 0 self.delta_t = 0 # Change in time between pose messages. self.delta_d = 0 # Change in distance between pose messages. self.delta_angle = 0 self.current_time = rospy.get_time() # Initial State: [x, y, phi, linear velocity] self.starting_state = np.array([[START_X, START_Y, 0, 0]]).reshape(4, 1) # Convert to 4x1 vector. # Initial velocity: [linear velocity, angular velocity] self.control_state = np.array([[0, 0]]).reshape(2, 1) self.filter = Kalman(self.starting_state, self.control_state) rospy.sleep(2) def coordinate_callback(self, message): # Track robot pose from odometry at t=0 in the event that initial odom value is non-zero. if self.odom_initial_x == None: self.odom_initial_x = message.pose.position.x if self.odom_initial_y == None: self.odom_initial_y = message.pose.position.y self.delta_angle = (message.pose.orientation.z - self.angular_displacement) self.delta_d = message.pose.position.x - self.odom_initial_x - self.pose_x self.delta_t = (rospy.get_time() - self.current_time) self.linear_velocity = float(self.delta_d/self.delta_t) self.angular_velocity = float(self.delta_angle/self.delta_t) self.control_state = np.array([[self.linear_velocity, self.angular_velocity]]).reshape(2, 1) self.current_time = rospy.get_time() self.pose_x = message.pose.position.x - self.odom_initial_x self.pose_y = message.pose.position.y - self.odom_initial_y self.angular_displacement = message.pose.orientation.z # Call Kalman Filter prediction with new odometry values. self.filter.prediction(self.control_state, self.angular_displacement, self.delta_t) def depth_callback(self, msg): self.d1 = None self.d2 = None # Update depth to landmarks with data from AR tags/RGB-D camera. if len(msg.markers) != 0: for marker in msg.markers: if marker.id == 0: # AR tag 1 self.d1 = marker.pose.pose.position.x if marker.id == 1: # AR tag 2 self.d2 = marker.pose.pose.position.x # If both landmarks are in frame, call Kalman Filter update step with new depth values. if self.d1 != None and self.d2 != None: self.filter.update(self.pose_x, self.pose_y, self.d1, self.d2, self.angular_displacement) def start(self): while not rospy.is_shutdown(): print(self.filter.x_t) pass
modVar = 0.5 pltStart = 1000 pltNum = 45 figSize = (11, 9) # Create some imaginary data obs = np.random.normal(obsMean, obsVar, size=totNum) model = obs + np.random.normal(modBias, modVar, size=len(obs)) # Define the plot range and size of the figure pltRange = range(pltStart, pltStart + pltNum) # Start Testing # Create an instance of the filter kf = Kalman(history, order) # Use the first #history of them for training obs_train = obs[:history] model_train = model[:history] # The rest to be used dynamically obs_dyn = obs[history:] model_dyn = model[history:] fcst = np.zeros_like(obs_dyn) # Perform an initial training of the model kf.trainMe(obs_train, model_train) for ij in range(len(obs_dyn)): # Provide a correction to the forecast
def __init__(self, Kp, Ki, n, A, B, C, Q, R): self.Kp = Kp self.Ki = Ki self.u = 0.0 self.err = 0.0 self.kalman = Kalman(n, A, B, C, Q, R)
def run_optical_flow_with_kalman(self, window_name, use_transformation=False): cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) cv2.setMouseCallback(window_name, self.on_mouse) # Init kalman filter params new_kalman, F, B, Q, H, R, init_mu, init_P, deltaT = self.init_kalman() kalman_array = [new_kalman] # Create a mask image for drawing purposes ret, old_frame = self.video.read() old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY) if use_transformation: old_gray = cv2.warpPerspective(old_gray, self.h, self.size) old_gray = self.rotateImage(old_gray, -90) mask = np.zeros_like(old_frame) has_completed_one_cycle = False while True: # Start timer timer = cv2.getTickCount() p0 = np.array([[[0, 0]]], dtype=np.float32) ret, frame = self.video.read() cv2.imwrite('Orig.png', frame) cv2.waitKey(10) throw_away_frame, background = self.subtract_background(frame) if use_transformation: frame = cv2.warpPerspective(frame, self.h, self.size) background = cv2.warpPerspective(background, self.h, self.size) frame = self.rotateImage(frame, -90) background = self.rotateImage(background, -90) background = cv2.cvtColor(background, cv2.COLOR_BGR2GRAY) frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) if has_completed_one_cycle: for point in self.trackedPoints: temp_array = np.append(p0, [[[point[0], point[1]]]], axis=0) p0 = np.array(temp_array, dtype=np.float32) new_points_to_track = self.find_new_trackpoints(background) for bbox in new_points_to_track: point_x = bbox[0] + bbox[2] / 2 point_y = bbox[1] + bbox[3] / 2 temp_array = np.append(p0, [[[point_x, point_y]]], axis=0) p0 = np.array(temp_array, dtype=np.float32) init_x = np.matrix([[point_x], [point_y], [0], [0]]) new_kalman = Kalman(F, B, Q, H, R, init_x, init_mu, init_P, deltaT) kalman_array.append(new_kalman) p0 = np.delete(p0, 0, axis=0) self.trackedPoints.clear() has_completed_one_cycle = True # Perform optical flow old_gray, kalman_array = self.increment_optical_flow_kalman( old_gray, frame_gray, mask, frame, p0, kalman_array) # Draw kalman self.draw_kalman_speed(frame, kalman_array, 40) # Calculate Frames per second (FPS) fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer) # Display FPS on frame cv2.putText(frame, "FPS : " + str(int(fps)), (100, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2) # Display result cv2.imshow(window_name, frame) # cv2.imwrite('Kalman.png', frame) # cv2.waitKey(500) # print("NOW!") # cv2.waitKey(500) # Exit if ESC pressed k = cv2.waitKey(1) & 0xff if k == 27: break
_, binary = cv2.threshold(img_blur, 25, 255, cv2.THRESH_BINARY) # Morphology kernel = np.ones((4, 4), np.uint8) binary = cv2.morphologyEx(binary, cv2.MORPH_OPEN, kernel) # find circles color, sorted_pt2 = find_circles(binary, color) # Kalman filter filtered_pts = np.zeros((4, 2)) pts_velocity = [] for i in range(0, len(sorted_pt2)): x_m = sorted_pt2[i][0] y_m = sorted_pt2[i][1] x_h, y_h, vx, vy, param = Kalman(x_m, y_m, Kalman_params[i]) Kalman_params[i] = param print(param.get_x()) filtered_pts[i, :] = np.array([x_h, y_h]) pts_velocity.append(vx) pts_velocity.append(vy) list_pts_vel.append(pts_velocity) """ # print the location retP, rvec, tvec = cv2.solvePnP(object_points, filtered_pts, cameraMtx, distCoff) text = 'Location: [' + \ str(int(tvec[0][0])) + ', ' + \ str(int(tvec[1][0])) + ', ' + \ str(int(tvec[2][0])) + ']' color = cv2.putText(color, text, (20, 20), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 0), 2) """
ax2.legend() # Guarda la primer gráfica con el nombre de los errores que se haya puesto; en el directorio 'Assets' fig.savefig(directorioAssets + "/" + tipo_sensor + " dt_" + str(dt) + " Q_" + str(_Q) + ".png") # Guarda la segunda gráfica con el nombre de los errores que se haya puesto; en el directorio 'Assets' fig2.savefig(directorioAssets + "/Ganancia_Kalman " + tipo_sensor + " dt_" + str(dt) + " Q_" + str(_Q) + ".png") plt.show() plt.show() if __name__ == "__main__": # Crea el objeto Kalman pasando 4 datos como parámetros. ka = Kalman(F, Q, H, R) i = 1 # Contador para guardar en el txt e identficar las iteraciones llenarDatos() verificarCarpetas() archivo = open(directorio + "/Datos_" + tipo_sensor + " dt_" + str(dt) + " Q_" + str(_Q) + ".txt", "w", encoding='utf-8') archivo.write("I, Z, Z_f, dt = " + str(dt) + "\t Q = " + str(_Q) + "\n") for dato_z in datos_x: x_f = np.dot(H, ka.predecir())[0] x_filtrada.append(x_f) ganancia.append(ka.actualizar(dato_z)[0])
def run(self): ## Short explanatory text re parameters, options and state. ## PARAMETERS ## Parameters are numbers where there is some optimal or obvious value, they might be tweaked between runs ## but ultimately we want to stick with one. ## ## OPTIONS ## Options are typically yes or no questions, but more over it is simple things that you might want to change ## between runs simply depending on what you want to do. Are you debugging? If so, printing output is nice. ## ## STATE ## State values are things that change during a run, it's all the stuff like the position of the particle ## the speed of the step engine, etc etc. # parameters holding object self.kalman = Kalman(fps_max=10, going_right=True, middle=self.parameters.middle) # option holding object options = self.options options.testing_disconnected = False options.printing_output = True # options.starting_left = True # options.needs_average = False options.saving_directory = "E:/staffana/Feb 26/" print "starting left is ", options.starting_left # state holding object. Could be replaced if we implement a Kalman filter #self.state = DetectorState(self.parameters, options) # Object stores the times taken for various computations in this class. timedata = TimeData( ) # The "class" that handles all the times we meassure stepfile = open("MetaData/step_data.txt", 'w') stepfile.write( "i \t re_rod_pos \t rod vel \t step vel \t\t correction_vel \t\t new velocity \n" ) #### CONNECT TO THE STEP MOTOR AND LET THE USER FIND A PARTICLE if not options.testing_disconnected: aver_im, self.state = InitializeStepEngine(self.order_list, self.im_list, options, self.parameters, timedata) #~ state.step_vel, aver_im, state.speed_error, state.vel_fac, state.going_right = outputs state = self.state # I think this is safe. ### TRY TO OPEN A SEPARATE WINDOW WHERE THE USER CAN PRESS STOP MAYBE BUT FIRST JUST SHOW IT else: # dirt_im = Image.open("Images/Averaging/average_image.jpg") dirt_im = Img.open("C:/test.jpg") dirt_arr = np.array(dirt_im) aver_im = dirt_arr.astype(np.float32) ## NOW WE START THE (POSSIBLY) NEVER ENDING THE LOOP OF TRACKING THE PARTICLE for j in range(1, 30000): timedata.iteration_start = time.clock() timedata.pos_save_total = 0 #print "before running SaveStep pos our timedata pos save total was ", timedata.pos_save_total ignorePosition = SaveStepPos(options, timedata) #print "After running SaveStep pos our timedata pos save total is ", timedata.pos_save_total if j % 2 == 0: i = j / 2 else: continue print "\n our current frame is ", i, "\n" #### ACQUIRE AND CROP THE IMAGE timedata.image_old = timedata.image_start timedata.image_start = time.clock() im = ImageGrab.grab(self.parameters.box) if options.printing_output: im.save("Images/cropped image" + str(i) + ".jpg") if self.image_to_show == "original": self.im_list.put(im) timedata.image_end = time.clock() #### PERFORM EDGE DETECTION AND CONTOUR DETECTION timedata.contour_start = time.clock() contours, pix = GetContours(im, aver_im, 190, options.printing_output, i) timedata.contour_end = time.clock() best_contour_nr, positions = DetectParticle( state, contours, i, timedata, pix, self.parameters, options) # i is the current nr of runs # To save space we could possibly merge contours with state? It seems a bit weird either way... ## Updates the state based on the particle we have, or have not, found. state.update(best_contour_nr, contours, positions, self.parameters, i, timedata, options) self.kalman.predict(state, self.parameters.step_2_pixel) self.kalman.update(state) print " the found position we think was ", state.pos_approx print "Kalman predicted the position to be ", [ self.kalman.x_pred.item(0), self.kalman.x_pred.item(2) ] print "Kalman thinks our actual position is ", [ self.kalman.x_est.item(0), self.kalman.x_est.item(2) ], if best_contour_nr < 0: continue ## If we are printing output we save a picture of the chosen contour if options.printing_output: try: cv2.drawContours(pix, [contours[best_contour_nr]], -1, (0, 255, 0), 3) except IndexError as ie: print "contours is ", contours, " and the best contour number is ", best_contour_nr raise im = Image.fromarray(pix) if self.image_to_show == "main_contour": self.im_list.put(im) #print "WE ADDED AN IMAGEHOORAY HOORAY \n \n" im.save("Images/main_contour" + str(i) + ".jpg") ## Saves the time now that we are done with detecting the particle timedata.detection_end #### MEASURE THE POSITION AND VELOCITY OF THE STEP ENGINE if not options.testing_disconnected: state.currentPosition = SaveStepPos(options, timedata) ## I HAVE CHAGED SAVE STEP POS NEED TO CHANGE THE DEFINITION state.update_correction_vector(state, self.parameters, options, current_frame=j) state.corr_vec *= 0 # this should probably also be a state operation.. it would really finish cutting down on the main loop! #### AND THEN DO THE REQUIRED CORRECTIONS timedata.step_control_start = time.clock() timedata.step_control_end = time.clock() ### IF WE ARE TOO CLOSE TO EITHER INLET WE WANT TO REVERSE THE PUMP state.check_pump() #### Check the image queue and the order queue if we should stop or something self.check_queues() #### SLEEP ANY TIME THAT IS LEFT timedata.iteration_end = time.clock() dt = timedata.iteration_end - timedata.iteration_start time_to_sleep = max(0, (1 / self.parameters.fps_max) - dt) timedata.write_times( ) ## Think of a better name for this function... # print "dt = ", dt, "time to sleep is ", time_to_sleep # print "we are taking approximately ", (dt*fps_max)*100 ,"% of the CPU" time.sleep(time_to_sleep) print " \n" # just to separate the iterations more.