def __init__(self, bzrc): self._kalman = Kalman() self.aliveTime = time.time() self.bzrc = bzrc self.constants = self.bzrc.get_constants() # print self.constants self.commands = [] self.potentialFields = [] self.updates = [] self.flag_sphere = 400 self.obstacle_sphere = 1000 self.enemy_sphere = 100 self.explore_sphere = 50 self.prev_x = {} self.prev_y = {} self.stuck_ticks = {} for x in xrange(20): self.prev_x[x] = 0 self.prev_y[x] = 0 self.stuck_ticks[x] = 0 self.path_fields = [] self.grid = Grid(int(self.constants['worldsize']), int(self.constants['worldsize']), self.initialGridConstant, self.initialGridConstant)
def __init__(self, interval=1): """ Constructor :type interval: int :param interval: Check interval, in seconds """ self.sensorfusion = Kalman() print("Connecting to sensortag...") self.tag = SensorTag(macAddress) print("connected.") self.tag.accelerometer.enable() self.tag.magnetometer.enable() self.tag.gyroscope.enable() self.tag.battery.enable() self.pitch = 0 self.angular_velocity = 0 self.linear_velocity = 0 self.acceleration = 0 time.sleep(1.0) # Loading sensors self.prev_time = time.time() thread = threading.Thread(target=self.run, args=()) thread.daemon = True # Daemonize thread thread.start() # Start the execution
def main(): sensorfusion = Kalman() print("Connecting to sensortag...") tag = SensorTag(macAddress) print("connected.") tag.accelerometer.enable() tag.magnetometer.enable() tag.gyroscope.enable() tag.battery.enable() time.sleep(1.0) # Loading sensors prev_time = time.time() while True: accelerometer_readings = tag.accelerometer.read() gyroscope_readings = tag.gyroscope.read() magnetometer_readings = tag.magnetometer.read() ax, ay, az = accelerometer_readings gx, gy, gz = gyroscope_readings mx, my, mz = magnetometer_readings curr_time = time.time() dt = curr_time - prev_time sensorfusion.computeAndUpdateRollPitchYaw(ax, ay, az, gx, gy, gz, mx, my, mz, dt) print(f"dt: {dt} pitch: {sensorfusion.pitch}") prev_time = curr_time print("Battery: ", tag.battery.read())
def __init__(self, bzrc, tank_index): #print tank_index self.agent_index = tank_index self._kalman = Kalman() self.aliveTime = time.time() self.bzrc = bzrc self.constants = self.bzrc.get_constants() self.mytanks = self.bzrc.get_mytanks() self.targetTank,self.targ = self.closest_enemy(self.mytanks[self.agent_index], self.bzrc.get_othertanks()) self._kalman.resetArrays(self.targetTank.x, self.targetTank.y) # print self.constants self.commands = [] self.potentialFields = [] self.updates = [] self.flag_sphere = 400 self.obstacle_sphere = 1000 self.enemy_sphere = 100 self.explore_sphere = 50 self.prev_x = {} self.prev_y = {} self.stuck_ticks = {} for x in xrange(20): self.prev_x[x] = 0 self.prev_y[x] = 0 self.stuck_ticks[x] = 0 self.path_fields = [] self.grid = Grid(int(self.constants['worldsize']), int(self.constants['worldsize']), self.initialGridConstant, self.initialGridConstant)
class BlueToothThreading: """ Bluetooth Threading The run() method will be started and it will run in the background until the application exits. """ def __init__(self, interval=1): """ Constructor :type interval: int :param interval: Check interval, in seconds """ self.sensorfusion = Kalman() print("Connecting to sensortag...") self.tag = SensorTag(macAddress) print("connected.") self.tag.accelerometer.enable() self.tag.magnetometer.enable() self.tag.gyroscope.enable() self.tag.battery.enable() self.pitch = 0 time.sleep(1.0) # Loading sensors self.prev_time = time.time() thread = threading.Thread(target=self.run, args=()) thread.daemon = True # Daemonize thread thread.start() # Start the execution def run(self): """ Method that runs forever """ while True: accelerometer_readings = self.tag.accelerometer.read() gyroscope_readings = self.tag.gyroscope.read() magnetometer_readings = self.tag.magnetometer.read() ax, ay, az = accelerometer_readings gx, gy, gz = gyroscope_readings mx, my, mz = magnetometer_readings curr_time = time.time() dt = curr_time - self.prev_time self.sensorfusion.computeAndUpdateRollPitchYaw( ax, ay, az, gx, gy, gz, mx, my, mz, dt) self.pitch = self.sensorfusion.pitch self.prev_time = curr_time print("Battery: ", self.tag.battery.read())
def __init__( self, set_point, water_level, maximum_flowrate, initial_flowrate=0, learning_rate=0.1, last_layer_activation_function='tanh', initial_error_covariance_matrix=initial_error_covariance_matrix, transition_matrix=transition_matrix, input_matrix=input_matrix, input_vector=input_vector, measurement_matrix=measurement_matrix, disturbance=disturbance, uncertainty=uncertainty): initial_state = np.asarray([initial_flowrate]).T self.kalman = Kalman( initial_state=initial_state, initial_error_covariance_matrix=initial_error_covariance_matrix, transition_matrix=transition_matrix, measurement_matrix=measurement_matrix, input_matrix=input_matrix, input_vector=input_vector, measurement_noise=uncertainty, process_noise=disturbance) self.pidnn = PIDNN( set_point=set_point, actual_value=water_level, prev_output=initial_flowrate, learning_rate=learning_rate, last_layer_activation_function=last_layer_activation_function) self.maximum_flowrate = maximum_flowrate
def _filter_xs(self, s_0, s_ts, pi_0, sigma_0, As, Cs, Qs, Rs): """ Computes x_1|1, P_1|1, ..., x_T|T, P_T|T given observations, parameters and values of switching variable at each time. :returns: -n_LF x T numpy array Smoothed means -T x n_LF x n_LF numpy array Smoothed covariances -T-1 x n_LF x n_LF numpy array Smoothed lagged covariances (ie, cov[x_t, x_t-1]) """ # Initialize Kalman filter using values of parameters at t = 0, even though they're never used kf = Kalman(mu_0=pi_0.copy(), sigma_0=sigma_0.copy(), A=As[s_0], B=None, C=Cs[s_0], D=None, Q=Qs[s_0], R=Rs[s_0]) # x_t|t, P_t|t, t = 1, ..., T x_filts = np.zeros([self.T, self.n_LF]) P_filts = np.zeros([self.T, self.n_LF, self.n_LF]) # x_t|t-1, P_t|t-1. t = 1, ..., T. Indexed by t. x_pred = np.zeros([self.T, self.n_LF]) P_pred = np.zeros([self.T, self.n_LF, self.n_LF]) # Filtering step for t in range(0, self.T): # corresponds to t = 1, ..., T # Change parameters. Never need to use A_{s_0}, etc. kf.A = As[s_ts[t]] kf.C = Cs[s_ts[t]] kf.Q = Qs[s_ts[t]] kf.R = Rs[s_ts[t]] # Compute x_{t|t-1}, P_{t|t-1} kf.predict() x_pred[t] = kf.mu P_pred[t] = kf.sigma # Compute x_{t|t}, P_{t|t} kf.update(self.y_ts[t]) x_filts[t] = kf.mu P_filts[t] = kf.sigma # TODO: run smoother to fill in missing data!!! return x_filts, P_filts, x_pred, P_pred
def __init__(self): rospy.init_node("IK_node", anonymous=False) self.input_sub = rospy.Subscriber("/vx250/input_arm_pose", ArmPose, self.callback) self.marker_pub = rospy.Publisher("/vx250/joint_state_est", Marker, queue_size=1) self.angles_pub = rospy.Publisher("/vx250/arm_controller/command", JointTrajectory, queue_size=1) self.measurement = ArmState() self.states = { 'Green': Kalman( np.ones((6, 1)) / 1000, np.diag([ 2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn() ]), 1 / LOOP_RATE), 'Red': Kalman( np.ones((6, 1)) / 1000, np.diag([ 2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn() ]), 1 / LOOP_RATE), 'Blue': Kalman( np.ones((6, 1)) / 1000, np.diag([ 2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn() ]), 1 / LOOP_RATE), 'Yellow': Kalman( np.ones((6, 1)) / 1000, np.diag([ 2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn() ]), 1 / LOOP_RATE) } self.prev_states = self.states self.rate = rospy.Rate(LOOP_RATE)
def __init__(self): self.A_k = None self.B_k = None self.dt = 0.02 # simulation time step self.t_rc = 0.005 # membrane RC time constant self.t_ref = 0.001 # refractory period self.tau = 0.014 # synapse time constant for standard first-order lowpass filter synapse self.N_A = 1000 # number of neurons in first population self.rate_A = 200, 400 # range of maximum firing rates for population A self.model = nengo.Network(label="Kalman SNN") self.sim = None self.kalman = Kalman() self.output = None self.testX = None self.count = 0 self.chN = None
def __init__(self, robot_id): # Does this need to be a node? Maybe it could be a tf? rospy.init_node('Filter' + str(robot_id)) self.pub = rospy.Publisher("Filter" + str(robot_id) + "/state", Odometry, queue_size=1) self.sub = rospy.Subscriber("Sensor/measurement" + str(robot_id), Odometry, self.new_measurement) self.kf = Kalman() self.state_out = Odometry() self.state = np.array([[0], [0], [0], [0], [0], [0]]) self.startTime = rospy.get_time() self.oldTime = 0.0
def init_demo_kalman(): n = m = 2 A = np.array([[0.9, 0.2], [-0.3, 0.5]]) H = np.array([[1.0, 0.1], [-0.2, 1.1]]) R = 0.5 * np.eye(m) Q = 1.4 * np.eye(n) x0 = [-0.7, 0.9] sys = LinearSystem(n, m, x0, A, H, R, Q) kalman = Kalman(n, m, sys) return kalman
def __init__(self, bzrc): self.bzrc = bzrc self.constants = self.bzrc.get_constants() self.pos_noise = 5 self.commands = [] self.kalman = Kalman(self.pos_noise) self.max_tank_speed = float(self.constants['tankspeed']) self.linear_acceleration = float(self.constants['linearaccel']) self.shot_speed = float(self.constants['shotspeed']) mytanks, othertanks, flags, shots = self.bzrc.get_lots_o_stuff() enemies = [tank for tank in othertanks if tank.color != self.constants['team']] covariance_list = [[100, 0, 0, 0, 0, 0], [ 0, 0.1, 0, 0, 0, 0], [ 0, 0, 0.1, 0, 0, 0], [ 0, 0, 0, 100, 0, 0], [ 0, 0, 0, 0, 0.1, 0], [ 0, 0, 0, 0, 0, 0.1]] self.observed_states = [] self.states = [] self.state_covariances = [] for enemy in enemies: new_observed = numpy.matrix(numpy.zeros((6, 1))) new_observed.getA()[0][0] = enemy.x new_observed.getA()[3][0] = enemy.y self.observed_states.append(new_observed) new_state = numpy.matrix(numpy.zeros((6, 1))) self.states.append(new_state) new_covariance = numpy.matrix(covariance_list) self.state_covariances.append(new_covariance) # PD control self.old_angle = [0] * len(mytanks) # initialize plot plt.ion() self.fig = plt.figure() self.fignum = 0 self.graph = self.fig.add_subplot(111) worldsize = float(self.constants['worldsize']) worldlimit = worldsize / 2.0 self.graph.axis([-worldlimit, worldlimit, -worldlimit, worldlimit]) plt.show()
def __init__(self, initial_point, trackIdCount): """Initialize variables used by Track class Args: prediction: predicted centroids of clusters to be tracked trackIdCount: identification of each track object Return: None """ self.track_id = trackIdCount # identification of each track object self.KF = Kalman(initial_point) # KF instance to track this object self.prediction = np.asarray( initial_point) # predicted centroids (x,y) self.skipped_frames = 0 # number of frames skipped undetected self.last_detection_assigment = 0 # number of frames skipped undetected self.age = 0 # the number of frames this tracker has lived for self.trace = [] # trace path
def new_measurement(self, data): self.startTime = rospy.get_time() timestep = self.startTime - self.oldTime self.oldTime = self.startTime self.state = Kalman.update( self.kf, self.state, data.twist.twist.linear.x, data.twist.twist.angular.z, timestep, [data.pose.pose.position.x, data.pose.pose.position.y]) self.state_out.pose.pose.position.x = self.state[0] self.state_out.pose.pose.position.y = self.state[2] self.state_out.twist.twist.linear.x = math.sqrt(self.state[1]**2 + self.state[3]**2) angle = 1 + self.state[4] - 1 quat = tf.transformations.quaternion_from_euler(0.0, 0.0, angle) self.state_out.pose.pose.orientation.x = quat[0] self.state_out.pose.pose.orientation.y = quat[1] self.state_out.pose.pose.orientation.z = quat[2] self.state_out.pose.pose.orientation.w = quat[3] self.state_out.twist.twist.angular.z = self.state[5] #print str(self.state) self.pub.publish(self.state_out)
def __init__(self, trackId, rect, img, mask): self.trackId = trackId self.lastClass = 'unknown' self.lastKeypoints = None self.rect = [] self.searchWindow = rect self.template = [] self.tracklet = [] self.trackletRects = [] self.direction = Enum(['LEFT', 'RIGHT', 'UNKNOWN']) self.actions = [] self.actions.append('unknown') self.classHistory = [] self.active = True self.occluded = False self.foundCount = 0 self.numTemplates = 6 self.outDimens = None #self.trackOutputStream = None self.doTrackWrite = False self.savedFrames = [] self.outFps = 30 height, width = img.shape[:2] self.imgBounds = (width, height) x, y, w, h = rect x, y, w, h = rect = self.resizeRect(rect, (1.0, 1.0), (0.0, 0.0)) #x,y,w,h = rect = self.resizeRect(rect,(1.0,0.75),(-0.15,-0.2)) #crop syntax is [starty:endy, startx:endx] template = img[y:y + h, x:x + w] #maskFiltered = mask[y:y+h, x:x+w] # 3 Kalman filters per research document self.kalmanFast = Kalman((x + (w / 2), y + (h / 2))) self.kalmanArea = Kalman((w, h)) # ,0.003,0.3) self.kalmanSlow = Kalman((x + (w / 2), y + (h / 2)), 0.003, 0.03) self.updateTemplate(rect, template) #x,y,w,h = rect = self.resizeRect(rect,(1.5,1.3),(0,0.5)) self.trackletSize = (0, 0) self.updateTracklet(rect, img)
import numpy as np import matplotlib.pyplot as plt from kalman import Kalman from scipy.stats import norm from scipy.integrate import quad ## Parameters theta = 10 A, G, Q, R = 1, 1, 0, 1 x_hat_0, Sigma_0 = 8, 1 epsilon = 0.1 ## Initialize Kalman filter kalman = Kalman(A, G, Q, R) kalman.set_state(x_hat_0, Sigma_0) T = 600 z = np.empty(T) for t in range(T): # Record the current predicted mean and variance, and plot their densities m, v = kalman.current_x_hat, kalman.current_Sigma m, v = float(m), float(v) f = lambda x: norm.pdf(x, loc=m, scale=np.sqrt(v)) integral, error = quad(f, theta - epsilon, theta + epsilon) z[t] = 1 - integral # Generate the noisy signal and update the Kalman filter kalman.update(theta + norm.rvs(size=1)) fig, ax = plt.subplots() ax.set_ylim(0, 1) ax.set_xlim(0, T) ax.plot(range(T), z)
#!/usr/bin/env python import flask from random import randrange from kalman import Kalman import time app = flask.Flask(__name__) input_data_range = 5000*2 k_f = Kalman() k_f.setAngle( 0.0 ) @app.route('/') def index(): input_ = [] setpoint = [] output_ra = [] output_ma = [] output_kf = [] output_cf = [] # Raw data for i in range(input_data_range): input_.append( round(randrange(-4,4)/float(randrange(2,5)),1) ) setpoint.append( 0.0 ) # Moving Average Algorithm m = input_[0] output_ma.append(m)
class TrackBase: def __init__(self, trackId, rect, img, mask): self.trackId = trackId self.lastClass = 'unknown' self.lastKeypoints = None self.rect = [] self.searchWindow = rect self.template = [] self.tracklet = [] self.trackletRects = [] self.direction = Enum(['LEFT', 'RIGHT', 'UNKNOWN']) self.actions = [] self.actions.append('unknown') self.classHistory = [] self.active = True self.occluded = False self.foundCount = 0 self.numTemplates = 6 self.outDimens = None #self.trackOutputStream = None self.doTrackWrite = False self.savedFrames = [] self.outFps = 30 height, width = img.shape[:2] self.imgBounds = (width, height) x, y, w, h = rect x, y, w, h = rect = self.resizeRect(rect, (1.0, 1.0), (0.0, 0.0)) #x,y,w,h = rect = self.resizeRect(rect,(1.0,0.75),(-0.15,-0.2)) #crop syntax is [starty:endy, startx:endx] template = img[y:y + h, x:x + w] #maskFiltered = mask[y:y+h, x:x+w] # 3 Kalman filters per research document self.kalmanFast = Kalman((x + (w / 2), y + (h / 2))) self.kalmanArea = Kalman((w, h)) # ,0.003,0.3) self.kalmanSlow = Kalman((x + (w / 2), y + (h / 2)), 0.003, 0.03) self.updateTemplate(rect, template) #x,y,w,h = rect = self.resizeRect(rect,(1.5,1.3),(0,0.5)) self.trackletSize = (0, 0) self.updateTracklet(rect, img) # set up the ROI for tracking #roi = template #hsv_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV) #maskFiltered = cv2.inRange(hsv_roi, np.array((0., 150.,60.)), np.array((180.,255.,255.))) #cv2.imshow('maskFiltered',maskFiltered) #self.roi_hist = cv2.calcHist([hsv_roi],[0,1],maskFiltered,[36,50],[0,180,0,255]) #cv2.normalize(self.roi_hist,self.roi_hist,0,255,cv2.NORM_MINMAX) # returns true if the provided point plus current track bounds go over the edge of the frame, false otherwise def isInBounds(self, point, bounds): bh, bw = bounds px, py = point x, y, w, h = self.getNewestRect() if px + w / 2 > bw or px - w / 2 < 0: return False if py + h / 2 > bh or py - h / 2 < 0: return False return True # search for this track in the provided frame by whatever concrete algorithm the track implements def update(self, img): self.savedFrames.append(img) # always do predictions before measurements pLoc = self.kalmanFast.predict() self.kalmanSlow.predict() self.kalmanArea.predict() # make sure we haven't already gone off the frame if not self.isInBounds(pLoc, img.shape[:2]): self.active = False print('Track #' + repr(self.trackId) + ' ' + repr(self.occluded) + ' ENDING TRACK: gone off frame ' + repr(pLoc)) return newroi = (0, 0, 0, 0) # search phase if not self.occluded: self.occluded, newroi = self.doNonOccludedSearch(img) else: self.occluded, newroi = self.doOccludedSearch(img) # update phase if not self.occluded: self.doNonOccludedUpdate(newroi, img) else: self.doOccludedUpdate(newroi, img) #if not self.trackOutputStream is None and self.doTrackWrite: #w,h = self.outDimens #resized = cv2.resize(self.getNewestTemplate(), (w,h)) # self.trackOutputStream.write(self.getNewestTracklet()) #print('Track #' + repr(self.trackId) + ' ' + repr(self.occluded) + ' ROI: ' + repr(self.rect[0])) # perform search for tracked object with the assumption it was occluded in the previous frame # default implementation provides a global template match for previously known good template def doOccludedSearch(self, img): # set search window so we're not performing a global search roi = self.resizeRect(self.getNewestRect(), (4, 4)) x, y, w, h = roi newroi = (0, 0, 0, 0) self.searchWindow = roi window = img[y:y + h, x:x + w] temp = self.getOldestTemplate() # make sure we haven't already gone off the frame if window.shape[:2][0] <= 0 or window.shape[:2][0] < temp.shape[:2][ 0] or window.shape[:2][1] <= 0 or window.shape[:2][ 1] < temp.shape[:2][1]: self.active = False print('Track #' + repr(self.trackId) + ' ' + repr(self.occluded) + ' ENDING TRACK: match window off frame ' + repr(window.shape[:2])) return True, newroi #print('Track #' + repr(self.trackId) + ' window:' + repr(window.shape[:2]) + ' temp:' + repr(temp.shape[:2])) #cv2.imshow('template',temp) # perform the template matching search res = cv2.matchTemplate(window, temp, cv2.TM_CCOEFF_NORMED) min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res) if max_val >= 0.7: #found match px, py = max_loc x, y, w, h = roi x2, y2, w2, h2 = self.getOldestRect() newroi = (px + x, py + y, w2, h2) return False, newroi else: return True, newroi #print('Track #' + repr(self.trackId) + ' ' + repr(self.occluded) + ' CORR: ' + repr(max_val)) # perform search for tracked object with the assumption it was not occluded in the previous frame # all subclasses should implement this method with their specific tracking algorithm def doNonOccludedSearch(self, img): return # updates the track when it was not found in the search frame using Kalman filter predictions def doOccludedUpdate(self, roi, img): if self.foundCount < 10: pLocS = self.kalmanFast.getLastPrediction() else: pLocS = self.kalmanSlow.getLastPrediction() x, y, w, h = self.getOldestRect() newRoi = self.resizeRect( (int(pLocS[0] - (w / 2)), int(pLocS[1] - (h / 2)), w, h), (1.0, 1.0)) self.updateTemplate(newRoi, None) self.updateTracklet(newRoi, img) # don't want missing frames in tracklet # updates the track with a real measurement when it was found in the search frame def doNonOccludedUpdate(self, roi, img): self.foundCount = self.foundCount + 1 x, y, w, h = roi # correct Kalman filters self.kalmanFast.correct((x + (w / 2), y + (h / 2))) self.kalmanSlow.correct((x + (w / 2), y + (h / 2))) self.kalmanArea.correct((w, h)) self.updateTemplate(roi, img[y:y + h, x:x + w]) self.updateTracklet(roi, img) # TODO: remove when deemed no longer necessary # callback when a potential seed roi is detected near this def onFindIntersect(self, roi, img): return # returns true if provided roi is near this track position def matches(self, roi, img): if self.intersects(roi, 100): self.onFindIntersect(roi, img) return True else: return False def updateTemplate(self, rect, template): x, y, w, h = rect #self.rect.insert( 0,rect ) if not rect is None: self.rect.append(rect) if len(self.rect) > self.numTemplates: del self.rect[0] #del self.rect[self.numTemplates-1] #self.template.insert( 0,template ) if not template is None: self.template.append(template) if len(self.template) > self.numTemplates: del self.template[0] #del self.template[self.numTemplates-1] def updateTracklet(self, rect, img): #x,y,w,h = rect = self.resizeRect(rect,(1.3,1.5),(-0.15,0.25)) #wave x, y, w, h = rect = self.resizeRect(rect, (1.2, 1.5), (0.0, 0.25)) if self.trackletSize[0] == 0: self.trackletSize = (w, h) else: w, h = self.trackletSize #x = rect[0] #y = rect[1] roiGray = cv2.cvtColor(img[y:y + h, x:x + w], cv2.COLOR_BGR2GRAY) self.tracklet.append(roiGray) self.trackletRects.append((x, y, w, h)) #if not self.trackOutputStream is None and self.doTrackWrite: # self.trackOutputStream.write(img[y:y+h, x:x+w]) # returns true is the provided rectangle touches this track's known rectangle # based on a circle drawn from the center def intersects(self, rect, dmin=100): x1, y1, w1, h1 = rect x2, y2, w2, h2 = self.getNewestRect() center1 = (int(x1 + w1 / 2), int(y1 + h1 / 2)) center2 = (int(x2 + w2 / 2), int(y2 + h2 / 2)) # radius of search area radius1 = int(math.hypot(w1, h1) / 2) radius2 = int(math.hypot(w2, h2) / 2) # distance between centers dist = int(math.hypot(center1[0] - center2[0], center1[1] - center2[1])) #cv2.line(img, center1, center2, (0,255,255),1) #cv2.circle(img, center1, radius1, (0,0,255), 1) #cv2.circle(img, center2, radius2, (0,255,255), 1) return dist < (radius1 + radius2) or dist < dmin # return the input rectangle resized by given area coefficient and bounds checked against provided bounds def resizeRect(self, rect, scale=(1.0, 1.0), offsets=(0, 0)): x, y, w, h = rect width, height = self.imgBounds if x < 0: x = 0 sx = x - (w * (scale[0] - 1.0)) / 2 sx = sx + offsets[0] * w if sx < 0: sx = 0 if sx >= width: sx = width - 1 ex = sx + (w * scale[0]) if ex >= width: ex = width - 1 if y < 0: y = 0 sy = y - (h * (scale[1] - 1.0)) / 2 sy = sy + offsets[1] * h if sy < 0: sy = 0 if sy >= height: sy = height - 1 ey = sy + (h * scale[1]) if ey >= height: ey = height - 1 return (int(sx), int(sy), int(ex - sx), int(ey - sy)) # Returns a measure of how close the provided image's histogram matches this one. Lower value = more similar def histogramsMatch(self, img1, img2, thresh=0.6): hsv_roi1 = cv2.cvtColor(img1, cv2.COLOR_BGR2HSV) hist1 = cv2.calcHist([hsv_roi1], [0, 1], None, [36, 50], [0, 180, 0, 255]) cv2.normalize(hist1, hist1, 0, 255, cv2.NORM_MINMAX) hsv_roi2 = cv2.cvtColor(img2, cv2.COLOR_BGR2HSV) hist2 = cv2.calcHist([hsv_roi2], [0, 1], None, [36, 50], [0, 180, 0, 255]) cv2.normalize(hist2, hist2, 0, 255, cv2.NORM_MINMAX) score = cv2.compareHist(hist1, hist2, cv2.HISTCMP_BHATTACHARYYA) return score <= thresh, score # TODO: resize templates more intelligently # returns true if the provided images match, false otherwise def templatesMatch(self, img1, img2, thresh=0.3): #cv2.imshow('template',img) h1, w1 = img1.shape[:2] h2, w2 = img2.shape[:2] w = min((w1, w2)) h = min((h1, h2)) if w <= 0 or h <= 0: return False, 0.0 x1 = (w1 - w) / 2 y1 = (h1 - h) / 2 x2 = (w2 - w) / 2 y2 = (h2 - h) / 2 crop1 = img1[y1:y1 + h, x1:x1 + w] crop2 = img2[y2:y2 + h, x2:x2 + w] #resized = cv2.resize(img2, (w,h)) #print('Track #' + repr(self.trackId) + ' img1:' + repr(img1.shape[:2]) + ' img2:' + repr(img2.shape[:2]) + ' resize:' + repr(resized.shape[:2])) #cv2.imshow('resized',resized) res = cv2.matchTemplate(crop1, crop2, cv2.TM_CCOEFF_NORMED) min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res) return max_val >= thresh, max_val def getNewestRect(self): #return self.rect[0] return self.rect[-1] def getTrackletRect(self): return self.trackletRects[-1] def getTrackletRects(self): return self.trackletRects def saveLastTracklet(self): lasttracklet = self.getLastTracklet(45) if not lasttracklet is None: h, w = lasttracklet[0].shape[:2] outputStream = cv2.VideoWriter( 'track' + repr(self.trackId) + '.avi', cv2.VideoWriter_fourcc(*'DIVX'), self.outFps, (w, h)) for frame in lasttracklet: outputStream.write(frame) outputStream.release() def getLastTracklet(self, numFrames): if len(self.trackletRects) > numFrames: h, w = self.savedFrames[0].shape[:2] temptracklet = [] #temprects = [] xmax, xmin, ymax, ymin = (0, w, 0, h) for rect in self.trackletRects[-numFrames:]: x, y, w, h = rect xmin = x if x < xmin else xmin xmax = (x + w) if (x + w) > xmax else xmax ymin = y if y < ymin else ymin ymax = (y + h) if (y + h) > ymax else ymax for frame in self.savedFrames[-numFrames:]: temptracklet.append(frame[ymin:ymin + (ymax - ymin), xmin:xmin + (xmax - xmin)]) return temptracklet else: return None def getNewestTracklet(self): return self.tracklet[-1] def getOldestRect(self): return self.rect[0] #if len(self.rect) < self.numTemplates : # return self.rect[len(self.rect)-1] #else: # return self.rect[self.numTemplates-1] def getNewestTemplate(self): return self.template[-1] def getOldestTemplate(self): return self.template[0] #if len(self.template) < self.numTemplates : # return self.template[len(self.template)-1] #else: # return self.template[self.numTemplates-1] def getPath(self): return self.kalmanFast.getPath() def getPredicted(self): return self.kalmanFast.getPredicted() def getTrackId(self): return self.trackId def isOccluded(self): return self.occluded def getSearchWindow(self): return self.searchWindow def isActive(self): return self.active def updateClass(self, classType, kp): self.lastKeypoints = kp self.lastClass = classType self.classHistory.append(classType) try: lc = mode(self.classHistory) self.lastClass = lc return self.lastClass except StatisticsError: return self.lastClass def getLastClass(self): return self.lastClass def getLastKeypoints(self): return self.lastKeypoints def getTracklet(self): return self.tracklet def updateAction(self, action): self.actions.append(action) return self.actions[-1] def getLastAction(self): return self.actions[-1] def setDoTrackWrite(self, doWrite): self.doTrackWrite = doWrite def setOutFps(self, fps): self.outFps = fps
image_folder_path = "./images/" measurements_path = "./measurements/ball_20.csv" dt = 0.1 u = 0 # Acceleration std_acc = 0.001 # Q std_pos = 0.5 # R image_names = os.listdir(image_folder_path) image_names.sort() measurements = np.genfromtxt(measurements_path, delimiter=",") kalman = Kalman() kalman.init(dt, u, std_acc, std_pos) for i in range(len(image_names)): image = cv2.imread(image_folder_path + image_names[i]) measurement = measurements[i].astype(np.int) # Given measurement color BLUE cv2.rectangle(image, (measurement[0], measurement[1]), (measurement[0]+measurement[2], measurement[1]+measurement[3]), (255, 0, 0), 2) (x, y, vx, vy) = kalman.predict() # Predicted state color GREEN cv2.rectangle(image, (x, y), (x + measurement[2], y + measurement[3]), (0, 255, 0), 2) update = np.array([[measurement[0]+(measurement[2]/2)], [measurement[1]+(measurement[3]/2)]])
from numpy.random import multivariate_normal import matplotlib.pyplot as plt from kalman import Kalman # The prior density Sigma = [[0.4, 0.3], [0.3, 0.45]] Sigma = np.array(Sigma) x_hat = np.array([0.2, -0.2]) # Define A, Q, G, R G = np.eye(2) R = 0.5 * Sigma A = np.eye(2) Q = np.zeros(2) ## Initialize Kalman filter kn = Kalman(A, G, Q, R) kn.set_state(x_hat, Sigma) # The true value of the state theta = np.zeros(2) + 4.0 T = 1000 z = np.empty(T) for t in range(T): # Measure the error y = multivariate_normal(mean=theta, cov=R) kn.update(y) z[t] = np.sqrt(np.sum((theta - kn.current_x_hat)**2)) fig, ax = plt.subplots() ax.plot(range(T), z)
# Create a socket object soc = socket.socket(socket.AF_INET, socket.SOCK_STREAM) time.sleep(3) # arduino will reset on serial open # let it calibrate, has to be still # Initialize orientation vectors acc = [0.0, 0.0, 0.0] mag = [0.0, 0.0, 0.0] # Reset rpy reset_condition = True # Kalman objects kalman_x = Kalman() kalman_y = Kalman() kalman_z = Kalman() ##################################### # YOU NEED TO ADJUST THESE (TASK 4) # ##################################### # Set Kalman sensor noise stdevs kalman_x.setSensorNoise(0.2, 0.03) # 0.2, 0.03 kalman_y.setSensorNoise(0.2, 0.03) # 0.2, 0.03 kalman_z.setSensorNoise(0.6, 0.03) # 0.6, 0.03 # kalman_x.setSensorNoise(0.01, 0.01) # kalman_y.setSensorNoise(0.01, 0.01) # kalman_z.setSensorNoise(0.01, 0.01)
import time import numpy as np from icm20948 import ICM20948 from kalman import Kalman import csv imu = ICM20948() sensorfusion = Kalman() imu.read_sensors() imu.computeOrientation() sensorfusion.roll = imu.roll sensorfusion.pitch = imu.pitch sensorfusion.yaw = imu.yaw currTime = time.time() while True: imu.read_sensors() imu.computeOrientation() print( f"roll: {round(imu.roll,3)}, pitch: {round(imu.pitch,3)}, yaw: {round(imu.yaw,3)}" ) newTime = time.time() dt = newTime - currTime currTime = newTime sensorfusion.computeAndUpdateRollPitchYaw( imu.accel_data[0], imu.accel_data[1], imu.accel_data[2],
# === Define A, Q, G, R === # G = np.eye(2) R = 0.5 * np.eye(2) A = [[0.5, 0.4], [0.6, 0.3]] Q = 0.3 * np.eye(2) # === Define the prior density === # Sigma = [[0.9, 0.3], [0.3, 0.9]] Sigma = np.array(Sigma) x_hat = np.array([8, 8]) # === Initialize the Kalman filter === # kn = Kalman(A, G, Q, R) kn.set_state(x_hat, Sigma) # === Set the true initial value of the state === # x = np.zeros(2) # == Print eigenvalues of A == # print("Eigenvalues of A:") print(eigvals(A)) # == Print stationary Sigma == # S, K = kn.stationary_values() print("Stationary prediction error variance:") print(S) # === Generate the plot === #
class KAgent(object): """Class handles all command and control logic for a teams tanks.""" initialGridConstant = 0 def __init__(self, bzrc): self._kalman = Kalman() self.aliveTime = time.time() self.bzrc = bzrc self.constants = self.bzrc.get_constants() # print self.constants self.commands = [] self.potentialFields = [] self.updates = [] self.flag_sphere = 400 self.obstacle_sphere = 1000 self.enemy_sphere = 100 self.explore_sphere = 50 self.prev_x = {} self.prev_y = {} self.stuck_ticks = {} for x in xrange(20): self.prev_x[x] = 0 self.prev_y[x] = 0 self.stuck_ticks[x] = 0 self.path_fields = [] self.grid = Grid(int(self.constants['worldsize']), int(self.constants['worldsize']), self.initialGridConstant, self.initialGridConstant) def tick(self, time_diff): """Some time has passed; decide what to do next.""" # don't need to know where the flags or shots are when exploring. Enemies are included in the 'othertanks' call # pos,partialGrid = self.bzrc.get_occgrid() # self.grid.updateGrid(pos,partialGrid) self._kalman.setDT(time_diff) self.mytanks = self.bzrc.get_mytanks() # self.othertanks = self.bzrc.get_othertanks() targetTank = self.bzrc.get_othertanks()[0] self.commands = [] if targetTank.status == 'alive': self.lock_on(targetTank) else: self.aliveTime = time.time() stopMoving = Command(0,0,0,False) self.commands.append(stopMoving) self.bzrc.do_commands(self.commands) self._kalman.resetArrays() def lock_on(self, targetTank): print str(targetTank.x) agentTank = self.mytanks[0] Zt = array([[targetTank.x], [targetTank.y]]) self._kalman.updateKalmanFilter(Zt) est = self._kalman.H.dot(self._kalman.mu) self.updates.append(((int(est[0][0]), int(est[1][0])),self._kalman.sigmaT)) aimAngle,distance = self.take_aim((agentTank.x,agentTank.y), agentTank.angle) command = Command(0,0,aimAngle*2,True) if aimAngle < 1 and aimAngle > -1: if distance < 350: command = Command(0,0,aimAngle*2,True) else: command = Command(0,0,aimAngle*2,False) else: command = Command(0,0,aimAngle*2,False) self.commands.append(command) self.bzrc.do_commands(self.commands) def take_aim(self, tankPosition, tankAngle): xPos = self._kalman.mu[0][0] xVel = self._kalman.mu[1][0] xAcc = self._kalman.mu[2][0] yPos = self._kalman.mu[3][0] yVel = self._kalman.mu[4][0] yAcc = self._kalman.mu[5][0] tankX, tankY = tankPosition velocity = math.sqrt(xVel**2 + yVel**2) acceleration = math.sqrt(xAcc**2 + yAcc**2) distance = self.dist(tankX,tankY,xPos,yPos) timeToEnemy = 0 r = (100-velocity)**2 - 4*(acceleration/2)*distance*-1 plusRoot = (-(100-velocity) + math.sqrt(r)) / acceleration minusRoot = (-(100-velocity) - math.sqrt(r)) / acceleration if plusRoot > minusRoot: timeToEnemy = plusRoot else: timeToEnemy = minusRoot self._kalman.setDT(timeToEnemy) projectionMatrix = self._kalman.projectPosition() projectedX = projectionMatrix[0][0] projectedY = projectionMatrix[3][0] distance = self.dist(tankX,tankY,projectedX,projectedY) angel = math.atan2(projectedY - tankY, projectedX - tankX) return (self.normalize_angle(angel - tankAngle), distance) def doUpdate(self): # print len(self.updates) for update in self.updates: pos,partialGrid = update self.grid.updateGrid(pos,partialGrid) self.updates = [] def dist(self, x1, y1, x2, y2): dist_result = math.sqrt((float(x1) - float(x2))**2 + (float(y1) - float(y2))**2) #print dist_result return dist_result 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
gx_data = df['gx'].as_matrix() gy_data = df['gy'].as_matrix() gz_data = df['gz'].as_matrix() offset_num = 500 ax_offset = np.mean(ax_data[0:offset_num]) ay_offset = np.mean(ay_data[0:offset_num]) az_offset = np.mean(az_data[0:offset_num]) gx_offset = np.mean(gx_data[0:offset_num]) gy_offset = np.mean(gy_data[0:offset_num]) gz_offset = np.mean(gz_data[0:offset_num]) fi_kf = Kalman(F=np.array([[1, 1], [0, 1]]), P = np.array([[10, 0], [0, 10]]), Q = np.array([[0.001, 0], [0, 0.001]]), R = np.array([[1000]]), H = np.array([[1, 0]]), x = np.array([[0], [0]]), u = np.array([[0], [0]])) theta_kf = Kalman(F=np.array([[1, 1], [0, 1]]), P = np.array([[1000, 0], [0, 1000]]), Q = np.array([[0.1, 0], [0, 0.1]]), R = np.array([[10]]), H = np.array([[1, 0]]), x = np.array([[0], [0]]), u = np.array([[0], [0]])) should_init_kf = True fi_raw_list = [] theta_raw_list = []
def kalmanSmooth(Y, pi0, sigma0, A, C, Q, R, nLF): """ Runs Kalman filtering and smoothing step of dynamic factor model estimator. Returns -nLF x T numpy array Smoothed means -T x nLF x nLF numpy array Smoothed covariances -T-1 x nLF x nLF numpy array Smoothed lagged covariances (ie, cov[x_t, x_t-1]) """ N, T = Y.shape # Initialize Kalman filter kf = Kalman(mu_0=pi0.copy(), sigma_0=sigma0.copy(), A=A, B=np.zeros(2*[2*nLF]), C=C, D=None, Q=Q, R=R) # sigma_t|t, mu_t|t sigma_filt = np.zeros([T, nLF, nLF]) sigma_filt[0] = sigma0.copy() mu_filt = np.zeros([T, nLF]) mu_filt[0] = pi0.copy() # sigma_t|t-1, mu_t|t-1. NOTE: indexed by t-1!!! sigma_pred = np.zeros([T-1, nLF, nLF]) mu_pred = np.zeros([T-1, nLF]) # Avoid printing repetitive errors #printedPosSemidefErr = False # Filtering step for t in range(1, T): kf.predict() # Save mu_t|t-1 and sigma_t|t-1 sigma_pred[t-1, :, :] = kf.sigma mu_pred[t-1] = kf.mu # Update if we have a measurement. Nans are handled by Kalman. kf.update(Y[:, t]) # Save filtered mean, covariance sigma_filt[t, :, :] = kf.sigma mu_filt[t] = kf.mu # Make sure filtered covariance is positive semidefinite! #eigs, _ = np.linalg.eig(sigma_filt[t]) #if len(np.where(eigs < 0)[0]) > 0 and not printedPosSemidefErr: # print "\tsigma_filt[%i] is not positive semidefinite"%t # printedPosSemidefErr = True # Make sure filtered covariance is symmetric (with REALLY loose tolerance...) #if np.allclose(sigma_filt[t], sigma_filt[t].T, rtol=1e-4) == False: # print "\tsigma_filt[%i] is not symmetric..."%t # sigma_t|T, mu_t|T sigma_smooth = np.zeros((T, nLF, nLF)) mu_smooth = np.zeros((T, nLF)) # Initialize: sigma_T|T = sigma_T|T(filtered) sigma_smooth[-1] = sigma_filt[-1] # mu_T|T = mu_T|T(filtered) mu_smooth[-1] = mu_filt[-1] # Lagged covariance. Indexed by t-1. sigmaLag_smooth = np.zeros((T-1, nLF, nLF)) # sigmaLag_{T,T-1} = (1 - K_T C) A V_{T-1|T-1}, where K_T is Kalman gain at last timestep. K_T = np.dot(sigma_pred[-1], np.dot(kf.C.T, np.linalg.pinv(np.dot(kf.C, \ np.dot(sigma_pred[-1], kf.C.T)) + kf.R))) sigmaLag_smooth[-1] = np.dot(np.dot((np.identity(nLF) - np.dot(K_T, kf.C)), kf.A), sigma_filt[-2]) # Backwards Kalman gain J = np.zeros((T-1, nLF, nLF)) # Smoothing step. Runs from t=T-1 to t=0. for t in range(T-2, -1, -1): # Backward Kalman gain matrix J[t] = np.dot(np.dot(sigma_filt[t], kf.A.T), np.linalg.pinv(sigma_pred[t])) # Smoothed mean mu_smooth[t] = mu_filt[t] + np.dot(J[t], mu_smooth[t+1] - mu_pred[t]) # Smoothed covariance. This is explicity symmetric. sigma_smooth[t, :, :] = sigma_filt[t] + np.dot(np.dot(J[t], sigma_smooth[t+1] - sigma_pred[t]), J[t].T) # Lagged smoothed covariance (NOT SYMMETRIC!). Pretty sure this is correct... for t in range(T-3, -1, -1): sigmaLag_smooth[t, :, :] = np.dot(sigma_filt[t+1], J[t].T) \ + np.dot(np.dot(J[t+1], (sigmaLag_smooth[t+1] - np.dot(kf.A, sigma_filt[t+1]))), J[t].T) # Fill in missing Y values YImp = Y.copy() nanRows, nanCols = np.where(np.isnan(YImp)) for s, t in zip(nanRows, nanCols): YImp[s, t] = np.dot(C[s, :], mu_smooth[t, :]) return mu_smooth.T, sigma_smooth, sigmaLag_smooth, YImp, sigma_filt
import matplotlib.pyplot as plt from kalman import Kalman # === define prior density === # Sigma = [[0.4, 0.3], [0.3, 0.45]] Sigma = np.array(Sigma) x_hat = np.array([0.2, -0.2]) # === define A, Q, G, R === # G = np.eye(2) R = 0.5 * Sigma A = np.eye(2) Q = np.zeros(2) # === initialize Kalman filter === # kn = Kalman(A, G, Q, R) kn.set_state(x_hat, Sigma) # === the true value of the state === # theta = np.zeros(2) + 4.0 # === generate plot === # T = 1000 z = np.empty(T) for t in range(T): # Measure the error y = multivariate_normal(mean=theta, cov=R) kn.update(y) z[t] = np.sqrt(np.sum((theta - kn.current_x_hat)**2)) fig, ax = plt.subplots()
class KAgent(object): """Class handles all command and control logic for a teams tanks.""" initialGridConstant = 0 def __init__(self, bzrc): self._kalman = Kalman() self.aliveTime = time.time() self.bzrc = bzrc self.constants = self.bzrc.get_constants() # print self.constants self.commands = [] self.potentialFields = [] self.updates = [] self.flag_sphere = 400 self.obstacle_sphere = 1000 self.enemy_sphere = 100 self.explore_sphere = 50 self.prev_x = {} self.prev_y = {} self.stuck_ticks = {} for x in xrange(20): self.prev_x[x] = 0 self.prev_y[x] = 0 self.stuck_ticks[x] = 0 self.path_fields = [] self.grid = Grid(int(self.constants['worldsize']), int(self.constants['worldsize']), self.initialGridConstant, self.initialGridConstant) def tick(self, time_diff): """Some time has passed; decide what to do next.""" # don't need to know where the flags or shots are when exploring. Enemies are included in the 'othertanks' call # pos,partialGrid = self.bzrc.get_occgrid() # self.grid.updateGrid(pos,partialGrid) self._kalman.setDT(time_diff) self.mytanks = self.bzrc.get_mytanks() # self.othertanks = self.bzrc.get_othertanks() targetTank = self.bzrc.get_othertanks()[0] self.commands = [] if targetTank.status == 'alive': self.lock_on(targetTank) else: self.aliveTime = time.time() stopMoving = Command(0, 0, 0, False) self.commands.append(stopMoving) self.bzrc.do_commands(self.commands) self._kalman.resetArrays() def lock_on(self, targetTank): print str(targetTank.x) agentTank = self.mytanks[0] Zt = array([[targetTank.x], [targetTank.y]]) self._kalman.updateKalmanFilter(Zt) est = self._kalman.H.dot(self._kalman.mu) self.updates.append( ((int(est[0][0]), int(est[1][0])), self._kalman.sigmaT)) aimAngle, distance = self.take_aim((agentTank.x, agentTank.y), agentTank.angle) command = Command(0, 0, aimAngle * 2, True) if aimAngle < 1 and aimAngle > -1: if distance < 350: command = Command(0, 0, aimAngle * 2, True) else: command = Command(0, 0, aimAngle * 2, False) else: command = Command(0, 0, aimAngle * 2, False) self.commands.append(command) self.bzrc.do_commands(self.commands) def take_aim(self, tankPosition, tankAngle): xPos = self._kalman.mu[0][0] xVel = self._kalman.mu[1][0] xAcc = self._kalman.mu[2][0] yPos = self._kalman.mu[3][0] yVel = self._kalman.mu[4][0] yAcc = self._kalman.mu[5][0] tankX, tankY = tankPosition velocity = math.sqrt(xVel**2 + yVel**2) acceleration = math.sqrt(xAcc**2 + yAcc**2) distance = self.dist(tankX, tankY, xPos, yPos) timeToEnemy = 0 r = (100 - velocity)**2 - 4 * (acceleration / 2) * distance * -1 plusRoot = (-(100 - velocity) + math.sqrt(r)) / acceleration minusRoot = (-(100 - velocity) - math.sqrt(r)) / acceleration if plusRoot > minusRoot: timeToEnemy = plusRoot else: timeToEnemy = minusRoot self._kalman.setDT(timeToEnemy) projectionMatrix = self._kalman.projectPosition() projectedX = projectionMatrix[0][0] projectedY = projectionMatrix[3][0] distance = self.dist(tankX, tankY, projectedX, projectedY) angel = math.atan2(projectedY - tankY, projectedX - tankX) return (self.normalize_angle(angel - tankAngle), distance) def doUpdate(self): # print len(self.updates) for update in self.updates: pos, partialGrid = update self.grid.updateGrid(pos, partialGrid) self.updates = [] def dist(self, x1, y1, x2, y2): dist_result = math.sqrt((float(x1) - float(x2))**2 + (float(y1) - float(y2))**2) #print dist_result return dist_result 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
from scipy.linalg import eigvals from kalman import Kalman # === Define A, Q, G, R === # G = np.eye(2) R = 0.5 * np.eye(2) A = [[0.5, 0.4], [0.6, 0.3]] Q = 0.3 * np.eye(2) # === Define the prior density === # Sigma = [[0.9, 0.3], [0.3, 0.9]] Sigma = np.array(Sigma) x_hat = np.array([8, 8]) # === Initialize the Kalman filter === # kn = Kalman(A, G, Q, R) kn.set_state(x_hat, Sigma) # === Set the true initial value of the state === # x = np.zeros(2) # == Print eigenvalues of A == # print("Eigenvalues of A:") print(eigvals(A)) # == Print stationary Sigma == # S, K = kn.stationary_values() print("Stationary prediction error variance:") print(S) # === Generate the plot === #
# Create a socket object soc = socket.socket(socket.AF_INET, socket.SOCK_STREAM) time.sleep(3) # arduino will reset on serial open # let it calibrate, has to be still # Initialize orientation vectors acc = [0.0, 0.0, 0.0] mag = [0.0, 0.0, 0.0] # Reset rpy reset_condition = True # Kalman objects kalman_x = Kalman() kalman_y = Kalman() kalman_z = Kalman() ##################################### # YOU NEED TO ADJUST THESE (TASK 4) # ##################################### # Set Kalman sensor noise stdevs #kalman_x.setSensorNoise(0.2, 0.03) # 0.2, 0.03 #kalman_y.setSensorNoise(0.2, 0.03) # 0.2, 0.03 #kalman_z.setSensorNoise(0.6, 0.03) # 0.6, 0.03 ##kalman_x.setSensorNoise(0.01, 0.01) ##kalman_y.setSensorNoise(0.01, 0.01)
figdistr, axis = pl.subplots(3, 1) figdistr.tight_layout() init_area = 5 s = 2 # Desired altitude and heading alt_d = 4 q1.yaw_d = -np.pi q2.yaw_d = np.pi/2 q3.yaw_d = 0 # Kalman filter for position estimation kf = Kalman() for t in time: # Simulation X = np.append(q1.xyz[0:2], np.append(q2.xyz[0:2], q3.xyz[0:2])) V = np.append(q1.v_ned[0:2], np.append(q2.v_ned[0:2], q3.v_ned[0:2])) U = fc.u_acc(X, V) q1.set_a_2D_alt_lya(U[0:2], -alt_d) q2.set_a_2D_alt_lya(U[2:4], -alt_d) q3.set_a_2D_alt_lya(U[4:6], -alt_d) # relative velocities! v2_relative = q2.v_ned - q1.v_ned v3_relative = q3.v_ned - q1.v_ned
def test(noise=20, time_max=10, show_acceleration_plots=True, filter_name='kalman'): import sys sys.path.append('../') import numpy as np import time # Record Data import time import numpy as np import pyautogui def record_data(noise=20, time_max=10): print("RECORDING DATA") print("Move your cursor as if you were driving it.") coords = [] coords_actual = [] timestamp = [] T = time.time() while time.time() - T < time_max: t = time.time() while True: if time.time() - t > .02: timestamp.append(time.time() - T) act = np.asarray(pyautogui.position()) pos = act + 2 * noise * np.asarray([ np.random.rand(), np.random.rand() ]) - noise * np.asarray( [np.random.rand(), np.random.rand()]) coords.append(pos) pos_actual = act coords_actual.append(pos_actual) break print("RECORDING DATA COMPLETED") return coords, coords_actual, timestamp coords, coords_actual, timestamp = record_data(noise=noise, time_max=time_max) ''' FLOW: INITIAL_STATE_VECTOR, INITIAL_ERROR_COVARIANCE_MATRIX PRIORI CALCULATION, PRIORI_ERROR_COVARIANCE OBSERVE_READINGS, EXPECTED_READINGS, EXPECTED_ERROR_COVARIANCE KALMAN_GAIN TAKES IN MEASUREMENT_NOISE, MEASUREMENT_MATRIX, NEW_ERROR_COVARIANCE POSTERIORI CALCULATION TAKES IN KALMAN_GAIN, OBSERVED_READINGS, PRIORI POSTERIORI_ERROR_COVARIANCE ''' ''' INPUT: cursor coords OUTPUT: priori, posteriori ''' # Dynamics # initial_state = [x_coord, y_coord, x_velocity, y_velocity, x_acceleration, y_acceleration] initial_state = np.asarray([coords[0][0], coords[0][1], 0, 0, 0, 0]).T initial_error_covariance_matrix = 0 * np.eye(6) dt = 0.02 transition_matrix = np.asarray([[1, 0, dt, 0, 0, 0], [0, 1, 0, dt, 0, 0], [0, 0, 1, 0, dt, 0], [0, 0, 0, 1, 0, dt], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]]) position_funciton = lambda position, velocity: position + velocity * dt velocity_funciton = lambda velocity, acceleration: velocity + acceleration * dt acceleration_function = lambda acceleration: acceleration def transition_function(state_vector, input_vector): # print('state_vector', state_vector.shape) position = position_funciton(state_vector[0:2], state_vector[2:4]) velocity = velocity_funciton(state_vector[2:4], state_vector[4:6]) acceleration = acceleration_function(state_vector[4:6]) return_state = np.vstack((position, velocity, acceleration)) return np.reshape(return_state, (-1, 1)) + np.zeros(6).T.reshape( state_vector.size, input_vector.shape[0]) * input_vector def measurement_function(state_vector): # print('state_vector', state_vector.shape) return np.reshape(state_vector[0:2], (-1, 1)) # print(nd.Jacobian(transition_function)(initial_state)) input_matrix = np.zeros(6).T input_vector = np.zeros((1, 1)) measurement_matrix = np.asarray([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]]) disturbance = 10 * np.eye(6) uncertainty = noise * np.eye(2) from kalman import Kalman from extended_kalman import ExtendedKalman kf = Kalman( initial_state=initial_state, initial_error_covariance_matrix=initial_error_covariance_matrix, transition_matrix=transition_matrix, measurement_matrix=measurement_matrix, input_matrix=input_matrix, input_vector=input_vector, measurement_noise=uncertainty, process_noise=disturbance) ekf = ExtendedKalman( initial_state=initial_state, initial_error_covariance_matrix=initial_error_covariance_matrix, transition_function=transition_function, measurement_function=measurement_function, input_matrix=input_matrix, input_vector=input_vector, measurement_noise=uncertainty, process_noise=disturbance) if filter_name == 'kalman': k = kf elif filter_name == 'extended': k = ekf # Visualization length = 1920 breadth = 1080 import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation fig, ax = plt.subplots() cursor_x, cursor_y = [], [] actual, = plt.plot([], [], '-', label='actual') xdata, ydata = [], [] recorded, = plt.plot([], [], '*', label='recorded') priori_x, priori_y = [], [] priori_plot, = plt.plot([], [], 'g-', label='priori_plot') posteriori_x, posteriori_y = [], [] posteriori_plot, = plt.plot([], [], 'r-', label='posteriori_plot') if show_acceleration_plots: fig_acc_x, acc_x = plt.subplots() priori_acceleration_x = [] posteriori_acceleration_x = [] priori_acceleration_plot_x, = plt.plot([], [], 'g-', label='priori_acc_x') posteriori_acceleration_plot_x, = plt.plot([], [], 'r-', label='posteriori_acc_x') fig_acc_y, acc_y = plt.subplots() priori_acceleration_y = [] posteriori_acceleration_y = [] priori_acceleration_plot_y, = plt.plot([], [], 'g-', label='priori_acc_y') posteriori_acceleration_plot_y, = plt.plot([], [], 'r-', label='posteriori_acc_y') timestamp_data = [] def init(): ax.set_xlim(0, length) ax.set_ylim(0, breadth) ax.legend() if show_acceleration_plots: acc_x.set_xlim(0, 10) acc_x.set_ylim(-1000, 1000) acc_x.legend() acc_y.set_xlim(0, 10) acc_y.set_ylim(-1000, 1000) acc_y.legend() return actual, recorded, priori_plot, posteriori_plot, priori_acceleration_plot_x, posteriori_acceleration_plot_x, priori_acceleration_plot_y, posteriori_acceleration_plot_y, else: return actual, recorded, priori_plot, posteriori_plot, def update(frame): n = frame # print(xdata[-1], ydata[-1]) # print(n - 1000, n) # print(np.asarray(coords[n - 100:n])[:, 0], 1080 - np.asarray(coords[n - 100:n])[:, 1]) pos_x = np.asarray(coords[n])[0] pos_y = breadth - np.asarray(coords[n])[1] # kpriori, kposteriori = kf(np.asarray([pos_x, pos_y]), None) # epriori, eposteriori = ekf(np.asarray([pos_x, pos_y]), None) priori, posteriori = k(np.asarray([pos_x, pos_y]), input_vector) priori_x.append(priori[0]) priori_y.append(priori[1]) priori_plot.set_data(priori_x, priori_y) xdata.append(pos_x) ydata.append(pos_y) recorded.set_data(xdata, ydata) posteriori_x.append(posteriori[0]) posteriori_y.append(posteriori[1]) posteriori_plot.set_data(posteriori_x, posteriori_y) cursor_x.append(np.asarray(coords_actual[n])[0]) cursor_y.append(breadth - np.asarray(coords_actual[n])[1]) actual.set_data(cursor_x, cursor_y) if show_acceleration_plots: timestamp_data.append(timestamp[n]) priori_acceleration_x.append(priori[4, 0]) priori_acceleration_plot_x.set_data(timestamp_data, priori_acceleration_x) posteriori_acceleration_x.append(posteriori[4, 0]) posteriori_acceleration_plot_x.set_data(timestamp_data, posteriori_acceleration_x) priori_acceleration_y.append(priori[5, 0]) priori_acceleration_plot_y.set_data(timestamp_data, priori_acceleration_y) posteriori_acceleration_y.append(posteriori[5, 0]) posteriori_acceleration_plot_y.set_data(timestamp_data, posteriori_acceleration_y) return actual, recorded, priori_plot, posteriori_plot, priori_acceleration_plot_x, posteriori_acceleration_plot_x, priori_acceleration_plot_y, posteriori_acceleration_plot_y, else: return actual, recorded, priori_plot, posteriori_plot, ani = FuncAnimation(fig, update, frames=np.arange(1, len(coords), 2), init_func=init, blit=True, interval=100) plt.show()
def main(): print('Program Started') sim.simxFinish(-1) clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if clientID != -1: print('Connected to remote API server') else: print('Failed connecting to remote API server') returnCode, leftMotor = sim.simxGetObjectHandle( clientID, "Pioneer_p3dx_leftMotor", sim.simx_opmode_oneshot_wait) returnCode, rightMotor = sim.simxGetObjectHandle( clientID, "Pioneer_p3dx_rightMotor", sim.simx_opmode_oneshot_wait) robot = Robot(clientID, 2) returnCode, robotHandle = sim.simxGetObjectHandle( clientID, "Pioneer_p3dx", sim.simx_opmode_oneshot_wait) returnCode, sensorData = sim.simxGetStringSignal(clientID, "scanRanges", sim.simx_opmode_streaming) l_rot_prev, r_rot_prev = 0, 0 # Initial state and initial covariance matrix prevPosition = np.matrix(getPosition(clientID, robotHandle)).T odPrevPos = np.matrix(getPosition(clientID, robotHandle)).T prevErrorPosition = np.zeros((3, 3)) odPrevError = np.zeros((3, 3)) a = 0 g = 0.3 count = 0 while sim.simxGetConnectionId(clientID) != -1: v = np.zeros((2, 1)) # speedMotors = robot.breit_controller(clientID) # sim.simxSetJointTargetVelocity(clientID, leftMotor, speedMotors[0], sim.simx_opmode_streaming) # sim.simxSetJointTargetVelocity(clientID, rightMotor, speedMotors[1], sim.simx_opmode_streaming) if count == 100: # Dead reckoning dPhiL, dPhiR, l_rot_prev, r_rot_prev = readOdometry( clientID, leftMotor, rightMotor, l_rot_prev, r_rot_prev) # Initialize Kalman kalman_filter = Kalman(dPhiL, dPhiR) odometry = Kalman(dPhiL, dPhiR) predPosition, predError = kalman_filter.prediction( prevPosition, prevErrorPosition) truePos = np.matrix(getPosition(clientID, robotHandle)).T odPosition, odError = odometry.prediction(odPrevPos, odPrevError) odPosition[2] = truePos[2] odPrevPos = odPosition odPrevError = odError # Observations observedFeatures = readObservations(clientID) x, y = lidar.arrangeData(observedFeatures) lidarInputs, nLidarInputs = lidar.split_and_merge(x, y) d_ant = 10000000000 S = np.zeros((2, 2)) H = np.zeros((2, 3)) predPosition[2] = truePos[2] for i in range(nLidarInputs): for j in range(len(mapInputs)): y, S, H = kalman_filter.getInnovation( predPosition, predError, mapInputs[j, :], lidarInputs[:, i]) d = y.T @ np.linalg.pinv(S) @ y if d < g**2 and d < d_ant: # print("=============== MATCH ================\n") v = y d_ant = d estPosition, estError, y, S = kalman_filter.update( predPosition, predError, v, S, H) prevPosition = estPosition prevErrorPosition = estError print( f'Odometry position (x) - Estimated position (x) = {float(odPosition[0] - estPosition[0])}\n' ) print( f'Odometry position (y) - Estimated position (y) = {float(odPosition[1] - estPosition[1])}\n' ) count = 0 count += 1
import numpy as np import matplotlib.pyplot as plt from kalman import Kalman from scipy.stats import norm ## Parameters theta = 10 A, G, Q, R = 1, 1, 0, 1 x_hat_0, Sigma_0 = 8, 1 ## Initialize Kalman filter kalman = Kalman(A, G, Q, R) kalman.set_state(x_hat_0, Sigma_0) N = 5 fig, ax = plt.subplots() xgrid = np.linspace(theta - 5, theta + 2, 200) for i in range(N): # Record the current predicted mean and variance, and plot their densities m, v = kalman.current_x_hat, kalman.current_Sigma m, v = float(m), float(v) ax.plot(xgrid, norm.pdf(xgrid, loc=m, scale=np.sqrt(v)), label=r'$t=%d$' % i) # Generate the noisy signal y = theta + norm.rvs(size=1) # Update the Kalman filter kalman.update(y) ax.set_title(r'First %d densities when $\theta = %.1f$' % (N, theta)) ax.legend(loc='upper left') plt.show()
# - Position and velocity states and estimates vs time # - Estimation error and error covariance vs time # - Kalman gains vs time if __name__ == "__main__": # UUV parameters x0 = np.zeros([1, 2]) # initial states duration = 50 # model test duration dt = 0.05 # time step R = np.array([[0.01, 0], [0, 0.0001]]) # process covariance Q = np.array([0.001]) # measurement covariance uuv = UUV(x0, duration, dt, R, Q) # UUV model object # Kalman Filter Init kalman = Kalman(uuv.A, uuv.B, uuv.C, uuv.R, uuv.Q) # plot data containers two_sig_v = np.zeros([int(duration / dt) + 1, 2]) # two-sigma velocity boundary two_sig_x = np.zeros([int(duration / dt) + 1, 2]) # two-sigma position boundary mu = np.zeros([int(duration / dt) + 1, 2]) # state estimation vector K = np.zeros([int(duration / dt) + 1, 2]) # Kalman gain vector # Input Command Simulation F = np.zeros([int(duration / dt)]) for i in range(int(duration / dt)): if i < int(5 / dt): F[i] = 500 elif i < int(25 / dt): F[i] = 0 elif i < int(30 / dt):
class BlueToothThreading: """ Bluetooth Threading The run() method will be started and it will run in the background until the application exits. """ def __init__(self, interval=1): """ Constructor :type interval: int :param interval: Check interval, in seconds """ self.sensorfusion = Kalman() print("Connecting to sensortag...") self.tag = SensorTag(macAddress) print("connected.") self.tag.accelerometer.enable() self.tag.magnetometer.enable() self.tag.gyroscope.enable() self.tag.battery.enable() self.pitch = 0 self.angular_velocity = 0 self.linear_velocity = 0 self.acceleration = 0 time.sleep(1.0) # Loading sensors self.prev_time = time.time() thread = threading.Thread(target=self.run, args=()) thread.daemon = True # Daemonize thread thread.start() # Start the execution def run(self): """ Method that runs forever """ while True: accelerometer_readings = self.tag.accelerometer.read() gyroscope_readings = self.tag.gyroscope.read() magnetometer_readings = self.tag.magnetometer.read() ax, ay, az = accelerometer_readings gx, gy, gz = gyroscope_readings mx, my, mz = magnetometer_readings curr_time = time.time() dt = curr_time - self.prev_time self.sensorfusion.computeAndUpdateRollPitchYaw( ax, ay, az, gx, gy, gz, mx, my, mz, dt) pitch = self.sensorfusion.pitch * 0.0174533 dp = pitch - self.pitch da = ax - self.acceleration self.angular_velocity = dp / dt self.linear_velocity = da * dt self.pitch = pitch self.acceleration = ax self.prev_time = curr_time print("Battery: ", self.tag.battery.read()) def take_observation(self): return [ self.pitch, self.angular_velocity, self.linear_velocity, self.acceleration ] # return observation with manual calbiration # expects list of length STATE_SIZE def take_observation_calibrated(self, calibration): return [ self.pitch - calibration[0], self.angular_velocity - calibration[1], self.linear_velocity - calibration[2], self.acceleration - calibration[3] ]
"la": AccelerationState("la"), "za": AccelerationState("za"), "p": PhiState("p"), "r": PhiState("r"), "headr": PhiState("headr"), "pr": ThetaState("pr", "p"), "rr": ThetaState("rr", "r"), "head": ThetaState("headr", "head"), } c = { "t": 1.0 } robot = Robot() k = Kalman(sc, c) imu = ImuSensor(k.state_keys) axis("equal") controls = [ (0.0, (2.0, 0.0)), (10.0, (0.0, 0.2)), (14.0, (0.0, 0.0)) ] controlIdx = 0 controlNext = controls[controlIdx][0] seed(123) t = 1.0 drift = 0.0
class Agent(object): """Class handles all command and control logic for a teams tanks.""" def __init__(self, bzrc): self.bzrc = bzrc self.constants = self.bzrc.get_constants() self.pos_noise = 5 self.commands = [] self.kalman = Kalman(self.pos_noise) self.max_tank_speed = float(self.constants['tankspeed']) self.linear_acceleration = float(self.constants['linearaccel']) self.shot_speed = float(self.constants['shotspeed']) mytanks, othertanks, flags, shots = self.bzrc.get_lots_o_stuff() enemies = [tank for tank in othertanks if tank.color != self.constants['team']] covariance_list = [[100, 0, 0, 0, 0, 0], [ 0, 0.1, 0, 0, 0, 0], [ 0, 0, 0.1, 0, 0, 0], [ 0, 0, 0, 100, 0, 0], [ 0, 0, 0, 0, 0.1, 0], [ 0, 0, 0, 0, 0, 0.1]] self.observed_states = [] self.states = [] self.state_covariances = [] for enemy in enemies: new_observed = numpy.matrix(numpy.zeros((6, 1))) new_observed.getA()[0][0] = enemy.x new_observed.getA()[3][0] = enemy.y self.observed_states.append(new_observed) new_state = numpy.matrix(numpy.zeros((6, 1))) self.states.append(new_state) new_covariance = numpy.matrix(covariance_list) self.state_covariances.append(new_covariance) # PD control self.old_angle = [0] * len(mytanks) # initialize plot plt.ion() self.fig = plt.figure() self.fignum = 0 self.graph = self.fig.add_subplot(111) worldsize = float(self.constants['worldsize']) worldlimit = worldsize / 2.0 self.graph.axis([-worldlimit, worldlimit, -worldlimit, worldlimit]) plt.show() def tick(self, time_diff): """Some time has passed; decide what to do next.""" mytanks, othertanks, flags, shots = self.bzrc.get_lots_o_stuff() self.mytanks = mytanks self.othertanks = othertanks self.enemies = [tank for tank in othertanks if tank.color != self.constants['team']] for index in xrange(len(self.enemies)): # don't run filter if enemy is dead if self.enemies[index].status != 'alive': continue # get the saved previous state previous_observed = self.observed_states[index] previous_state = self.states[index] # get the observed state at this time enemy = self.enemies[index] observed_velocity_x = self.approximateDerivative(previous_observed.getA()[0][0], enemy.x, time_diff, self.max_tank_speed) observed_velocity_y = self.approximateDerivative(previous_observed.getA()[3][0], enemy.y, time_diff, self.max_tank_speed) observed_acceleration_x = self.approximateDerivative(previous_observed.getA()[1][0], observed_velocity_x, time_diff, self.linear_acceleration) observed_acceleration_y = self.approximateDerivative(previous_observed.getA()[4][0], observed_velocity_y, time_diff, self.linear_acceleration) observed_state = numpy.matrix(numpy.zeros((6, 1))) observed_state.getA()[0][0] = enemy.x observed_state.getA()[1][0] = observed_velocity_x observed_state.getA()[2][0] = observed_acceleration_x observed_state.getA()[3][0] = enemy.y observed_state.getA()[4][0] = observed_velocity_y observed_state.getA()[5][0] = observed_acceleration_y self.observed_states[index] = observed_state # get the saved previous state covariance previous_covariance = self.state_covariances[index] predicted_state, updated_covariance = self.kalman.predictState( previous_state, observed_state, previous_covariance, time_diff ) x = numpy.arange(-400, 400) y = numpy.arange(-400, 400) X, Y = numpy.meshgrid(x, y) Z = mlab.bivariate_normal(X, Y, updated_covariance.getA()[0][0], updated_covariance.getA()[3][3], predicted_state.getA()[0][0], predicted_state.getA()[3][0], updated_covariance.getA()[0][3]) self.graph.clear() self.graph.plot([observed_state.getA()[0][0]], [observed_state.getA()[3][0]], 'ro') self.graph.contour(X, Y, Z) shots_x = [shot.x for shot in shots] shots_y = [shot.y for shot in shots] self.graph.plot(shots_x, shots_y, 'bo') self.states[index] = predicted_state self.state_covariances[index] = updated_covariance # create tank commands (shoot stuff) for tank in mytanks: if len(self.states) > 0: # get predicted enemy position first_enemy_index = 0 predicted_state = self.states[first_enemy_index] pred_x = predicted_state.getA()[0][0] pred_y = predicted_state.getA()[3][0] distance = math.sqrt((pred_x - tank.x) **2 + (pred_y - tank.y) **2) secondsTilCollision = distance / self.shot_speed future_state = self.kalman.predictFutureState(predicted_state, secondsTilCollision) fut_x = future_state.getA()[0][0] fut_y = future_state.getA()[3][0] # move self.create_command(tank, fut_x, fut_y, time_diff) results = self.bzrc.do_commands(self.commands) self.fig.canvas.draw() #if self.fignum < 20: # self.fig.savefig("plots/kalman_{0}.png".format(self.fignum), bbox_inches="tight") # self.fignum += 1 def approximateDerivative(self, p1, p2, time_diff, cap): difference = p2 - p1 derivative = difference / time_diff absDerivative = abs(derivative) if absDerivative > cap: derivative = cap * ( derivative / absDerivative ) return derivative def create_command(self, tank, target_x, target_y, time_diff): """Set command to move to given coordinates.""" angvel = 0 speed = 0 shoot = 0 if time_diff <= 0: return self.graph.plot([tank.x], [tank.y], 'gs') self.graph.plot([target_x], [target_y], 'go') # PD Controller - angle target_angle = math.atan2(target_y - tank.y, target_x - tank.x) angle_remaining = self.angle_remaining(tank.angle, target_angle) angvel = self.pd_angvel(tank, target_angle, time_diff) plot_line_length = 350 x_forward = plot_line_length * math.cos(tank.angle) + tank.x y_forward = plot_line_length * math.sin(tank.angle) + tank.y self.graph.plot([tank.x, x_forward], [tank.y, y_forward], color='r', linestyle='-', linewidth=1) if angvel > 1: angvel = 1 elif angvel < -1: angvel = -1 if abs(angle_remaining) < 0.01: shoot = 1 command = Command(tank.index, speed, angvel, shoot) self.commands.append(command) self.old_angle[tank.index] = tank.angle def pd_angvel(self, tank, target_angle, time_diff): """PD Controller for the angular velocity of the tank.""" kp = 1.0 kd = -0.2 angle_remaining = self.angle_remaining(tank.angle, target_angle) differential = self.angle_remaining(self.old_angle[tank.index], tank.angle) / time_diff angvel = ( kp * angle_remaining ) + ( kd * differential ) return angvel def angle_remaining(self, tank_angle, target_angle): """Find the angle remaining (in radians) between the tank and the target.""" tank_angle = self.normalize_angle(tank_angle) target_angle = self.normalize_angle(target_angle) # If the angles are on the same hemisphere, target - tank. if tank_angle * target_angle > 0: return target_angle - tank_angle # Otherwise they are on opposite hemispheres. positive_angle = max(tank_angle, target_angle) negative_angle = min(tank_angle, target_angle) tank_positive = True if tank_angle < 0: tank_positive = False # Compare the angles to turn right and left. right_angle = -1 * ( positive_angle - negative_angle ) left_angle = 2 * math.pi - positive_angle + negative_angle if not tank_positive: temp_angle = -1 * right_angle right_angle = -1 * left_angle left_angle = temp_angle # Pick the smallest angle. if abs(right_angle) <= abs(left_angle): return right_angle return left_angle 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