def run_kal(measurements, training_size=40): # count the number of measurements num_measurements = len(measurements) # find the dimension of each row dim = len(measurements[0]) if dim == 3: simple_mode = True elif dim == 9: simple_mode = False else: print "Error: Dimensions for run_kal must be 3 or 9" exit(1) training_set = [] for i in range(min(training_size,len(measurements))): training_set.append(measurements[i]) if simple_mode: # run the filter kf = KalmanFilter(initial_state_mean=[0,0,0], n_dim_obs=3) (smoothed_state_means, smoothed_state_covariances) = kf.em(training_set).smooth(measurements) else: kf = KalmanFilter(initial_state_mean=[0,0,0,0,0,0,0,0,0], n_dim_obs=9) (smoothed_state_means, smoothed_state_covariances) = kf.em(training_set).smooth(measurements) # means represent corrected points return smoothed_state_means, smoothed_state_covariances, simple_mode
def run_kal(measurements, training_size=40): # count the number of measurements num_measurements = len(measurements) # find the dimension of each row dim = len(measurements[0]) if dim == 3: simple_mode = True elif dim == 9: simple_mode = False else: print "Error: Dimensions for run_kal must be 3 or 9" exit(1) training_set = [] for i in range(min(training_size, len(measurements))): training_set.append(measurements[i]) if simple_mode: # run the filter kf = KalmanFilter(initial_state_mean=[0, 0, 0], n_dim_obs=3) (smoothed_state_means, smoothed_state_covariances) = kf.em(training_set).smooth(measurements) else: kf = KalmanFilter(initial_state_mean=[0, 0, 0, 0, 0, 0, 0, 0, 0], n_dim_obs=9) (smoothed_state_means, smoothed_state_covariances) = kf.em(training_set).smooth(measurements) # means represent corrected points return smoothed_state_means, smoothed_state_covariances, simple_mode
def kalman_filter(X, Y): outlier_thresh = 0 # Treat y as position, and that y-dot is # an unobserved state - the velocity, # which is modelled as changing slowly (inertia) # state vector [y, # y_dot] # transition_matrix = [[1, dt], # [0, 1]] observation_matrix = np.asarray([[1, 0]]) # observations: #t = [1,10,22,35,40,51,59,72,85,90,100] t = X # dt betweeen observations: dt = [np.mean(np.diff(t))] + list(np.diff(t)) transition_matrices = np.asarray([[[1, each_dt], [0, 1]] for each_dt in dt]) # observations ##y = np.transpose(np.asarray([[0.2,0.23,0.3,0.4,0.5,0.2, ## 0.65,0.67,0.62,0.5,0.4]])) y = np.transpose(np.asarray([Y])) y = np.ma.array(y) leave_1_out_cov = [] for i in range(len(y)): y_masked = np.ma.array(copy.deepcopy(y)) y_masked[i] = np.ma.masked kf1 = KalmanFilter(transition_matrices=transition_matrices, observation_matrices=observation_matrix) kf1 = kf1.em(y_masked) leave_1_out_cov.append(kf1.observation_covariance[0, 0]) # Find indexes that contributed excessively to observation covariance outliers = (leave_1_out_cov / np.mean(leave_1_out_cov)) < outlier_thresh for i in range(len(outliers)): if outliers[i]: y[i] = np.ma.masked kf1 = KalmanFilter(transition_matrices=transition_matrices, observation_matrices=observation_matrix) kf1 = kf1.em(y) (smoothed_state_means, smoothed_state_covariances) = kf1.smooth(y) return smoothed_state_means
def main(): # get the data readings, data_format = unpack_binary_data_into_list(BSN_DATA_FILE_NAME) # just the data/readings, no timestamps bsn_data = np.array([np.array(x[1:]) for x in readings[0:]]) # TODO # initialize filter # (all constructor parameters have defaults, and pykalman supposedly does a # good job of estimating them, so we will be lazy until there is a need to # define the initial parameters) bsn_kfilter = KalmanFilter(initial_state_mean=bsn_data[0], n_dim_state=len(bsn_data[0]), n_dim_obs=len(bsn_data[0]), em_vars='all') # perform parameter estimation and do predictions print("Estimating parameters...") bsn_kfilter.em(X=bsn_data, n_iter=5, em_vars='all') print("Creating smoothed estimates...") filtered_bsn_data = bsn_kfilter.smooth(bsn_data)[0] # re-attach the time steps to observations filtered_bsn_data = bind_timesteps_and_data_in_list( [x[0:1][0] for x in readings], filtered_bsn_data) # write the data to a new file with open(FILTERED_BSN_DATA_FILE_NAME, "wb") as filtered_file: filtered_file.write(string.ljust(data_format, 25)) for filtered_item in filtered_bsn_data: print(filtered_item) filtered_file.write(struct.pack(data_format, *filtered_item)) filtered_file.close()
def kalman_filter_one_factor_trained(x_series, y_series, train_period): x_train = x_series[:train_period] y_train = y_series[:train_period] x_train = np.vstack([x_train]).T[:, np.newaxis] # shape = (N,1,1) y_train = np.vstack([y_train]) # shape = (1, N) kf = KalmanFilter(transition_matrices=[1], observation_matrices=x_train, initial_state_mean=[1]) kf.em(y_train) state_mean, state_cov = kf.filter(y_train) x_predict = x_series[train_period:] y_predict = y_series[train_period:] predict = {} i = 0 state_mean_cache, state_cov_cache = state_mean[-1], state_cov[-1] for ind, x, y in zip(x_predict.index, x_predict, y_predict): res = kf.filter_update([state_mean_cache], state_cov_cache, observation=y, observation_matrix=np.array([[x]])) state_mean_cache = res[0][0] state_cov_cache = res[1] predict[i] = { x_series.index.name: ind, "state_mean": float(state_mean_cache), "state_cov": float(state_cov_cache) } i = i + 1 return kf, pd.DataFrame(predict).T.set_index(x_series.index.name)
def test_kalman_fit(): # check against MATLAB dataset kf = KalmanFilter( data.transition_matrix, data.observation_matrix, data.initial_transition_covariance, data.initial_observation_covariance, data.transition_offsets, data.observation_offset, data.initial_state_mean, data.initial_state_covariance, em_vars=['transition_covariance', 'observation_covariance']) loglikelihoods = np.zeros(5) for i in range(len(loglikelihoods)): loglikelihoods[i] = kf.loglikelihood(data.observations) kf.em(X=data.observations, n_iter=1) assert_true(np.allclose(loglikelihoods, data.loglikelihoods[:5])) # check that EM for all parameters is working kf.em_vars = 'all' n_timesteps = 30 for i in range(len(loglikelihoods)): kf.em(X=data.observations[0:n_timesteps], n_iter=1) loglikelihoods[i] = kf.loglikelihood(data.observations[0:n_timesteps]) for i in range(len(loglikelihoods) - 1): assert_true(loglikelihoods[i] < loglikelihoods[i + 1])
def getTimeSeriesCNNSample(self, stockData, useSmoothedData=False): self.permno = stockData.PERMNO.iloc[0] data = stockData.drop('PERMNO', axis=1).T self.nDays = data.shape[1] for i in range(self.data_len, self.nDays, self.retrain_freq): series = data.iloc[:, i - self.data_len:i] if useSmoothedData: Smoother = KalmanFilter( n_dim_obs=series.shape[0], n_dim_state=series.shape[0], em_vars=[ 'transition_matrices', 'observation_matrices', 'transition_offsets', 'observation_offsets', 'transition_covariance', 'observation_convariance', 'initial_state_mean', 'initial_state_covariance' ]) measurements = series.T.values Smoother.em(measurements, n_iter=5) series, _ = Smoother.smooth(measurements) series = series.T if self.method == 'GADF': gadf = GADF(self.image_size) self.GADFSample.append(gadf.fit_transform(series).T) elif self.method == 'GASF': gasf = GASF(self.image_size) self.GASFSample.append(gasf.fit_transform(series).T) self.nSamples = len(self.GADFSample) return self
def kalmanFilter(measurements): initial_state_mean = [measurements[0, 0], 0, measurements[0, 1], 0] transition_matrix = [[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]] observation_matrix = [[1, 0, 0, 0], [0, 0, 1, 0]] kf1 = KalmanFilter(transition_matrices = transition_matrix, observation_matrices = observation_matrix, initial_state_mean = initial_state_mean) kf1 = kf1.em(measurements, n_iter=5) (smoothed_state_means, smoothed_state_covariances) = kf1.smooth(measurements) kf2 = KalmanFilter(transition_matrices = transition_matrix, observation_matrices = observation_matrix, initial_state_mean = initial_state_mean, observation_covariance = 10*kf1.observation_covariance, em_vars=['transition_covariance', 'initial_state_covariance']) kf2 = kf2.em(measurements, n_iter=5) (smoothed_state_means, smoothed_state_covariances) = kf2.smooth(measurements) return smoothed_state_means[:, 0], smoothed_state_means[:, 2]
def apply_kalman_physics_model(coordinates, vis_name='test', vis=False): initial_state_mean = [coordinates[0, 0], 0, coordinates[0, 1], 0] transition_matrix = [[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]] observation_matrix = [[1, 0, 0, 0], [0, 0, 1, 0]] kf1 = KalmanFilter(transition_matrices=transition_matrix, observation_matrices=observation_matrix, initial_state_mean=initial_state_mean) kf1 = kf1.em(coordinates, n_iter=5) (smoothed_state_means, smoothed_state_covariances) = kf1.smooth(coordinates) # smoothen out further kf2 = KalmanFilter( transition_matrices=transition_matrix, observation_matrices=observation_matrix, initial_state_mean=initial_state_mean, observation_covariance=10 * kf1.observation_covariance, em_vars=['transition_covariance', 'initial_state_covariance']) kf2 = kf2.em(coordinates, n_iter=5) (smoothed_state_means, smoothed_state_covariances) = kf2.smooth(coordinates) # speed = np.sqrt(np.square(smoothed_state_means[:, 1]) + np.square(smoothed_state_means[:, 3]))#*fps*scale*3.6 # in km/h speed_x = smoothed_state_means[:, 1] # print speed.shape # speed = speed[(speed[:]>0) & (speed[:]<max_speed)]; avg_speed_x = float(np.average(speed_x)) #*fps*scale*3.6 # in km/h dist_x = avg_speed_x * len(coordinates) # # plot fiter by kalman if vis: fig = plt.figure() ax = fig.add_subplot(111) times = range(coordinates.shape[0]) ax.plot( times, coordinates[:, 0], 'bo', times, coordinates[:, 1], 'ro', times, smoothed_state_means[:, 0], 'b--', times, smoothed_state_means[:, 2], 'r--', ) fig.savefig('vis_outputs/' + vis_name + '.png') plt.close(fig) # if avg_speed > max_avg_speed: # print vis_name, # print 'speed: {}'.format(avg_speed) # return [None]*2 return dist_x
def grava(): kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]), transition_covariance=.009 * np.eye(2)) kf2 = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]), transition_covariance=.009 * np.eye(2)) kf3 = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]), transition_covariance=.009 * np.eye(2)) x = history_x_ z = history_z_ y = history_y_ cm = np.asarray(x)[0] cm2 = np.asarray(z)[0] cm3 = np.asarray(y)[0] cm_seq = np.arange(1, cm, step=150) cm_seq2 = np.arange(1, cm2, step=150) cm_seq3 = np.arange(1, cm3, step=150) cm_lis = np.asarray(cm_seq) cm_lis2 = np.asarray(cm_seq2) cm_lis3 = np.asarray(cm_seq3) cm_com = cm_lis.tolist() cm_com2 = cm_lis2.tolist() cm_com3 = cm_lis3.tolist() cm_com = cm_com + np.asarray(history_x_).tolist() cm_com2 = cm_com2 + np.asarray(history_z_).tolist() cm_com3 = cm_com3 + np.asarray(history_y_).tolist() pos = [x0, y0, z0] # posição inicial states_pred_x = kf.em(cm_com).smooth(cm_com)[0] states_pred_z = kf2.em(cm_com2).smooth(cm_com2)[0] states_pred_y = kf3.em(cm_com3).smooth(cm_com3)[0] ax.scatter(pos[0], pos[1], pos[2], marker='o', color='green', label="Original", alpha=0.4) ax.scatter(states_pred_x[:, 0].mean(), states_pred_y[:, 0].mean(), states_pred_z[:, 0].mean(), marker='^', color='blue', alpha=1)
def get_state(self, data): self.steps += 1 if self.steps < 16: fetch = 16 else: fetch = self.steps if self.steps > 32: fetch = 32 d = np.log(np.array([self.env.data.get_t_data(-i)[2:6] for i in range(fetch)])) mean = d.mean() kf = KalmanFilter(initial_state_mean=mean, n_dim_obs=4) v = kf.em(d) p = v.smooth(d)[0] d = p.flatten() d = d[::-1] s = d.std() vo = [] for i in range(fetch): vo.append([self.env.data.get_t_data(-i)[3] - self.env.data.get_t_data(-i)[4]]) vo = np.log(np.array(vo[::-1])+1) s='{:0.6f}'.format(s ) di = [] for i in range(fetch): di.append([self.env.data.get_t_data(-i)[5] - self.env.data.get_t_data(-i)[2]]) di = np.array(di[::-1]) signal=di/np.linalg.norm(di) x5 = [] n5 = int(fetch / 16) m5 = fetch % 16 print(n5, m5) aa = signal[0:m5] y5 = modwt(aa, 'db2', 5)[5] nd5 = np.linalg.norm(y5) if nd5 != 0: y5 = 2 * y5 / nd5 x5 = x5 + list(y5) for i in range(n5): aa = signal[m5 + i * 16:m5 + (i + 1) * 16] y5 = modwt(aa, 'db2', 5)[5] nd5 = np.linalg.norm(y5) if nd5 != 0: y5 = 2 * y5 / nd5 x5 = x5 + list(y5) return x5,s
def test_kalman_filter(self): kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]]) measurements = np.asarray([[1,0], [0,0], [0,1]]) # 3 observations kf = kf.em(measurements, n_iter=5) (filtered_state_means, filtered_state_covariances) = kf.filter(measurements) (smoothed_state_means, smoothed_state_covariances) = kf.smooth(measurements) return filtered_state_means
def get_kmf(self, after_noise_cancel): kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]), transition_covariance=([[0.25, 0], [0, 0]]), initial_state_mean=[29, -1.5]) self.after_kf = kf.em(after_noise_cancel).smooth(after_noise_cancel)[0] return self.after_kf
def onEnd(self): print("Launching Figures for Estimator") #print(self.xr) #print(self.yr) kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]), transition_covariance=0.01 * np.eye(2)) self.states_pred = kf.em(self.yr).smooth(self.yr)[0] self.signalr = pl.scatter(self.xr, self.yr, linestyle='-', marker='o', color='b', label='Data Received from Sensor') self.position_line = pl.plot(self.xr,self.states_pred[:, 0], linestyle='-', marker='o', color='g', label='Data Estimated from Sensor') self.position_error = pl.plot(self.xr, (self.yr-self.states_pred[:, 0]), linestyle='-', marker='o', color='m', label='Relative Estimation Error') pl.legend(loc='upper right') pl.xlim(xmin=0, xmax=self.counterr) pl.xlabel('time[s]') pl.ylabel('Position [m]') pl.ylim(ymin=-(A+0.25), ymax=(A+0.25)) pl.show() # First, form the AMS AID to inform start self.informAMSr = spade.AID.aid(name="ams.127.0.0.1", addresses=["xmpp://ams.127.0.0.1"]) # Second, build the message self.msg = spade.ACLMessage.ACLMessage() # Instantiate the message self.msg.setPerformative("inform") # Set the "inform" FIPA performative self.msg.setOntology("Estimation") # Set the ontology of the message content self.msg.setLanguage("OWL-S") # Set the language of the message content self.msg.addReceiver(self.informAMSr) # Add the message receiver self.msg.setContent("End of Reception") # Set the message content print(self.msg.content) # Third, send the message with the "send" method of the agent self.myAgent.send(self.msg)
def trainParam(self): ''' From 2017-05-01 Using fixed trans_mat and obs_mat to train to get the two noise covariance matrix and the initial covariance matrix ''' train_dat = self.data['2017-05-01 01:00:00':].iloc[:self.train_size, :] trans_mat = np.eye(self.dim) obs_mat = np.eye(self.dim) init = train_dat.iloc[-1, :] from pykalman import KalmanFilter kf = KalmanFilter(transition_matrices=trans_mat, observation_matrices=obs_mat, initial_state_mean=init, em_vars=[ 'observation_covariance', 'transition_covariance', 'initial_state_covariance' ]) init_filter = kf.em(train_dat, n_iter=3) x, p = init_filter.filter(train_dat) self.init_val = x[-1] self.init_prob = p[-1] self.R = init_filter.observation_covariance self.Q = init_filter.transition_covariance
def kalman_fit(x): train = x[:, 0:1500] kf = KalmanFilter(n_dim_state=5, n_dim_obs=train.shape[0]) train = train.T for i in range(10): #increase this kf = kf.em(X=train, n_iter=1, em_vars='all') print(i) #testing data test = x[:, 1500:3000] test = test.T mean = np.matmul(kf.observation_matrices, kf.smooth(test)[0].T) for i in range(len(mean)): mean[:, i] = mean[:, i] + kf.observation_offsets r_squared = np.sum(np.square(mean - test.T)) return r_squared
def applySmoothing(playerDict, nIter): smoothedDict = {} frameDict = {} for i in playerDict: arr = np.array(playerDict[i]) #we will remove continuous (0,0) observations measurements, firstNonZero, lastNonZero = findNonZero(arr) # frameDict[i] = (firstNonZero, lastNonZero) n = measurements.shape[0] #if there are more than 4 observations, apply smoothing if n > 4: kf = KalmanFilter(n_dim_obs=2, n_dim_state=2) #smooth all excpet first and last 2 coordinates (not enough obs for those) smoothed = kf.em(measurements, n_iter=nIter).smooth(measurements[2:(n - 2)]) adjusted1 = np.vstack((measurements[0:2], smoothed[0])) adjusted = np.vstack((adjusted1, measurements[(n - 2):(n + 1)])) #split array and convert to (x,y) coords xcoords = np.reshape(adjusted[:, 0], (len(measurements))) ycoords = np.reshape(adjusted[:, 1], (len(measurements))) smoothedDict[i] = list(zip(xcoords, ycoords)) else: #if <= 4 obs do not smooth smoothedDict[i] = measurements return smoothedDict
def processing_function(file_path, joints): _, keypoints = readAllFramesDATA(file_path) for i in range(len(joints)): measurements = np.copy(keypoints[:, i]) initial_state_mean = [measurements[0, 0], 0, measurements[0, 1], 0] transition_matrix = [[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]] observation_matrix = [[1, 0, 0, 0], [0, 0, 1, 0]] kf1 = KalmanFilter(transition_matrices=transition_matrix, observation_matrices=observation_matrix, initial_state_mean=initial_state_mean) kf1 = kf1.em(measurements, n_iter=5) (smoothed_state_means, smoothed_state_covariances) = kf1.smooth(measurements) keypoints[:, i, 0] = smoothed_state_means[:, 0] keypoints[:, i, 1] = smoothed_state_means[:, 2] return keypoints
def fitKCA(t,z,q,fwd=0): # #Inputs: # t: Iterable with time indices # z: Iterable with measurements # q: Scalar that multiplies the seed states covariance # fwd: number of steps to forecast (optional, default=0) #Output: #x[0]: smoothed state means of position velocity and acceleration #x[1]: smoothed state covar of position velocity and acceleration #Dependencies: numpy, pykalman # #1) Set up matrices A,H and a seed for Q h=(t[-1]-t[0])/t.shape[0] A=np.array([[1,h,.5*h**2], [0,1,h], [0,0,1]]) Q=q*np.eye(A.shape[0]) #2) Apply the filter kf=KalmanFilter(transition_matrices=A,transition_covariance=Q) kf=kf.em(z) #4) Smooth x_mean,x_covar=kf.smooth(z) #5) Forecast for fwd_ in range(fwd): x_mean_,x_covar_=kf.filter_update(filtered_state_mean=x_mean[-1], filtered_state_covariance=x_covar[-1]) x_mean=np.append(x_mean,x_mean_.reshape(1,-1),axis=0) x_covar_=np.expand_dims(x_covar_,axis=0) x_covar=np.append(x_covar,x_covar_,axis=0) #6) Std series x_std=(x_covar[:,0,0]**.5).reshape(-1,1) for i in range(1,x_covar.shape[1]): x_std_=x_covar[:,i,i]**.5 x_std=np.append(x_std,x_std_.reshape(-1,1),axis=1) return x_mean,x_std,x_covar
def get_feed_dict(self, data, today ): l = self.encode_series_len dl = self.decode_series_len d = np.array([data.get_t_data(-i)[2:6] for i in range(l)]) mean = d.mean() kf = KalmanFilter(em_vars=['transition_covariance', 'observation_covariance'], initial_state_mean=mean, n_dim_obs=4) v = kf.em(d) h = v.smooth(d) h=h[0].flatten() self.stoday = datetime.strftime(today, '%m/%d/%Y') is_today = [data.get_t_data(-i)[0] == self.stoday for i in range(l-dl)] vol = [data.get_t_data(-i)[6] for i in range(l-dl)] return { self.y_encode: [h[:-dl].tolist()], self.volume_encode: [vol], self.is_today: [is_today], self.decode_len: [dl], self.y_decode: [h[0:dl].tolist()], self.encode_len: [l-dl], self.lr: self.learning_rate }
def fix_meta_sequence_kalman(metas): measurements = np.ma.asarray(np.zeros((len(metas), 6))) for i, meta in enumerate(metas): if meta is not None: measurements[i] = meta.flatten() else: measurements[i] = np.ma.masked means = np.mean(measurements, axis=0) vars = np.var(measurements, axis=0) measurements -= means measurements /= vars kf = KalmanFilter( n_dim_obs=6, n_dim_state=6, observation_covariance=1e4 * np.eye(6), transition_covariance=1e4 * np.eye(6), em_vars=['initial_state_mean', 'initial_state_covariance']) kf = kf.em(measurements) smoothed, _ = kf.smooth(measurements) smoothed *= vars smoothed += means metas = [np.reshape(sm, (2, 3)) for sm in smoothed] return metas
def pkm(): #https://pykalman.github.io #READ Basic Usage print("Beginning Kalman Filtering....") kf = KalmanFilter(initial_state_mean=0, n_dim_obs=9) #measurements - array of 9x1 vals #each a_x, a_y, a_z, g_x, g_y, g_z, m_x, m_y, m_z measurements = getMeasurementsFromDB() smooth_inputs = [[2,0], [2,1], [2,2]] #traditional assumed params are known, but we can get from EM on the measurements #we get better predictions of hidden states (true position) with smooth function kf.em(measurements).smooth(smooth_inputs) print(measurements) print(kf.em(measurements, n_iter=5))
def getSmoothTrack(self, radarPeriod): from pykalman import KalmanFilter roughTrackArray = self.backtrackMeasurement() initialNode = self.getInitial() depth = initialNode.depth() initialState = initialNode.x_0 for i, m in enumerate(roughTrackArray): if m is None: roughTrackArray[i] = [np.NaN, np.NaN] measurements = np.ma.asarray(roughTrackArray) for i, m in enumerate(measurements): if np.isnan(np.sum(m)): measurements[i] = np.ma.masked assert measurements.shape[1] == 2, str(measurements.shape) if depth < 2: pos = measurements.filled(np.nan) vel = np.empty_like(pos) * np.nan return pos, vel, False kf = KalmanFilter(transition_matrices=model.Phi(radarPeriod), observation_matrices=model.C_RADAR, initial_state_mean=initialState) kf = kf.em(measurements, n_iter=5) (smoothed_state_means, _) = kf.smooth(measurements) smoothedPositions = smoothed_state_means[:, 0:2] smoothedVelocities = smoothed_state_means[:, 2:4] assert smoothedPositions.shape == measurements.shape, \ str(smoothedPositions.shape) + str(measurements.shape) assert smoothedVelocities.shape == measurements.shape, \ str(smoothedVelocities.shape) + str(measurements.shape) return smoothedPositions, smoothedVelocities, True
def smooth_trajectories(self): kf = KalmanFilter(transition_matrices=self.Fk, observation_matrices=self.Hk) measurements = np.asarray(self.measured_state) kf = kf.em(measurements, n_iter=5) (smoothed_state_means, smoothed_state_covariances) = kf.smooth(measurements) [self.updated_state.append(u) for u in smoothed_state_means] self.update()
def kf_naive_outlier_detection(self, input_series, idx_position): """ This function detects outlier for the specified index position of the series. :param numpy.array input_series: Input time series :param int idx_position: Target index position :return: Anomaly flag :rtype: bool >>> input_series = [110, 119, 316, 248, 451, 324, 241, 275, 381] >>> self.kf_naive_outlier_detection(input_series, 6) False """ import numpy as np from pykalman import KalmanFilter kf = KalmanFilter() filtered_state_means, filtered_state_covariance = kf.em( input_series).filter(input_series) residuals = input_series - filtered_state_means[:, 0] is_anomaly = residuals[idx_position] < np.mean(residuals) \ - (2.5 * np.sqrt(filtered_state_covariance)[idx_position][0][0]) \ or residuals[idx_position] > np.mean(residuals) \ + (2.5 * np.sqrt(filtered_state_covariance)[idx_position][0][0]) return is_anomaly
def test_kalman_pickle(): kf = KalmanFilter( data.transition_matrix, data.observation_matrix, data.transition_covariance, data.observation_covariance, data.transition_offsets, data.observation_offset, data.initial_state_mean, data.initial_state_covariance, em_vars='all') # train and get log likelihood X = data.observations[0:10] kf = kf.em(X, n_iter=5) loglikelihood = kf.loglikelihood(X) # pickle Kalman Filter store = StringIO() pickle.dump(kf, store) clf = pickle.load(StringIO(store.getvalue())) # check that parameters came out already np.testing.assert_almost_equal(loglikelihood, kf.loglikelihood(X)) # store it as BytesIO as well store = BytesIO() pickle.dump(kf, store) kf = pickle.load(BytesIO(store.getvalue()))
def KFilt(sample,fs=25): """Input (sample) is fill_in_data output. Outputs two lists of color data """ #kalman filter inputs # Dimensions of parameters: # 'transition_matrices': 2, # 'transition_offsets': 1, # 'observation_matrices': 2, # 'observation_offsets': 1, # 'transition_covariance': 2, # 'observation_covariance': 2, # 'initial_state_mean': 1, # 'initial_state_covariance': 2, n_timesteps = len(sample) trans_mat = [] #mask missing values observations = np.ma.array(sample,mask=np.zeros(sample.shape)) missing_loc = np.where(np.isnan(sample)) observations[missing_loc[0][:],missing_loc[1][:]] = np.ma.masked #Import Kalman filter, inerpolate missing points and get 2nd, 3rd orde kinematics dt = 1./25 #Length of each frame (should be iether 1/25 or 1/30) n_timesteps = len(sample) observation_matrix = np.array([[1,0,0,0], [0,1,0,0]])#np.eye(4) t = np.linspace(0,len(observations)*dt,len(observations)) q = np.cov(observations.T[:2,:400]) qdot = np.cov(np.diff(observations.T[:2,:400]))#np.cov(observations[:1,:400]) h=(t[-1]-t[0])/t.shape[0] A=np.array([[1,0,h,.5*h**2], [0,1,0,h], [0,0,1,0], [0,0,0,1]]) init_mean = [sample[0],0,0] #initial mean should be close to the first point, esp if first point is human-picked and tracking starts at the beginning of a video observation_covariance = q*500 #ADJUST THIS TO CHANGE SMOOTHNESS OF FILTER init_cov = np.eye(4)*.001#*0.0026 transition_matrix = A transition_covariance = np.array([[q[0,0],q[0,1],0,0], [q[1,0],q[1,1],0,0], [0,0,qdot[0,0],qdot[0,1]], [0,0,qdot[1,0],qdot[1,1]]]) kf = KalmanFilter(transition_matrix, observation_matrix,transition_covariance,observation_covariance,n_dim_obs=2) kf = kf.em(observations,n_iter=1,em_vars=['transition_covariance','transition_matrix','observation_covariance']) #pdb.set_trace() global trans_mat, trans_cov, init_cond x_filt = kf.filter(observations[0])[0]#observations.T[0])[0] kf_means = kf.smooth(observations[0])[0] return kf_means,x_filt #np.column_stack((color_x[:,0],color_y[:,0],color_x[:,1],color_y[:,1])),frames
def kalman_smooth_and_diff(x, dt, nders=2, em_Q=False): """ Smooth a set of timeseries and differentiate them. Args: x (torch.tensor): (B,T,n) timeseries dt (float): time-step nders (int): number of derivatives to take em_Q (bool): learn transition cov? Returns: smooth_series (list of torch.tensor): list of (B,T,n) smooth tensors (xsm, dxsmdt, ...) """ retvals = [torch.zeros_like(x) for i in range(nders + 1)] em_vars = [ 'initial_state_mean', 'initial_state_covariance', 'observation_covariance' ] if em_Q: em_vars += ['transition_covariance'] if nders != 2: raise NotImplementedError A = np.array([[0., 1., 0.], [0., 0., 1.], [0., 0., 0.]]) Ad = expm(A * dt) Bd = np.array([[1., 0., 0.]]) Q = np.diag([0.001, 0.001, 1.0]) B, T, N = x.shape for b in tqdm(range(B)): for n in range(N): ser = x[b, :, n:n + 1].detach().numpy() kf = KalmanFilter(transition_matrices=Ad, observation_matrices=Bd, transition_covariance=Q) kf.em(ser, em_vars=em_vars) sm_x, _ = kf.smooth(ser) sm_x = torch.tensor(sm_x) for i in range(3): retvals[i][b, :, n] = sm_x[:, i] return retvals
def kalman_filter_forcast(observation_matrices, transition_matrices, trainset, data, end_time): #卡尔曼滤波进行预测 kf = KalmanFilter(transition_matrices=transition_matrices, observation_matrices=observation_matrices) kf.em(trainset) filter_mean, filter_cov = kf.filter(trainset) index1 = np.where(data.index.month == end_time)[0][0] for i in range(index1, len(data)): next_filter_mean, next_filter_cov = kf.filter_update( filtered_state_mean=filter_mean[-1], filtered_state_covariance=filter_cov[-1], observation=data[i]) filter_mean = np.vstack((filter_mean, next_filter_mean)) filter_cov = np.vstack((filter_cov, next_filter_cov.reshape(1, 8, 8))) AR_kalman_forcast = np.matmul(np.array(filter_mean), transition_matrices[7, :]) #相当于用AR(8)来预测 return AR_kalman_forcast
def get_state(self,data): #using Kalman filter to get the true value/price in a minute observations, open,high,low,close prices d = np.array([data.get_t_data(-i)[2:6] for i in range(self.config.observation_window)]) mean = d.mean() kf = KalmanFilter(initial_state_mean=mean, n_dim_obs=4) v = kf.em(d) h=self.norm_it(v.smooth(d)[0]) return h
def denoise(self, df): for col in df.columns: if col not in ["target"]: kf = KalmanFilter(initial_state_mean=0, n_dim_obs=1) prices = df[col].values results = kf.em(prices).smooth(prices) df[col] = results[0] df.dropna(inplace=True) return df
def kalman_filter(observation_matrices, transition_matrices, trainset, data, end_time): #卡尔曼滤波进行过滤 kf = KalmanFilter(transition_matrices=transition_matrices, observation_matrices=observation_matrices) kf.em(trainset) filter_mean, filter_cov = kf.filter(trainset) index1 = np.where(data.index.month == end_time)[0][0] for i in range(index1, len(data)): next_filter_mean, next_filter_cov = kf.filter_update( filtered_state_mean=filter_mean[-1], filtered_state_covariance=filter_cov[-1], observation=data[i]) filter_mean = np.vstack((filter_mean, next_filter_mean)) filter_cov = np.vstack((filter_cov, next_filter_cov.reshape(1, 8, 8))) AR_kalman_filter = pd.Series( filter_mean[index1:, 7], index=data.index[index1:]) #filter_mean的最后一列为当日的过滤结果 return AR_kalman_filter
class LDS: """ Train an LDS with the EM algorithm, find latent paths using Kalman Filter and Smoother Here I iterate over individual trials calling the EM algorithm once on each trial... """ def __init__(self, X, Dz): self.X = X self.NTrials, self.NTbins, self.Dx = X.shape self.Dz = Dz self.kf = KalmanFilter(n_dim_state=Dz, n_dim_obs=self.Dx, em_vars=[ 'transition_matrices', 'observation_matrices', 'transition_covariance', 'observation_covariance' ]) self.Z = np.zeros([self.NTrials, self.NTbins, self.Dz]) self.filtered_paths = np.zeros([self.NTrials, self.NTbins, self.Dz]) self.filtered_covar = np.zeros( [self.NTrials, self.NTbins, self.Dz, self.Dz]) self.smoothed_covar = np.zeros( [self.NTrials, self.NTbins, self.Dz, self.Dz]) def train(self, epochs): print("EM Algorithm Training...") for i in range(epochs): print("- Epoch %i" % i) for n in range(self.NTrials): print("-- Trial %i" % n) self.kf.em(self.X[n], n_iter=1) self.A = self.kf.transition_matrices self.Q = self.kf.transition_covariance self.C = self.kf.observation_matrices self.Sigma = self.kf.observation_covariance def inference(self): for n in range(self.NTrials): print("-- Trial %i" % n) self.filtered_paths[n], self.filtered_covar[n] = self.kf.filter( self.X[n]) self.Z[n], self.smoothed_covar[n] = self.kf.smooth(self.X[n])
def kalman(observations): masked = np.ma.append(observations, [0.] * seconds_to_predict) stop = len(masked) start = stop - seconds_to_predict for i in range(start, stop): masked[i] = np.ma.masked kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]), transition_covariance=0.01 * np.eye(2)) states_pred_x = kf.em(masked).smooth(masked)[0] return states_pred_x[-1, 0]
def kalmanfileter(): # Read read_variable var_a = acc_read_variable[["ax"]].values kf = KalmanFilter(initial_state_mean=0, n_dim_obs=1) kf = kf.em(var_a, n_iter=5) (filtered_state_means, filtered_state_covariances) = kf.filter(acc_read_variable["ax"]) return kf
def make_prediction(self, measurements): """ Compute prediction of object position for the next step. """ initial_state_mean = [measurements[0][0], 0, measurements[0][1], 0] kf1 = KalmanFilter(transition_matrices=self.transition_matrix, observation_matrices=self.observation_matrix, initial_state_mean=initial_state_mean) kf1 = kf1.em(list(measurements), n_iter=2) return kf1.smooth(measurements)
def df_kalman(self): """ Smooths trip using Kalman method * https://github.com/pykalman/pykalman * http://pykalman.github.io * https://ru.wikipedia.org/wiki/Фильтр_Калмана * http://bit.ly/1Dd1bhn """ df = self.trip_data.copy() df['_v_'] = self.distance(self.trip_data, '_x_', '_y_') df['_a_'] = df._v_.diff() # Маскируем ошибочные точки df._x_ = np.where( (df._a_ > MIN_A) & (df._a_ < MAX_A), df._x_, np.ma.masked) df._y_ = np.where( (df._a_ > MIN_A) & (df._a_ < MAX_A), df._y_, np.ma.masked) df['_vx_'] = df._x_.diff() df['_vy_'] = df._y_.diff() # Параметры физической модели dx = v * dt transition_matrix = [[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]] observation_matrix = [[1, 0, 0, 0], [0, 1, 0, 0]] xinit, yinit = df._x_[0], df._y_[0] vxinit, vyinit = df._vx_[1], df._vy_[1] initcovariance = 1.0e-4 * np.eye(4) transistionCov = 1.0e-3 * np.eye(4) observationCov = 1.0e-2 * np.eye(2) # Фильтр Калмана kfilter = KalmanFilter( transition_matrices=transition_matrix, observation_matrices=observation_matrix, initial_state_mean=[xinit, yinit, vxinit, vyinit], initial_state_covariance=initcovariance, transition_covariance=transistionCov, observation_covariance=observationCov ) measurements = df[['_x_', '_y_']].values kfilter = kfilter.em(measurements, n_iter=5) (state_means, state_covariances) = kfilter.smooth(measurements) kdf = pd.DataFrame(state_means, columns=('x', 'y', 'vx', 'vy')) kdf['v'] = np.sqrt(np.square(kdf.vx) + np.square(kdf.vy)) self.trip_data[['x', 'y', 'v']] = kdf[['x', 'y', 'v']]
def main(): # get the data readings, data_format = unpack_binary_data_into_list(BSN_DATA_FILE_NAME) # just the data/readings, no timestamps bsn_data = np.array([np.array(x[1:]) for x in readings[0:]]) # TODO # initialize filter # (all constructor parameters have defaults, and pykalman supposedly does a # good job of estimating them, so we will be lazy until there is a need to # define the initial parameters) bsn_kfilter = KalmanFilter( initial_state_mean = bsn_data[0], n_dim_state = len(bsn_data[0]), n_dim_obs = len(bsn_data[0]), em_vars = 'all' ) # perform parameter estimation and do predictions print("Estimating parameters...") bsn_kfilter.em(X=bsn_data, n_iter=5, em_vars = 'all') print("Creating smoothed estimates...") filtered_bsn_data = bsn_kfilter.smooth(bsn_data)[0] # re-attach the time steps to observations filtered_bsn_data = bind_timesteps_and_data_in_list( [x[0:1][0] for x in readings], filtered_bsn_data ) # write the data to a new file with open(FILTERED_BSN_DATA_FILE_NAME, "wb") as filtered_file: filtered_file.write(string.ljust(data_format, 25)) for filtered_item in filtered_bsn_data: print(filtered_item) filtered_file.write(struct.pack(data_format, *filtered_item)) filtered_file.close()
def learn(self): dt = 0.10 kf = KalmanFilter( em_vars=["transition_covariance", "observation_covariance"], observation_covariance=np.eye(2) * 1, transition_covariance=np.array( [[0.000025, 0, 0.0005, 0], [0, 0.000025, 0, 0.0005], [0.0005, 0, 0.001, 0], [0, 0.000025, 0, 0.001]] ), transition_matrices=np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]), observation_matrices=np.array([[1, 0, 0, 0], [0, 1, 0, 0]]), ) states, co = kf.em(self.offline_data).smooth(self.offline_data) print kf.transition_covariance self.lx = [s[0] for s in states] self.ly = [s[1] for s in states]
def weigh_data_point(self, map_number): """ Determine a point's weight """ # Look @ how many points on that X,Y spot through various Z positions # If most recent positions show obstacle, probably obstacle #for range(map_number-5, map_number): # Find X, Y for that map number... # pass # Load up pickles X, Y, Z = self.get_pickles() # Perform Kalman filters # Initial state = 0 because we're starting off at coords 0,0 kf = KalmanFilter(initial_state_mean = 0, n_dim_obs=2) measurements = kf.em(measurements.smooth(measurements)) # TODO: Deal w/ updating locations # means, covars = self.update_data_points(measurements) # measurements = means # Return data points return measurements
def KFilt(sample): """Input (sample) is fill_in_data output. Outputs two lists of color data """ #kalman filter inputs n_timesteps = len(sample) trans_mat = [] trans_cov = [] init_cond = [],[] #TODO: set up specific parameters (observation matrix, etc) #mask missing values observations = np.ma.array(sample,mask=np.zeros(sample.shape)) missing_loc = np.where(np.isnan(sample)) observations[missing_loc[0][:],missing_loc[1][:]] = np.ma.masked #Import Kalman filter, inerpolate missing points and get 2nd, 3rd orde kinematics dt = 1./25 #Length of each frame (should be iether 1/25 or 1/30) #trans_mat = np.array([[1, 0 ,1, 0],[0, 1, 0, 1],[0,0,1,0],[0,0,0,1]]) #trans_cov = 0.01*np.eye(4) if not trans_mat: #if there's not a global variable defining tranisiton matrices and covariance, make 'em and optimize trans_mat = np.array([[1,1],[0,1]]) trans_cov = 0.01*np.eye(2) kf = KalmanFilter(transition_matrices = trans_mat, transition_covariance=trans_cov) kf = kf.em(observations.T[0],n_iter=5) #optimize parameters trans_mat = kf.transition_matrices trans_cov = kf.transition_covariance init_mean = kf.initial_state_mean init_cov = kf.initial_state_covariance else: kf = KalmanFilter(transition_matrices = trans_mat, transition_covariance=trans_cov,\ initial_state_mean = init_mean,initial_state_covariance = init_cov) global trans_mat, trans_cov, init_cond color_x = kf.smooth(observations.T[0])[0] color_y = kf.smooth(observations.T[1])[0] return color_x,color_y #np.column_stack((color_x[:,0],color_y[:,0],color_x[:,1],color_y[:,1])),frames
def get_interpolated_speed_list(path,kalman=False,smoothFactor=0.01): gpx_file = open(path, 'r') gpx = gpxpy.parse(gpx_file) interpolated_list = [] previous_speed = 0 for point in range(len(gpx.tracks[0].segments[0].points)-1): p1 = gpx.tracks[0].segments[0].points[point] p1_time = time.mktime(p1.time.timetuple()) p2 = gpx.tracks[0].segments[0].points[point+1] p2_time = time.mktime(p2.time.timetuple()) ## average speed between data points speed = p1.speed_between(p2) if speed is None or speed == 0: if previous_speed > 7: if speed == 0: speed = previous_speed-1 else: speed = previous_speed else: speed = 0 previous_speed = speed interpolated_list.append(speed) ## if time difference is greater than one second, the missing seconds are filled with average speed if p2_time-p1_time > 1: seconds = int(p2_time-p1_time-1) for second in range(seconds): interpolated_list.append(speed) if kalman is True: kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]), transition_covariance=smoothFactor*np.eye(2)) states_pred = kf.em(interpolated_list).smooth(interpolated_list)[0] return states_pred[:, 0] else: return interpolated_list
from pykalman import KalmanFilter import numpy as np kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]]) measurements = np.asarray([[1,0], [0,0], [0,1]]) # 3 observations kf = kf.em(measurements, n_iter=5) (filtered_state_means, filtered_state_covariances) = kf.filter(measurements) print filtered_state_means
accel_z.append((acceleration_z[i-2] + acceleration_z[i-1] + acceleration_z[i]) / n) ''' ''' for i in range(len(acceleration_x)): if abs(acceleration_x[i]) < 2: acceleration_x[i] = 0 if abs(acceleration_y[i]) < 2: acceleration_y[i] = 0 if abs(acceleration_z[i]) < 2: acceleration_z[i] = 0 ''' t = np.arange(0, (len(acceleration_x))*dt, dt) kf = KalmanFilter(transition_matrices=np.array([[1.15, 1], [0, 1]]), transition_covariance=0.01 * np.eye(2)) states_pred = kf.em(acceleration_x).smooth(acceleration_x)[0] plt.figure(1) obs_scatter = plt.plot(t[0:len(acceleration_x)], acceleration_x, color='b', label='observations x') acceleration_x = states_pred[:, 0] position_line = plt.plot(t[0:len(states_pred[:, 0])], states_pred[:, 0], linestyle='-', color='r', label='filtered x') plt.legend(loc='lower right') plt.xlabel('time') plt.grid() #dist, cost, path = mlpy.dtw_subsequence(acceleration_x, states_pred[:, 0], dist_only=False) lines = [' '.join(list(map(str, states_pred[:, 0]))) + "\n"]
from pylab import * from pykalman import KalmanFilter kf = KalmanFilter(); data = loadtxt('data_gps1.txt'); lat = radians(data[0,:]); lon = radians(data[1,:]); dlat = lat[1:] - lat[0:-1]; dlon = lon[1:] - lon[0:-1]; R = 6373; #Radius of earth in kilometers a = (sin(dlat/2))**2 + cos(lat[0:-1])*cos(lat[1:])*((sin(dlon/2))**2); c = 2*arctan2(sqrt(a),sqrt(1-a)); d = R * c; kf = kf.em(d,n_iter = 5); (smoothed_means,smoothed_cov) = kf.smooth(d); (filtered_means,filtered_cov) = kf.filter(d); plot(d,label = 'Original Data'); plot(smoothed_means,label = 'Kalman Smoothed'); plot(filtered_means,label = 'Kalman Filtered'); legend(); show();
from pykalman import KalmanFilter import pylab as pl F = np.array([[math.cos(math.pi/16), math.sin(math.pi/16)],[ -math.sin(math.pi/16), math.cos(math.pi/16)]])*1.01 x = np.array([[1],[1]]) for i in range(1, 100): # loop from 1 to 99 x = np.concatenate((x, np.dot(F, x[:, x.shape[1]-1].reshape(x.shape[0], 1))+(np.array(np.random.normal(0,1,x.shape[0]))*0.1).reshape(x.shape[0],1)), axis=1) z = x + np.array((np.random.normal(0,1,x.size)*0.1).reshape(x.shape[0], x.shape[1])) pl.plot(x.T[:,0], x.T[:,1], linestyle='-', marker='o', color='r', label='original') pl.plot(z.T[:,0], z.T[:,1], linestyle='-', marker='x', color='b', label='observation') pl.legend(loc='lower right') pl.show() kf = KalmanFilter(transition_matrices = F, observation_matrices = [[1, 0], [0, 1]]) print('Model: {0}'.format(kf)) observations = z.T x = x.T states_pred = kf.em(observations).smooth(observations)[0] print('fitted model:{0}'.format(kf)) obs_scatter = pl.plot(observations[:,0], observations[:,1], linestyle='-', marker='x', color='b', label='observations') position_line = pl.plot(states_pred[:,0], states_pred[:, 1], linestyle='-', marker='o', color='r', label='position est.') pl.legend(loc='lower right') pl.show()
# If you already have good guesses for the initial parameters, put them in here. # The Kalman filter will try to learn the values of all variables # The Kalman Filter is parameterized by 3 arrays for state transitions, 3 for measurements, and 2 more for initial conditions. """ kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]), transition_covariance=0.01 * np.eye(2)) """ ## This worked kf = KalmanFilter(transition_matrices=np.array([[1, 1,0], [0, 1,0],[0,0,1]]), observation_matrices = [X[0],X[1],X[2]], transition_covariance=0.01 * np.eye(3)) kf = kf.em(X, n_iter=5) (filtered_state_means, filtered_state_covariances) = kf.filter(X) (smoothed_state_means, smoothed_state_covariances) = kf.smooth(X) # measurements = np.asarray(X) # You can use the kalman Filter immediately without fitting, but its estimates # may not be as good as if you fit first # The KalmanFilter class however can learn parameters using KalmanFilter.em() (fitting is optional). # Then the hidden sequence of states can be predicted using KalmanFilter.smooth(): # Parameters : Z : [n_timesteps, n_dim_state] array states_pred = kf
from pykalman import KalmanFilter n_timesteps = len(Gx) x = np.linspace(0, n_timesteps, n_timesteps) # observations[0] is Gx, etc. observations = [Gx,Gy,Gz] # create a Kalman Filter by hinting at the size of the state and observation # space. If you already have good guesses for the initial parameters, put them # in here. The Kalman Filter will try to learn the values of all variables. kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]), transition_covariance=0.01 * np.eye(2)) # You can use the Kalman Filter immediately without fitting, but its estimates # may not be as good as if you fit first. states_predx = kf.em(observations[0]).smooth(observations[0])[0] states_predy = kf.em(observations[1]).smooth(observations[1])[0] states_predz = kf.em(observations[2]).smooth(observations[2])[0] # In[56]: # Make plots fig = plt.figure(figsize=(16, 6)) ax1 = fig.add_subplot(211) plt.plot(x,observations[2], 'b-') plt.plot(x,states_predz[:, 0], 'r-',linewidth=2) plt.xlim(0,max(x)) plt.ylabel("G(z)",fontsize=20) plt.xlabel("time (s)",fontsize=20)
Lambda_KK = rn.uniform(-0.75, 0.75, size=(K, K)) # initialize KxK state transition matrix Phi_VK = rn.uniform(-0.75, 0.75, size=(V, K)) # initialize VxK observation matrix kf = KalmanFilter(observation_matrices=Phi_VK, transition_matrices=Lambda_KK, stabilize=False) # stabilize=True uses the SVD trick to make sure eigenvalues<1 em_vars = ['transition_covariance', 'transition_matrices', 'observation_matrices', 'observation_covariance', 'initial_state_covariance', 'initial_state_mean'] kf = kf.em(Y_TV, em_vars=em_vars) # fit the model Lambda_KK = kf.transition_matrices assert (np.abs(np.linalg.eigvals(Lambda_KK)) <= 1.).all() Phi_VK = kf.observation_matrices Sigma_KK = kf.transition_covariance D_VV = kf.observation_covariance # predict the test set (Y_SV) pred_Y_SV = np.zeros((S, V)) z_K = kf.smooth(Y_TV)[0][-1] for s in xrange(S): z_K = np.dot(Lambda_KK, z_K) pred_Y_SV[s] = np.dot(Phi_VK, z_K) print 'lds MAE: %.5f' % np.abs(N_SV - pred_Y_SV + E_V).mean()
# data.initial_transition_covariance, # data.initial_observation_covariance, # data.transition_offsets, # data.observation_offset, # data.initial_state_mean, # data.initial_state_covariance, # em_vars=[ # 'transition_matrices', 'observation_matrices', # 'transition_covariance', 'observation_covariance', # 'observation_offsets', 'initial_state_mean', # 'initial_state_covariance' # ] ) # Learn good values for parameters named in `em_vars` using the EM algorithm kf = kf.em(X=data.observations, n_iter=100) # loglikelihoods = np.zeros(10) # for i in range(len(loglikelihoods)): # kf = kf.em(X=data.observations, n_iter=1) # loglikelihoods[i] = kf.loglikelihood(data.observations) # # Estimate the state without using any observations. This will let us see how # # good we could do if we ran blind. # n_dim_state = data.transition_matrix.shape[0] # n_timesteps = data.observations.shape[0] # blind_state_estimates = np.zeros((n_timesteps, n_dim_state)) # for t in range(n_timesteps - 1): # if t == 0: # blind_state_estimates[t] = kf.initial_state_mean # blind_state_estimates[t + 1] = ( # np.dot(kf.transition_matrices, blind_state_estimates[t])
def apply(self, dataset): kf = KalmanFilter(initial_state_mean=0, n_dim_obs=len(dataset.data)) predicted = kf.em(dataset.data).smooth(dataset.data) return data
else: try: file = open(sys.argv[1], "r") except IOError: print("ERROR file") x, y, z = [], [], [] for line in file: accelerations = line.split() accelerations = list(map(float, accelerations)) x.append(accelerations[0]) y.append(accelerations[1]) z.append(accelerations[2]) file.close() # to write in file "password" lines = [] kf = KalmanFilter(transition_matrices=np.array([[1.15, 1], [0, 1]]), transition_covariance=0.01 * np.eye(2)) states_pred = kf.em(x).smooth(x)[0] filtered_x = states_pred[:, 0] lines.append(" ".join(list(map(str, filtered_x))) + "\n") kf = KalmanFilter(transition_matrices=np.array([[1.1, 0.5], [0, 1]]), transition_covariance=0.01 * np.eye(2)) states_pred = kf.em(y).smooth(y)[0] filtered_y = states_pred[:, 0] lines.append(" ".join(list(map(str, filtered_y))) + "\n") file = open("password", "w") file.writelines(lines) file.close()
x_ec_nans = x_ec.resample('D') print "Gaps in time series filled with NaNs." # use numpy mask for nans - required for pykalman x_ec_nans_masked = np.ma.masked_invalid(np.array(x_ec_nans)) # initialize dataframe for kalman-filtered data x_ec_int_kf = pd.DataFrame(np.zeros((x_ec_nans_masked.shape[0],x_ec_nans_masked.shape[1])),index=x_ec_nans.index,columns=x_ec_nans.columns) ### initialize kalman filter function for 1-D time series kf = KalmanFilter(n_dim_obs=1) ### run kalman filter - estimate parameters for each OTU (5 iterations) for i in range(x_ec_nans_masked.shape[1]): (filtered_state_means, filtered_state_covariances) = kf.em(x_ec_nans_masked[:,i], n_iter=5).filter(x_ec_nans_masked[:,i]) for x in range(x_ec_nans_masked.shape[0]): x_ec_int_kf.iloc[x,i] = np.array(filtered_state_means).flatten()[x] print "Kalman filtering and interpolation successful." ###log-transform kalman filtered data x_ec_int_kf_log = x_ec_int_kf.apply(np.log) print "Log transform successful." ###initialize dataframe for first-differenced data x_ec_int_kf_log_delta = x_ec_int_kf_log.iloc[:-1,:]
import random N = 3 * 500 sigma = 0.7 # Function to estimate W = numpy.zeros(N) W[0 : 500 ] = 1 W[500 : 1000 ] = 3 W[1000 : 1500 ] = 2 # Generate noisy measures X = numpy.zeros(N) for i in range(N): X[i] = W[i] + random.gauss(0, sigma) kf = KalmanFilter(initial_state_mean=X[0], n_dim_obs=1) kf.transition_covariance = 1e-3 Z = kf.em(X).smooth(X)[0] #Z = kf.smooth(X)[0] plt.figure(1) plt.plot( X, 'r:' ) plt.plot( W, 'b-' ) plt.plot( Z, 'g-' ) plt.show()
observation_matrix = [X[0],X[1],X[2]] # create a Kalman Filter by hinting at the size of the state and observation space. # If you already have good guesses for the initial parameters, put them in here. # The Kalman filter will try to learn the values of all variables # The Kalman Filter is parameterized by 3 arrays for state transitions, 3 for measurements, and 2 more for initial conditions. ## This worked kf = KalmanFilter(transition_matrices=transition_matrix, observation_matrices=observation_matix, transition_covariance=0.01 * np.eye(3) ) kf = kf.em(X, n_iter=5) (filtered_state_means, filtered_state_covariances) = kf.filter(X) (smoothed_state_means, smoothed_state_covariances) = kf.smooth(X) # kf.em(X).smooth([X]) ## this gives the shape error kf = kf.em(X).smooth(X) # This returns large tuple # print len(kf_test) # Plot lines for the observations without noise, the estimated position of the # target before fitting, and the estimated position after fitting. states_pred = kf n_timesteps = X.shape[0] x = np.linspace(0, 3 * np.pi, n_timesteps)
from pykalman import KalmanFilter x = np.array(cluster3) #z = x + np.array((np.random.normal(0,1,x.size)*0.1).reshape(x.shape[0], x.shape[1])) x = x z = x kf = KalmanFilter() kf = kf.em(x, n_iter=5) state_pred=kf.em(z).smooth(z)[0] print('fitted model:{0}'.format(kf)) plt.imshow(gray, cmap=plt.cm.gray) obs_scatter = plt.plot(z[:,0], z[:,1], linestyle='-', marker='x', color='b', label='observations') position_line = plt.plot(states_pred[:,0], states_pred[:, 1], linestyle='-', marker='o', color='r', label='position est.') pl.legend(loc='lower right') pl.show()
def pykalman_test(): kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]]) measurements = numpy.asarray([[1,0], [0,0], [0,1]]) # 3 observations kf = kf.em(measurements, n_iter=5) print "pykalman test >>>", kf.filter(measurements)
) # sample from model kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, transition_offset, observation_offset, initial_state_mean, initial_state_covariance, random_state=random_state, em_vars=[ 'transition_matrices', 'observation_matrices', 'transition_covariance', 'observation_covariance', 'observation_offsets', 'initial_state_mean', 'initial_state_covariance' ] ) kf = kf.em(X=observations, n_iter=50) # estimate state with filtering and smoothing filtered_state_estimates = kf.filter(observations)[0] smoothed_state_estimates = kf.smooth(observations)[0] # draw estimates pl.figure() lines_true = pl.plot(states, color='b') lines_filt = pl.plot(filtered_state_estimates, color='r') lines_smooth = pl.plot(smoothed_state_estimates, color='g') pl.legend((lines_true[0], lines_filt[0], lines_smooth[0]), ('true', 'filt', 'smooth'), loc='lower right' ) pl.show()
#%% Plot distance to manually identify start/stop indicies #plt.plot(pythagoras(one_taxi.latitude[0:101], one_taxi.longitude[0:101])) #%% Kalman Filter and plot results #kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]]) start_index = 38 end_index = 69 measurements = np.asarray([one_taxi.longitude, one_taxi.latitude]) measurements = measurements.T[start_index:end_index] kf = KalmanFilter(initial_state_mean = measurements[0], \ n_dim_obs=2, n_dim_state=2) kf = kf.em(measurements) #(filtered_state_means, filtered_state_covariances) = kf.filter(measurements) (smoothed_state_means, smoothed_state_covariances) = kf.smooth(measurements) #plt.plot(filtered_state_means.T[0],filtered_state_means.T[1]) plt.plot(smoothed_state_means.T[0],smoothed_state_means.T[1]) plt.title("One Trip smoothed with Karman Filter") plt.xlabel('Longitude (degrees)') plt.ylabel('Latitude (degrees)') print "Taxi data for Taxi ID: %d" % one_taxi.taxi_id[0] print "Start date and time: ", one_taxi.date_time[start_index] print "Duration:", one_taxi.date_time[end_index] - one_taxi.date_time[start_index] #%% Kalman Filter and plot results #kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]])
from pykalman import KalmanFilter kf = KalmanFilter(initial_state_mean=0, n_dim_obs=2) measurements = [[1,0], [0,0], [0,1]] kf.em(measurements).smooth([[2,0], [2,1], [2,2]])[0] array([[ 0.85819709], [ 1.77811829], [ 2.19537816]])