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 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)
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]))
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 kalman_estimate_1D(x,P,va_measurements,x_history): """ This function takes the filtered velocity, acceleration data and covariance matrix (from previous filtration), the filtered position history (for only a single dimension) and the measurements (for velocity and acceleration). And returns the updated velocity,acceleration data and covariance matrix and also updates the position history. It uses an Additive Unscented Kalman Filter and calls "kalman_estimate_v_a" in order to achieve this. The functions 'lambda x,w' and 'lambda x,v' are respectively the transition and observation functions (https://pykalman.github.io/#id3). # measurements for 1 dim in the form [v,a] # x_history is history of in format [x1,x2,x3,...] """ (x,P)=kalman_estimate_v_a(x,P,va_measurements) aukf = AdditiveUnscentedKalmanFilter(lambda x, w: x + w, lambda x, v: x + v, observation_covariance=1) x_history=x_history[-30:] + x[0]*dt + 0.5 * x[1] * dt * dt x_filtered,temp = aukf.filter(x_history) return (x,P,x_filtered)
def kalman_estimate_1D(x, P, va_measurements, x_history): """ This function takes the filtered velocity, acceleration data and covariance matrix (from previous filtration), the filtered position history (for only a single dimension) and the measurements (for velocity and acceleration). And returns the updated velocity,acceleration data and covariance matrix and also updates the position history. It uses an Additive Unscented Kalman Filter and calls "kalman_estimate_v_a" in order to achieve this. The functions 'lambda x,w' and 'lambda x,v' are respectively the transition and observation functions (https://pykalman.github.io/#id3). # measurements for 1 dim in the form [v,a] # x_history is history of in format [x1,x2,x3,...] """ (x, P) = kalman_estimate_v_a(x, P, va_measurements) aukf = AdditiveUnscentedKalmanFilter(lambda x, w: x + w, lambda x, v: x + v, observation_covariance=1) x_history = x_history[-30:] + x[0] * dt + 0.5 * x[1] * dt * dt x_filtered, temp = aukf.filter(x_history) return (x, P, x_filtered)
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]))
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)
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]))
# init stateMean = np.zeros((n + 1, 2)) stateCov = np.zeros((n + 1, 2, 2)) stateMean[0] = param['x0'] stateCov[0] = param['p0'] for i in range(n): (stateMeanPred, stateCovPred) = filter_predict(f, jf, Q, stateMean[i], stateCov[i]) (stateMean[i + 1], stateCov[i + 1]) = filter_update(h, jh, np.array([param['r']]), stateMeanPred, stateCovPred, observations[i]) plt.plot(stateMean[:, 0]) plt.plot(states[:, 0]) plt.plot(observations, '.') ## Unscented Kalman Filter # In[107]: ukf = AdditiveUnscentedKalmanFilter(f, h, Q, param['r'], param['x0'], param['p0']) stateMean = ukf.filter(observations)[0] plt.plot(stateMean[:, 0]) plt.plot(states[:, 0]) plt.plot(observations, '.')
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
def E_step(self): # print(self.g_coords.shape) # print(self.w_cov,self.v_cov) ukf = AdditiveUnscentedKalmanFilter(self.f_estimator, self.g_estimator, transition_covariance=self.w_cov, observation_covariance=self.v_cov) self.hidden_sequence = ukf.smooth(self.output_sequence)[0]
def runFilter(observations, params, dname, filterType): """ Run Kalman Filter (UKF/KF) and return smoothed means/covariances observations : nsample x T params : {'dname'... contains all the necessary parameters for KF/UKF} filterType : 'KF' or 'UKF' """ s1 = set(params[dname].keys()) s2 = set(['trans_fxn','obs_fxn','trans_cov','obs_cov','init_mu','init_cov', 'trans_mult','obs_mult','trans_drift','obs_drift','baseline']) for k in s2: assert k in s1,k+' not found in params' #assert s1.issubset(s2) and s1.issuperset(s2),'Missing in params: '+', '.join(list(s2.difference(s1))) assert filterType=='KF' or filterType=='UKF','Expecting KF/UKF' model,mean,var = None,None,None X = observations.squeeze() #assert len(X.shape)==2,'observations must be nsamples x T' if filterType=='KF': def setupArr(arr): if type(arr) is np.ndarray: return arr else: return np.array([arr]) model=KalmanFilter( transition_matrices = setupArr(params[dname]['trans_mult']), #multiplier for z_t-1 observation_matrices = setupArr(params[dname]['obs_mult']).T, #multiplier for z_t transition_covariance = setupArr(params[dname]['trans_cov_full']), #transition cov observation_covariance= setupArr(params[dname]['obs_cov_full']), #obs cov transition_offsets = setupArr(params[dname]['trans_drift']),#additive const. in trans observation_offsets = setupArr(params[dname]['obs_drift']), #additive const. in obs initial_state_mean = setupArr(params[dname]['init_mu']), initial_state_covariance = setupArr(params[dname]['init_cov_full'])) else: #In this case, the transition and emission function may have other parameters #Create wrapper functions that are instantiated w/ the true parameters #and pass them to the UKF def trans_fxn(z): return params[dname]['trans_fxn'](z, fxn_params = params[dname]['params']) def obs_fxn(z): return params[dname]['obs_fxn'](z, fxn_params = params[dname]['params']) model=AdditiveUnscentedKalmanFilter( transition_functions = trans_fxn, #params[dname]['trans_fxn'], observation_functions = obs_fxn, #params[dname]['obs_fxn'], transition_covariance = np.array([params[dname]['trans_cov']]), #transition cov observation_covariance= np.array([params[dname]['obs_cov']]), #obs cov initial_state_mean = np.array([params[dname]['init_mu']]), initial_state_covariance = np.array(params[dname]['init_cov'])) #Run smoothing algorithm with model dim_stoc = params[dname]['dim_stoc'] if dim_stoc>1: mus = np.zeros((X.shape[0],X.shape[1],dim_stoc)) cov = np.zeros((X.shape[0],X.shape[1],dim_stoc)) else: mus = np.zeros(X.shape) cov = np.zeros(X.shape) ll = 0 for n in range(X.shape[0]): (smoothed_state_means, smoothed_state_covariances) = model.smooth(X[n,:]) if dim_stoc>1: mus[n,:] = smoothed_state_means cov[n,:] = np.concatenate([np.diag(k)[None,:] for k in smoothed_state_covariances],axis=0) else: mus[n,:] = smoothed_state_means.ravel() cov[n,:] = smoothed_state_covariances.ravel() if filterType=='KF': ll += model.loglikelihood(X[n,:]) return mus,cov,ll
def main(argv): config = read_parser(argv, Inputs, InputsOpt_Defaults) if config['mode'] == 'normal': print('nothing') n = 500 x_pure = np.arange(n) * 0.1 x = x_pure + np.random.normal(scale=0.25, size=n) y = np.cos(x) # y = 2*x+ 6 kf = KalmanFilter(initial_state_mean=0, n_dim_obs=1, random_state=1) # measurements = [[x[i], y[i]] for i in range(int(n))] measurements = [[y[i]] for i in range(int(n / 2))] # measurements_full = [[y[i]] for i in range(int(n))] kf.em(measurements) # print(kf.initial_state_mean) # print(kf.initial_state_covariance) # print(type(kf.initial_state_mean)) # print(type(kf.initial_state_covariance)) # exit() z = kf.filter(measurements) means = z[0] covs = z[1] it_mean = means[len(means) - 1] it_cov = covs[len(covs) - 1] h = list(means) r = h # kf2 = KalmanFilter(initial_state_mean=it_mean, initial_state_covariance=it_cov, n_dim_obs=1) # kf2.em(measurements) f = list( kf.sample(int(n / 2), initial_state=it_mean, random_state=1)[0]) # f = h + list(np.array(f) + it_mean) f = h + f for i in range(int(n / 2)): it_z = kf.filter_update(filtered_state_mean=it_mean, filtered_state_covariance=it_cov, observation=[y[i + int(n / 2)]]) it_mean = it_z[0] it_cov = it_z[1] h.append(it_mean) plt.plot(x_pure, h, 'r') plt.plot(x_pure, y) plt.plot(x_pure, f, 'g') plt.show() elif config['mode'] == 'ukalman': print('Ukalman') n = 1000 x_pure = np.arange(n) * 0.1 x = x_pure + np.random.normal(scale=0.5, size=n) y = np.cos(x) # y = 2*x kf = AdditiveUnscentedKalmanFilter(n_dim_obs=1) measurements = [[y[i]] for i in range(int(n / 2))] kf.initial_state_mean = np.array([0.]) kf.initial_state_covariance = np.array([[0.1]]) it_mean = kf.initial_state_mean it_cov = kf.initial_state_covariance print(kf.initial_state_mean) print(kf.initial_state_covariance) # exit() h = [] for element in measurements: it_z = kf.filter_update(filtered_state_mean=it_mean, filtered_state_covariance=it_cov, observation=element) it_mean = it_z[0] it_cov = it_z[1] h.append(it_mean) f = list(kf.sample(int(n / 2), initial_state=it_mean)[0]) # f = h + list(np.array(f) + it_mean) f = h + f plt.plot(x_pure, y) plt.plot(x_pure, f, 'g') plt.show() return
from __future__ import division from pykalman import AdditiveUnscentedKalmanFilter import numpy as np import numpy.linalg as la def AFK(y): #Inputs: GPS coordinates; nx2 vector #Outputs: smoothed states; nx2 vector def TransObsFunc(state): #Same functions for simplified filter return state alpha = 0.8 #Tuning parameter for smoothness akf = AdditiveUnscentedKalmanFilter( TransObsFunc, TransObsFunc, np.eye(2)*alpha, np.eye(2), [y[0,0], y[0,1]], np.eye(2)) return akf.filter(y)[0]
n = int(len(observations)) # init stateMean = np.zeros((n+1, 2)) stateCov = np.zeros((n+1, 2, 2)) stateMean[0] = param['x0'] stateCov[0] = param['p0'] for i in range(n): (stateMeanPred, stateCovPred) = filter_predict(f, jf, Q, stateMean[i], stateCov[i]) (stateMean[i+1], stateCov[i+1]) = filter_update(h, jh, np.array([param['r']]), stateMeanPred, stateCovPred, observations[i]) plt.plot(stateMean[:,0]) plt.plot(states[:,0]) plt.plot(observations, '.') ## Unscented Kalman Filter # In[107]: ukf = AdditiveUnscentedKalmanFilter(f, h, Q, param['r'], param['x0'], param['p0']) stateMean = ukf.filter(observations)[0] plt.plot(stateMean[:,0]) plt.plot(states[:,0]) plt.plot(observations, '.')
random_state = np.random.RandomState(0) observation_covariance = np.eye(2) + random_state.randn(2, 2) * 0.1 initial_state_mean = [0, 0] initial_state_covariance = [[1, 0.1], [0.1, 1]] # sample from model ukf = UnscentedKalmanFilter(transition_function, observation_function, transition_covariance, observation_covariance, initial_state_mean, initial_state_covariance, random_state=random_state) akf = AdditiveUnscentedKalmanFilter(additive_transition_function, additive_observation_function, transition_covariance, observation_covariance, initial_state_mean, initial_state_covariance) states, observations = ukf.sample(50, initial_state_mean) # estimate state with filtering ukf_state_estimates = ukf.filter(observations)[0] akf_state_estimates = akf.filter(observations)[0] # draw estimates pl.figure() lines_true = pl.plot(states, color='b') lines_ukf = pl.plot(ukf_state_estimates, color='r') lines_akf = pl.plot(akf_state_estimates, color='g') pl.legend((lines_true[0], lines_ukf[0], lines_akf[0]), ('true', 'UKF', 'AddUKF'),
transition_covariance = np.eye(2) random_state = np.random.RandomState(0) observation_covariance = np.eye(2) + random_state.randn(2, 2) * 0.1 initial_state_mean = [0, 0] initial_state_covariance = [[1, 0.1], [-0.1, 1]] # sample from model ukf = UnscentedKalmanFilter( transition_function, observation_function, transition_covariance, observation_covariance, initial_state_mean, initial_state_covariance, random_state=random_state ) akf = AdditiveUnscentedKalmanFilter( additive_transition_function, additive_observation_function, transition_covariance, observation_covariance, initial_state_mean, initial_state_covariance ) states, observations = ukf.sample(50, initial_state_mean) # estimate state with filtering ukf_state_estimates = ukf.filter(observations)[0] akf_state_estimates = akf.filter(observations)[0] # draw estimates pl.figure() lines_true = pl.plot(states, color='b') lines_ukf = pl.plot(ukf_state_estimates, color='r') lines_akf = pl.plot(akf_state_estimates, color='g') pl.legend((lines_true[0], lines_ukf[0], lines_akf[0]), ('true', 'UKF', 'AddUKF'),
from pykalman import AdditiveUnscentedKalmanFilter 'Initialize Parameters' def transition_function(X): ... def observation_function(X): ... transition_covariance = np.eye(2) observation_covariance = np.eye(2) + something initial_state_mean = [0, 0] initial_state_covariance = [[1, 0.1], [0.1, 1]] akf = AdditiveUnscentedKalmanFilter(transition_function,observation_function, \ transition_covariance,observation_covariance,initial_state_mean, \ initial_state_covariance) akf_state_estimates = akf.filter(timesteps, states) pl.figure() lines_true = pl.plot(states, color='b') lines_akf = pl.plot(akf_state_estimates, color='g', ls='-.') pl.show()
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