class IMULocation: def __init__(self): self.N = 10 self.M = 3 self.x = zeros((1,self.N))[0] self.R = eye(3)*0.02 self.P = Q self.time = -1 self.ukfinit() self.state_equation = copy.deepcopy(state_equation) def ukfinit(self): self.ukf = AdditiveUnscentedKalmanFilter(n_dim_obs = self.M, n_dim_state = self.N, transition_functions = self.transition_function, observation_functions = self.observation_function, transition_covariance = Q, observation_covariance = self.R, initial_state_mean = self.x, initial_state_covariance = Q) def locate(self, state, state_cov, delt_time, euler_angle, linear_acc, angular_rate): self.delt_time = delt_time self.u = tuple((hstack((linear_acc, angular_rate)))) (self.x, self.P) = self.ukf.filter_update(state, state_cov, euler_angle) print linalg.norm(euler_angle-self.x[3:6]) return (self.x, self.P) def transition_function(self, state): return odeint(self.state_equation, state, [0, self.delt_time], tuple([self.u]))[1] def observation_function(self, state): return hstack((state[3:6]))
class UWBLocation: def __init__(self): self.N = 10 self.M = 4 self.x = zeros((1, self.N))[0] self.R = eye(4) * 0.0001 self.R[1:4, 1:4] = eye(3) * 0.000001 self.P = Q self.time = -1 self.ukfinit() self.state_equation = copy.deepcopy(state_equation) self.u = tuple([[0, 0, -g, 0, 0, 0]]) def ukfinit(self): self.ukf = AdditiveUnscentedKalmanFilter( n_dim_obs=self.M, n_dim_state=self.N, transition_functions=self.transition_function, observation_functions=self.observation_function, transition_covariance=Q, observation_covariance=self.R, initial_state_mean=self.x, initial_state_covariance=Q) def locate(self, state, state_cov, delt_time, anchor_dis, anchor_pos, euler_angle, linear_acc, angular_rate): self.anchor_pos = anchor_pos self.delt_time = delt_time # [phi, theta, psi] = [state[3], state[4], state[5]] # [cp, sp, ct, st, cs, ss] = [cos(phi), sin(phi), cos(theta), # sin(theta), cos(psi), sin(psi)] #self.u = tuple([[g * st, -g * ct * sp, -g * ct * cp,0,0,0]]) self.u = tuple([hstack((linear_acc, angular_rate))]) #print self.u (self.x, self.P) = self.ukf.filter_update(state, state_cov, hstack((anchor_dis, euler_angle))) return (self.x, self.P) def transition_function(self, state): return odeint(self.state_equation, state, [0, self.delt_time], self.u)[1] def observation_function(self, state): return hstack((linalg.norm(state[0:3] - self.anchor_pos), state[3:6]))
class UWBLocation: def __init__(self): self.N = 10 self.M = 4 self.x = zeros((1,self.N))[0] self.R = eye(4)*0.0001 self.R[1:4,1:4] = eye(3)*0.000001 self.P = Q self.time = -1 self.ukfinit() self.state_equation = copy.deepcopy(state_equation) self.u = tuple([[0,0,-g,0,0,0]]) def ukfinit(self): self.ukf = AdditiveUnscentedKalmanFilter(n_dim_obs = self.M, n_dim_state = self.N, transition_functions = self.transition_function, observation_functions = self.observation_function, transition_covariance = Q, observation_covariance = self.R, initial_state_mean = self.x, initial_state_covariance = Q) def locate(self, state, state_cov, delt_time, anchor_dis, anchor_pos, euler_angle, linear_acc, angular_rate): self.anchor_pos = anchor_pos self.delt_time = delt_time # [phi, theta, psi] = [state[3], state[4], state[5]] # [cp, sp, ct, st, cs, ss] = [cos(phi), sin(phi), cos(theta), # sin(theta), cos(psi), sin(psi)] #self.u = tuple([[g * st, -g * ct * sp, -g * ct * cp,0,0,0]]) self.u = tuple([hstack((linear_acc, angular_rate))]) #print self.u (self.x, self.P) = self.ukf.filter_update(state, state_cov, hstack((anchor_dis, euler_angle))) return (self.x, self.P) def transition_function(self, state): return odeint(self.state_equation, state, [0, self.delt_time], self.u)[1] def observation_function(self, state): return hstack((linalg.norm(state[0:3] - self.anchor_pos), state[3:6]))
class IMULocation: def __init__(self): self.N = 10 self.M = 3 self.x = zeros((1, self.N))[0] self.R = eye(3) * 0.02 self.P = Q self.time = -1 self.ukfinit() self.state_equation = copy.deepcopy(state_equation) def ukfinit(self): self.ukf = AdditiveUnscentedKalmanFilter( n_dim_obs=self.M, n_dim_state=self.N, transition_functions=self.transition_function, observation_functions=self.observation_function, transition_covariance=Q, observation_covariance=self.R, initial_state_mean=self.x, initial_state_covariance=Q) def locate(self, state, state_cov, delt_time, euler_angle, linear_acc, angular_rate): self.delt_time = delt_time self.u = tuple((hstack((linear_acc, angular_rate)))) (self.x, self.P) = self.ukf.filter_update(state, state_cov, euler_angle) print linalg.norm(euler_angle - self.x[3:6]) return (self.x, self.P) def transition_function(self, state): return odeint(self.state_equation, state, [0, self.delt_time], tuple([self.u]))[1] def observation_function(self, state): return hstack((state[3:6]))
class FusionUKF: n_state_dims = 11 n_lidar_obs_dims = 3 n_radar_obs_dims = 4 state_var_map = { 'x': 0, 'vx': 1, 'ax': 2, 'y': 3, 'vy': 4, 'ay': 5, 'z': 6, 'vz': 7, 'az': 8, 'yaw': 9, 'vyaw': 10 } OK = 0 UNRELIABLE_OBSERVATION = 1 NOT_INITED = 2 RESETTED = 3 def __init__(self, object_radius): self.initial_state_covariance = FusionUKF.create_initial_state_covariance( ) self.radar_observation_function = FusionUKF.create_radar_observation_function( ) self.lidar_observation_covariance = FusionUKF.create_lidar_observation_covariance( ) self.radar_observation_covariance = FusionUKF.create_radar_observation_covariance( object_radius) self.object_radius = object_radius # how much noisy observations to reject before resetting the filter self.reject_max = 2 # radar position measurements are coarse at close distance # discard radar observations closer than this self.min_radar_radius = 0. self.max_timejump = 1e9 self.reset() def set_min_radar_radius(self, min_radius): self.min_radar_radius = min_radius def set_max_timejump(self, max_timejump): self.max_timejump = max_timejump @staticmethod def create_initial_state_covariance(): # converges really fast, so don't tweak too carefully return np.diag([ 2., # x 0.8, # x' 0.8, # x'' 2., # y 0.8, # y' 0.8, # y'' 10., # z 10., # z' 10., # z'' 10., # alpha 10. # alpha ]) @staticmethod def create_transition_function(dt): dt2 = 0.5 * dt**2 return lambda s: [ s[0] + s[1] * dt + s[2] * dt2, s[1] + s[2] * dt, s[2], s[3] + s[4] * dt + s[5] * dt2, s[4] + s[5] * dt, s[5], # let's don't track z velocity and acceleration, because it's unreliable # s[6] + s[7] * dt + s[8] * dt2, s[6], s[7], s[8], s[9], s[10] ] @staticmethod def lidar_observation_function(s): m = FusionUKF.state_var_map return [s[m['x']], s[m['y']], s[m['z']], s[m['yaw']]] @staticmethod def create_radar_observation_function(): m = FusionUKF.state_var_map return lambda s: [s[m['x']], s[m['vx']], s[m['y']], s[m['vy']]] @staticmethod def create_transition_covariance(): return np.diag([ 1e-2, # x 1e-2, # vx 1e-2, # ax 1e-2, # y 1e-2, # vy 1e-2, # ay 1e-5, # z 1e-5, # vz 1e-5, # az 1e-1, # yaw 1e-3 # vyaw ]) @staticmethod def create_lidar_observation_covariance(): return np.diag([ 0.05, # x 0.05, # y 0.05, # z 0.001 # yaw ]) @staticmethod def create_radar_observation_covariance(object_radius): cov_x, cov_vx, cov_y, cov_vy = FusionUKF.calc_radar_covariances( object_radius) print cov_x, cov_vx, cov_y, cov_vy return np.diag([cov_x, cov_vx, cov_y, cov_vy]) @staticmethod def calc_radar_covariances(object_radius): # object_radius = radius of the circumscribing circle of the tracked object sigma_xy = object_radius / 1. cov_xy = sigma_xy**2 # jitter derived from http://www.araa.asn.au/acra/acra2015/papers/pap167.pdf jitter_v = 0.12 sigma_v = jitter_v / 10. cov_v = sigma_v**2 return cov_xy, cov_v, cov_xy, cov_v def obs_as_kf_obs(self, obs): if isinstance(obs, RadarObservation): return [obs.x, obs.vx, obs.y, obs.vy] elif isinstance(obs, LidarObservation): return [obs.x, obs.y, obs.z, obs.yaw] elif isinstance(obs, EmptyObservation): return None else: raise ValueError @staticmethod def obs_as_state(obs): if isinstance(obs, RadarObservation): z = -0.8 # radar doesn't measure Z-coord, so we need an initial estimation of Z. return [obs.x, 0., 0., obs.y, 0., 0., z, 0., 0., 0., 0.] elif isinstance(obs, LidarObservation): return [obs.x, 0., 0., obs.y, 0., 0., obs.z, 0., 0., obs.yaw, 0.] else: raise ValueError def looks_like_noise(self, obs): if not self.initialized or isinstance(obs, EmptyObservation): return state_mean = self.last_state_mean state_deviation = np.sqrt(np.diag(self.last_state_covar)) mul = 10. deviation_threshold = mul * state_deviation oas = np.array(self.obs_as_state(obs)) oas_deviation = np.abs(oas - state_mean) reject_mask = oas_deviation > deviation_threshold m = self.state_var_map bad_x = reject_mask[m['x']] bad_y = reject_mask[m['y']] bad_z = reject_mask[m['z']] who_bad = '' if bad_x: who_bad += 'x' elif bad_y: who_bad += 'y' elif bad_z: who_bad += 'z' return who_bad if who_bad != '' else None def check_covar(self): if not self.initialized: return state_deviation = np.sqrt(np.diag(self.last_state_covar)) mul = 1.5 deviation_threshold = mul * np.diag(self.initial_state_covariance) mask = state_deviation > deviation_threshold nz = np.nonzero(mask)[0] var_name = [ 'x', 'vx', 'ax', 'y', 'vy', 'ay', 'z', 'vz', 'az', 'yaw', 'vyaw' ] for i in nz: print 'Fusion: {} exceeded deviation ({}>{})'.format( var_name[i], state_deviation[i], deviation_threshold[i]) #print mask return mask.sum() == 0 # m = self.state_var_map # bad_x = mask[m['x']] # bad_y = mask[m['y']] # bad_z = mask[m['z']] # if bad_x: # return 'x', state_deviation[m['x']] # elif bad_y: # return 'y', state_deviation[m['y']] # elif bad_z: # return 'z', state_deviation[m['z']] #return None def reset(self): self.kf = AdditiveUnscentedKalmanFilter(n_dim_state=self.n_state_dims) self.obs_yaw_lin = YawLinear() self.last_state_mean = None self.last_state_covar = None self.last_obs = None self.initialized = False self.reject_count = 0 def filter(self, obs): if not self.initialized and isinstance(obs, EmptyObservation): return self.NOT_INITED if isinstance( obs, RadarObservation) and obs.radius() < self.min_radar_radius: #print 'rejecting radar observation because its too close' return self.UNRELIABLE_OBSERVATION #print obs # we need initial estimation to feed it to filter_update() if not self.initialized: if not self.last_obs: # need two observations to get a filtered state self.last_obs = obs return self.NOT_INITED last_state_mean = self.obs_as_state(self.last_obs) last_state_covar = self.initial_state_covariance else: last_state_mean = self.last_state_mean last_state_covar = self.last_state_covar dt = obs.timestamp - self.last_obs.timestamp if np.abs(dt) > self.max_timejump: print 'Fusion: {}s time jump detected, allowed is {}s. Resetting.'.format( dt, self.max_timejump) self.reset() return self.RESETTED who_is_bad = self.looks_like_noise(obs) if who_is_bad is not None: print 'Fusion: rejected noisy observation (bad {}): {}'.format( who_is_bad, obs) self.reject_count += 1 if self.reject_count > self.reject_max: print 'Fusion: resetting filter because too much noise' self.reset() return self.RESETTED return self.UNRELIABLE_OBSERVATION if isinstance(obs, LidarObservation): transition_function = self.create_transition_function(dt) transition_covariance = self.create_transition_covariance() observation_function = self.lidar_observation_function observation_covariance = self.lidar_observation_covariance elif isinstance(obs, RadarObservation): transition_function = self.create_transition_function(dt) transition_covariance = self.create_transition_covariance() observation_function = self.radar_observation_function observation_covariance = self.radar_observation_covariance else: # EmptyObservation transition_function = self.create_transition_function(dt) transition_covariance = self.create_transition_covariance() observation_function = None observation_covariance = None try: self.last_state_mean, self.last_state_covar =\ self.kf.filter_update( last_state_mean, last_state_covar, self.obs_as_kf_obs(obs), transition_function, transition_covariance, observation_function, observation_covariance) except: print 'Fusion: ====== WARNING! ====== filter_update() failed!' #bad_covar = self.check_covar() #if bad_covar is not None: #print 'Fusion: ({}) resetting filter because too high deviation in {}={}'.format(obs.timestamp, bad_covar[0], bad_covar[1]) if self.check_covar() == False: print 'Fusion: resetting filter because too high deviation' self.reset() return self.RESETTED self.last_obs = obs self.initialized = True return self.OK
class Kalman: """ Handles a Kalman filter for computing drone and transmitter locations. """ # Our initial uncertainty of our drone positions. GPS is not terribly # innacurate, so there isn't a ton of uncertainty here. DRONE_POSITION_UNCERTAINTY = 0.001 # Out initial uncertainly of our drone velocities. This is reasonably accurate # as well. DRONE_VELOCITY_UNCERTAINTY = 0.05 # Our initial uncertainty on our LOBs. # TODO(danielp): I have zero idea of what this should be. Figure that out. LOB_UNCERTAINTY = np.radians(5) # The indices of various elements in the state. _POS_X = 0 _POS_Y = 1 _VEL_X = 2 _VEL_Y = 3 # The index of the first LOB. _LOB = 4 # Indices of x and y in coordinate tuples. _X = 0 _Y = 1 def __init__(self, position, velocity): """ Args: position: Where we are. (X, Y) velocity: How fast we're going. (X, Y) """ # The transmitter x and y position should technically never change, so it's # not part of our state. self.__transmitter_positions = [] # We need an x and y position, an x and y velocity. (LOBs go after that.) self.__state = np.array([position[self._X], position[self._Y], velocity[self._X], velocity[self._Y]]) logger.debug("Initial state: %s" % (self.state)) self.__state_size = self.__state.shape[0] # Default observations. self.__observations = np.copy(self.__state) # Build a covariance matrix. We will use our initial uncertainty constants, # and assume that nothing in the state is correlated with anything else. self.__state_covariances = np.zeros((self.__state_size, self.__state_size)) # Calculate our diagonal fill, which will repeat, since the state repeats # for each drone. diagonal_fill = [self.DRONE_POSITION_UNCERTAINTY, self.DRONE_POSITION_UNCERTAINTY, self.DRONE_VELOCITY_UNCERTAINTY, self.DRONE_VELOCITY_UNCERTAINTY] diagonal_indices = np.diag_indices(self.__state_size) self.__state_covariances[diagonal_indices] = diagonal_fill logger.debug("Initializing state covariances: %s" % \ (self.__state_covariances)) # Since our initial state is directly what's measured from our sensors, it # seems logical that the initial observation covariances would be about what # our initial state covariances are. self.__observation_covariances = np.copy(self.__state_covariances) # Initialize the Kalman filter. self.__kalman = \ AdditiveUnscentedKalmanFilter( \ transition_functions=self.__transition_function, observation_functions=self.__observation_function) def __transition_function(self, current_state): """ Transition function. Tells us how to get the next state from the current state. Args: current_state: The current state. Returns: The next state. """ new_state = np.copy(current_state) # Updating the position is easy, we just add the velocity. new_state[self._POS_X] += current_state[self._VEL_X] new_state[self._POS_Y] += current_state[self._VEL_Y] for i in range(0, len(self.__transmitter_positions)): position = self.__transmitter_positions[i] # We can calculate our LOB too, based on our position. new_state[self._LOB + i] = np.arctan2(position[self._X] - current_state[self._POS_X], position[self._Y] - current_state[self._POS_Y]) # We use the velocity vector to gauge the drone's heading, and correct the # LOB accordingly. heading_correction = np.arctan2(current_state[self._VEL_X], current_state[self._VEL_Y]) new_state[self._LOB + i] -= heading_correction logger.debug("New state prediction: %s" % (new_state)) return new_state def __observation_function(self, current_state): """ Observation function. Tells us what our sensor readings should look like if our state estimate were correct and our sensors were perfect. Returns: The sensor readings we would expect. """ # We basically measure our state directly, so this isn't all that # interesting. return current_state def set_observations(self, position, velocity, *args): """ Sets what our observations are. Args: position: Where the GPS thinks we are. (X, Y) velocity: How fast we think we're going. (X, Y) Additional arguments are the LOBs on any transmitters we are tracking. """ observations = [position[self._X], position[self._Y], velocity[self._X], velocity[self._Y]] expecting_lobs = self.__observations.shape[0] - len(observations) if len(args) != expecting_lobs: raise ValueError("Expected %d LOBs, got %d." % (expecting_lobs, len(args))) observations.extend(args) self.__observations = np.array(observations) logger.debug("Setting new observations: %s" % (self.__observations)) def update(self): """ Updates the filter for one iteration. """ logger.info("Updating kalman filter.") output = self.__kalman.filter_update(self.__state, self.__state_covariances, observation=self.__observations, observation_covariance= self.__observation_covariances) self.__state, self.__state_covariances = output logger.debug("New state: %s, New state covariance: %s" % \ (self.__state, self.__state_covariances)) def state(self): """ Returns: The current state. """ return self.__state def state_covariances(self): """ Returns: The current state covariances. """ return self.__state_covariances def add_transmitter(self, lob, location): """ Adds a new transmitter for us to track. Args: lob: Our LOB to the transmitter. location: Where we think that the transmitter is located. """ self.__transmitter_positions.append(location) # Add the LOB to the state. self.__state = np.append(self.__state, lob) new_state_size = self.__state.shape[0] # Make sure the observations vector is the proper size. self.__observations = np.append(self.__observations, lob) # Now resize everything else to make it fit. Start by adding an extra row # and column of zeros to the state covariance matrix. new_state_cov = _expand_matrix(self.__state_covariances) # Add in a new default value for the variance of the new LOB. new_state_cov[new_state_size - 1, new_state_size - 1] = \ self.LOB_UNCERTAINTY self.__state_covariances = new_state_cov self.__state_size = new_state_size logger.debug("New state size: %d" % (self.__state_size)) logger.debug("New state covariances: %s" % (self.__state_covariances)) # Do the same thing for the observation covariances. new_observation_cov = _expand_matrix(self.__observation_covariances) new_observation_cov[new_state_size - 1, new_state_size - 1] = \ self.LOB_UNCERTAINTY self.__observation_covariances = new_observation_cov logger.debug("New observation covariances: %s" % \ (self.__observation_covariances)) def position_error_ellipse(self, stddevs): """ Gets a confidence error ellipse for our drone position measurement. Args: stddevs: How many standard deviations we want the ellipse to encompass. Returns: The width, the height, and the angle to the x axis of the ellipse, (radians) in a tuple in that order. """ # Take the subset of the covariance matrix that pertains to our position. position_covariance = self.__state_covariances[:2, :2] # Calculate its eigenvalues, and sort them. eigenvalues, eigenvectors = np.linalg.eigh(position_covariance) order = eigenvalues.argsort()[::-1] eigenvalues = eigenvalues[order] eigenvectors = eigenvectors[:, order] # Ellipse parameters. angle = np.arctan2(*eigenvectors[:, 0][::-1]) width, height = 2 * stddevs * np.sqrt(eigenvalues) logger.debug("Position error: width: %f, height: %f, angle: %f" % \ (width, height, angle)) return (width, height, angle) def lob_confidence_intervals(self, stddevs): """ Gets confidence intervals for our LOBs. Args: stddevs: How many standard deviations we want the interval to encompass. Returns: A list of the margin of errors for each LOB measurement. """ # Get the indices of the LOB covariances. indices = np.diag_indices(self.__state_size) # The first four are the position and velocity variances. indices = (indices[0][self._LOB:], indices[1][self._LOB:]) if not indices[0]: # We're not tracking any transmitters: return [] # Get the variances. variances = self.__state_covariances[indices] omega = np.sqrt(variances) margins_of_error = stddevs * omega logger.debug("Margins of error for LOBs: %s" % (margins_of_error)) return margins_of_error def transmitter_error_region(self, stddevs): """ Calculates error ellipses for all the transmitter positions. It does this by looking at the worst-case scenario for both the error on the drone position and the error on the LOB. Returns: A list of data for each transmitter. Each item in the list is itself a list of 8 points that define the error region for that transmitter. This region is roughly fan shaped, and the points are in clockwise order, starting from the bottom left corner. """ # Basically, we're taking every point on the ellipse and projecting it # through the LOB to reach a new error region, which is sort of fan-shaped. # Start by finding both the error regions for our position and lobs. ellipse_width, ellipse_height, ellipse_angle = \ self.position_error_ellipse(stddevs) # Turn the error ellipse into a set of points. center = (self.__state[self._POS_X], self.__state[self._POS_Y]) spread_x = ellipse_width / 2.0 spread_y = ellipse_height / 2.0 low_x = (center[self._X] - spread_x * np.cos(ellipse_angle), center[self._Y] - spread_x * np.sin(ellipse_angle)) high_x = (center[self._X] + spread_x * np.cos(ellipse_angle), center[self._Y] + spread_x * np.sin(ellipse_angle)) low_y = (center[self._X] - spread_y * np.sin(ellipse_angle), center[self._Y] - spread_y * np.cos(ellipse_angle)) high_y = (center[self._X] + spread_y * np.sin(ellipse_angle), center[self._Y] + spread_y * np.cos(ellipse_angle)) lob_errors = self.lob_confidence_intervals(stddevs) lobs = self.__state[self._LOB:] output = [] for i in range(0, len(lobs)): lob = lobs[i] lob_error = lob_errors[i] transmitter_position = self.__transmitter_positions[i] lob_low = lob - lob_error lob_high = lob + lob_error # Figure out what the transmitter position would be for each of these # scenarios. radius = np.sqrt((transmitter_position[self._X] - center[self._X]) ** 2 + \ (transmitter_position[self._Y] - center[self._Y]) ** 2) transmitter_low = (radius * np.cos(lob_low), radius * np.sin(lob_low)) transmitter_high = (radius * np.cos(lob_high), radius * np.sin(lob_high)) # Calculate points on the error ellipse when centered about all three of # these positions. recenter_vector_low = (transmitter_low[self._X] - center[self._X], transmitter_low[self._Y] - center[self._Y]) recenter_vector_mean = (transmitter_position[self._X] - center[self._X], transmitter_position[self._Y] - center[self._Y]) recenter_vector_high = (transmitter_high[self._X] - center[self._X], transmitter_high[self._Y] - center[self._Y]) bottom_left = map(add, low_y, recenter_vector_low) left_middle = map(add, low_x, recenter_vector_low) top_left = map(add, high_y, recenter_vector_low) top_middle = map(add, high_y, recenter_vector_mean) top_right = map(add, high_y, recenter_vector_high) right_middle = map(add, high_x, recenter_vector_high) bottom_right = map(add, low_y, recenter_vector_high) bottom_middle = map(add, low_y, recenter_vector_mean) # These points define our error region. error_region = [bottom_left, left_middle, top_left, top_middle, top_right, right_middle, bottom_right, bottom_middle] logger.debug("Error region for transmitter at %s: %s" % \ (transmitter_position, error_region)) output.append(error_region) return output