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 smooth_test(coef, sysinfo): X_valid, y_valid = sysinfo[X_columns], sysinfo[y_column] # Preset in hint transition_stddev = 2.0 observation_stddev = 2.0 dims = X_valid.shape[-1] initial = X_valid.iloc[0] observation_covariance = np.diag([observation_stddev, 2, 10, 1])**2 transition_covariance = np.diag([transition_stddev, 80, 100, 10])**2 # Transition = identity for all variables, except we'll replace the top row # to make a better prediction, which was the point of all this. transition = np.eye(dims) # identity matrix, except... transition[0] = coef # replace top row kf = KalmanFilter( initial_state_mean=initial, initial_state_covariance=observation_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance, transition_matrices=transition, ) kalman_smoothed, _ = kf.smooth(X_valid) plt.figure(figsize=(15, 6)) plt.plot(sysinfo['timestamp'], sysinfo['temperature'], 'b.', alpha=0.5) plt.plot(sysinfo['timestamp'], kalman_smoothed[:, 0], 'g-') plt.savefig('smoothed.png')
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 main(): with open(sys.argv[1], "r") as fin: tracks = cPickle.load(fin) print "%d tracks loaded."%len(tracks) index = 5 measurements = [] track = tracks[index] t0 = track.utm[0][2]/1e6 for pt in track.utm: t = pt[2]/1e6 - t0 measurements.append([pt[0], pt[1]]) measurements = np.asarray(measurements) kf = KalmanFilter(n_dim_obs=2, n_dim_state=2).em(measurements, n_iter=100) results = kf.smooth(measurements)[0] fig = plt.figure(figsize=(9,9)) ax = fig.add_subplot(111, aspect='equal') ax.plot([pt[0] for pt in results], [pt[1] for pt in results], 'r-', linewidth=2) ax.plot([pt[0] for pt in track.utm], [pt[1] for pt in track.utm], 'x-') #ax.set_xlim([const.SF_small_RANGE_SW[0], const.SF_small_RANGE_NE[0]]) #ax.set_ylim([const.SF_small_RANGE_SW[1], const.SF_small_RANGE_NE[1]]) plt.show()
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 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 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 main(): points = ET.parse(sys.argv[1]) root = points.getroot() coordinates = pd.DataFrame( columns = ['lat', 'lon']) for trkpt in root.findall('.//{http://www.topografix.com/GPX/1/0}trkpt'): lat = float(trkpt.get('lat')) lon = float(trkpt.get('lon')) latlon = {'lat' : [lat], 'lon' : [lon]} df = pd.DataFrame(data = latlon) coordinates = coordinates.append(df) distance(coordinates) print('Unfiltered distance: %0.2f' % (distance(coordinates))) kalman_data = coordinates initial_state = kalman_data.iloc[0] observation_covariance = np.diag([0.55, 0.55]) ** 2 # TODO: shouldn't be zero transition_covariance = np.diag([0.5, 0.5]) ** 2 # TODO: shouldn't be zero transition = [[1, 0], [0, 1]] # TODO: shouldn't (all) be zero kf = KalmanFilter( initial_state_mean=initial_state, initial_state_covariance=observation_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance, transition_matrices=transition ) smoothed_points, _ = (kf.smooth(kalman_data.values)) smoothed_data = pd.DataFrame(smoothed_points, columns = ['lat', 'lon']) print('Filtered distance: %0.2f' % (distance(smoothed_data))) output_gpx(smoothed_data, 'out.gpx')
def smooth_features(feature_data, method='LDS'): """ Input: feature_data: (channel_N, frequency_band_N ,sample_point_N) feature array method: 'LDS': Kalman Smooth (Linear Dynamic System) 'moving_avg': Moving average method Output: Smoothed data which has the same shape as input """ smoothed_data = np.zeros_like(feature_data) state_mean = np.mean(feature_data, axis=2) for channel_index in range(feature_data.shape[0]): for feature_band_index in range(feature_data.shape[1]): kf = KalmanFilter(transition_matrices=1, observation_matrices=1, transition_covariance=0.001, observation_covariance=1, initial_state_covariance=0.1, initial_state_mean=state_mean[channel_index, feature_band_index]) measurement = feature_data[channel_index, feature_band_index, :] smoothed_data[channel_index, feature_band_index, :] = kf.smooth(measurement)[0].flatten() return smoothed_data
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 smooth_test(coef, sysinfo): X_test, y_test = sysinfo[X_columns], sysinfo[y_column] # feel free to tweak these if you think it helps. transition_stddev = 2.0 observation_stddev = 2.0 dims = X_test.shape[-1] initial = X_test.iloc[0] observation_covariance = np.diag([observation_stddev, 2, 10, 1])**2 transition_covariance = np.diag([transition_stddev, 80, 100, 10])**2 # Transition = identity for all variables, except we'll replace the top row # to make a better prediction, which was the point of all this. transition = np.eye(dims) # identity matrix, except... # TODO: replace the first row of transition to use the coefficients we just calculated (which were passed into this function as coef.). transition[0] = coef kf = KalmanFilter( initial_state_mean=initial, initial_state_covariance=observation_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance, transition_matrices=transition, ) kalman_smoothed, _ = kf.smooth(X_test) plt.figure(figsize=(15, 6)) plt.plot(sysinfo['timestamp'], sysinfo['temperature'], 'b.', alpha=0.5) plt.plot(sysinfo['timestamp'], kalman_smoothed[:, 0], 'g-') plt.savefig('smoothed.png')
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 smooth_test(coef): """ Do a Kalman filter on the test data, using the prediction derived from the regression. """ sysinfo, X_test, _ = get_data(testing_data_file) # feel free to tweak these if you think it helps. transition_stddev = 1.5 observation_stddev = 2.0 dims = X_test.shape[-1] kalman_data = X_test initial = X_test[0] observation_covariance = np.diag([observation_stddev, 2, 10, 1])**2 transition_covariance = np.diag([transition_stddev, 80, 100, 10])**2 # TODO: update transition to incorporate coef # ... except temperature, which was the point of all this. transition = np.eye(dims) # transition = identity for all variables transition = transition * coef # Multiply the coefficients onto the indentity matrix .. transition[0] = [1, 0, 0, 0] # .. but don't touch the temperature kf = KalmanFilter( initial_state_mean=initial, initial_state_covariance=observation_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance, transition_matrices=transition, ) kalman_smoothed, _ = kf.smooth(kalman_data) plt.figure(figsize=(12, 4)) plt.plot(sysinfo['timestamp'], sysinfo['temperature'], 'b.', alpha=0.5) plt.plot(sysinfo['timestamp'], kalman_smoothed[:, 0], 'g-') plt.savefig('smoothed.png')
def smooth_test(coef): sysinfo, X_test, _ = get_data(testing_data_file) # feel free to tweak these if you think it helps. # transition_stddev = 1.5 # observation_stddev = 2.0 # transition_stddev = 2.5 # observation_stddev = 1.0 transition_stddev = 0.5 observation_stddev = 3.0 dims = X_test.shape[-1] kalman_data = X_test initial = X_test[0] observation_covariance = np.diag([observation_stddev, 2, 10, 1])**2 transition_covariance = np.diag([transition_stddev, 80, 100, 10])**2 transition = [[1, 0, 0, 0], [0, coef[1], 0, 0], [0, 0, coef[2], 0], [0, 0, 0, coef[3]]] kf = KalmanFilter( initial_state_mean=initial, initial_state_covariance=observation_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance, transition_matrices=transition, ) kalman_smoothed, _ = kf.smooth(kalman_data) plt.figure(figsize=(12, 4)) plt.plot(sysinfo['timestamp'], sysinfo['temperature'], 'b.', alpha=0.5) plt.plot(sysinfo['timestamp'], kalman_smoothed[:, 0], 'g-') plt.savefig('smoothed.png')
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 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 smooth_test(coef): """ Do a Kalman filter on the test data, using the prediction derived from the regression. """ weatherAttributes, X_test, _ = get_data2() transition_stddev = 1.5 observation_stddev = 4 dims = X_test.shape[-1] kalman_data = X_test initial = X_test[0] observation_covariance = np.diag([observation_stddev, 2, 10, 1]) ** 2 transition_covariance = np.diag([transition_stddev, 80, 100, 10]) ** 2 transition = np.diag(coef) # transition = identity for all variables # TODO: update transition matrix, using coef to predict the temperature kf = KalmanFilter( initial_state_mean=initial, initial_state_covariance=observation_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance, transition_matrices=transition, ) kalman_smoothed, _ = kf.smooth(kalman_data) plt.figure(figsize=(12, 4)) plt.plot(weatherAttributes['Date/Time'], weatherAttributes['Temp (°C)'], 'b.', alpha=0.5, label='Temperature (°C)') plt.plot(weatherAttributes['Date/Time'], kalman_smoothed[:, 0], 'g-', label='Feels Like (°C)') plt.xlabel('Dates') plt.ylabel('Temp (°C)') plt.title('Filter on Temp(°C) using Regression ') plt.legend() plt.savefig('smoothed.png')
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 autoLabel(self): sonarKey = 'sonar-observation:Value' smoothedKey = 'Sonar:Smoothed' arrays = self.__consecutive(np.where(self.df[sonarKey] == 0)[0]) mask = np.zeros(len(self.df[sonarKey])) self.df[smoothedKey] = self.df[sonarKey] self.df.loc[np.isnan(self.df[smoothedKey]), smoothedKey] = self.labelConf.maxDistanceMarker masked = ma.masked_array(self.df[smoothedKey], mask=mask) # # Assume that if there are multiple zeros in a row this indicates that the # there is nothing there. Obstacles are beyond the max range of the sensor. # If there are too few measurements in a row, extend the last measurement. if len(arrays[0]) > 0: for array in arrays: masked.mask[array] = 1 if len(array) > self.labelConf.consecutiveZeroForPositive: self.df.loc[array, ( smoothedKey)] = self.labelConf.maxDistanceMarker else: self.df.loc[array, (smoothedKey)] = self.df[sonarKey][max( array[0] - 1, 0)] kf = KalmanFilter([1], [1], [0.2**2], [0.2**2]) means, cov = kf.smooth(masked) # # Create the labels based on a distance threshold. self.df[smoothedKey] = np.squeeze(means)
def main(): filename = sys.argv[1] cpu_data = pd.read_csv(filename, parse_dates=['timestamp']) # Loess loess_smoothed = lowess(cpu_data['temperature'], cpu_data['timestamp'], frac=0.02) # Kalman kalman_data = cpu_data[['temperature', 'cpu_percent', 'sys_load_1', 'fan_rpm']] initial_state = kalman_data.iloc[0] observation_covariance = np.diag([0.6, 1, 0.3 ,60]) ** 2 transition_covariance = np.diag( [0.01, 0.01, 0.03 ,7]) ** 2 transition_matrix = [[0.97,0.5,0.2,-0.001],[0.1,0.4,2.2,0],[0,0,0.95,0],[0,0,0,1]] kf = KalmanFilter( initial_state_mean=initial_state, initial_state_covariance=observation_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance, transition_matrices=transition_matrix ) kalman_smoothed, _ = kf.smooth(kalman_data) #Reference: https://matplotlib.org/devdocs/gallery/subplots_axes_and_figures/subplots_demo.html fig, (ax1,ax2) = plt.subplots(2, figsize=(12, 5), sharex=True) ax1.set_title("Loess") ax1.plot(cpu_data['timestamp'].values, cpu_data['temperature'].values, 'b.', alpha=0.5) ax1.plot(cpu_data['timestamp'].values, loess_smoothed[:, 1], 'r-') ax2.set_title("Kalman") ax2.plot(cpu_data['timestamp'].values, cpu_data['temperature'].values, 'b.', alpha=0.5) ax2.plot(cpu_data['timestamp'].values, kalman_smoothed[:, 0], 'r-') fig.savefig('cpu.svg')
def smooth_test(coef): """ Do a Kalman filter on the test data, using the prediction derived from the regression. """ sysinfo, X_test, _ = get_data(testing_data_file) # feel free to tweak these if you think it helps. transition_stddev = 6 observation_stddev = 0.1 dims = X_test.shape[-1] kalman_data = X_test initial = X_test[0] observation_covariance = np.diag([observation_stddev, 2, 10, 1])**2 transition_covariance = np.diag([transition_stddev, 80, 100, 10])**2 transition = np.diag(coef) # transition = identity for all variables # TODO: update transition matrix, using coef to predict the temperature kf = KalmanFilter( initial_state_mean=initial, initial_state_covariance=observation_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance, transition_matrices=transition, ) kalman_smoothed, _ = kf.smooth(kalman_data) plt.figure(figsize=(12, 4)) plt.plot(sysinfo['timestamp'], sysinfo['temperature'], 'b.', alpha=0.5) plt.plot(sysinfo['timestamp'], kalman_smoothed[:, 0], 'g-') plt.savefig('smoothed.png')
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 main(): filename = sys.argv[1] cpu_data = pd.read_csv(filename, parse_dates=[4]) plt.figure(figsize=(12, 4)) plt.plot(cpu_data['timestamp'], cpu_data['temperature'], 'b.', alpha=0.5) loess_smoothed = lowess(cpu_data['temperature'], cpu_data['timestamp'], frac=0.035) plt.plot(cpu_data['timestamp'], loess_smoothed[:, 1], 'r-') observation_covariance = np.diag([ cpu_data['temperature'].std(), cpu_data['cpu_percent'].std(), cpu_data['sys_load_1'].std() ])**2 kalman_data = cpu_data[['temperature', 'cpu_percent', 'sys_load_1']] initial_value_guess = kalman_data.iloc[0] transition_matrix = [[1, 0, 0], [0, 0.6, 0], [0, 0, 0.8]] transition_covariance = np.diag([0.3, 0.3, 0.3])**2 kf = KalmanFilter(initial_state_mean=initial_value_guess, initial_state_covariance=observation_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance, transition_matrices=transition_matrix) kalman_smoothed, _ = kf.smooth(kalman_data) plt.plot(cpu_data['timestamp'], kalman_smoothed[:, 0], 'g-') plt.legend(['scatterplot', 'LOESS Filtering', 'Kalman FIltering']) plt.savefig('cpu.svg')
def smooth_test(coef): sysinfo, X_test, _ = get_data(testing_data_file) # feel free to tweak these if you think it helps. transition_stddev = 1.5 observation_stddev = 2.0 dims = X_test.shape[-1] kalman_data = X_test initial = X_test[0] observation_covariance = np.diag([observation_stddev, 2, 10, 1])**2 transition_covariance = np.diag([transition_stddev, 80, 100, 10])**2 transition = np.eye(dims) # transition = identity for all variables transition[0] = coef # TODO: update transition to incorporate coef # ... except temperature, which was the point of all this. kf = KalmanFilter( initial_state_mean=initial, initial_state_covariance=observation_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance, transition_matrices=transition, ) kalman_smoothed, _ = kf.smooth(kalman_data) plt.figure(figsize=(12, 4)) plt.plot(sysinfo['timestamp'], sysinfo['temperature'], 'b.', alpha=0.5) plt.plot(sysinfo['timestamp'], kalman_smoothed[:, 0], 'g-') plt.savefig('smoothed.png')
def smooth(points): ''' Find the Kalman smoothed distance using the same points. (Based on Exercise 3) ''' kalman_data = points[['lat', 'lon']] initial_state = kalman_data.iloc[0] # GPS can be accurate to about 5 metres but the reality seems to be several times that: maybe 15 or 20 metres with my phone. observation_covariance = np.diag([.0175, .0175])**2 # Without any knowledge of what direction I was walking, we assume that my current position is the same as my previous position. transition_matrices = [[1, 0], [0, 1]] # I usually walk something like 1 m/s and the data contains an observation about every 10 s. transition_covariance = np.diag([0.01, 0.01])**2 # Kalman Filter parameters kf = KalmanFilter(initial_state_mean=initial_state, initial_state_covariance=observation_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance, transition_matrices=transition_matrices) kalman_smoothed, _ = kf.smooth(kalman_data) # Create the kalman dataframe kalman_df = pd.DataFrame(data=kalman_smoothed[:], columns=['lat', 'lon']) return kalman_df
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(x): series = [x['sales_0'], x['sales_1'], x['sales_2'], x['sales_3'], x['sales_4'], x['sales_5'], x['sales_6'], x['sales_7'], x['sales_8'], x['sales_9'], x['sales_10'], x['sales_11'], x['sales_12'], x['sales_13'], x['sales_14']] kf = KalmanFilter(n_dim_obs=1, n_dim_state=1, initial_state_mean=series[0]) state_means, state_covariance = kf.smooth(series) return state_means.ravel().tolist()
def interpusblposition(usbl): # ok lest make sure the GPS is on the second usbl = usbl.drop_duplicates() timesteps = np.diff(usbl.index) / np.timedelta64(1, 's') Transition_Matrix = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]]) temp = np.zeros((len(timesteps), 4, 4)) for i in range(len(timesteps)): Transition_Matrix[0, 2] = timesteps[i] Transition_Matrix[1, 3] = timesteps[i] temp[i] = Transition_Matrix Transition_Matrix = temp mask = usbl['UsblNorthing'][:-1].isna() lat = ma.array(usbl['UsblNorthing'][:-1], mask=mask) lon = ma.array(usbl['UsblEasting'][:-1], mask=mask) #print(timesteps.min()) lonSpeed = ma.array(np.diff(usbl.ShipEasting) / timesteps) lonSpeed[np.isnan(lonSpeed)] = 0 latSpeed = ma.array(np.diff(usbl.ShipNorthing) / timesteps) latSpeed[np.isnan(latSpeed)] = 0 coord = ma.dstack((lon, lat, lonSpeed, latSpeed))[0] Observation_Matrix = np.eye(4) xinit = lon[0] yinit = lat[0] vxinit = lonSpeed[0] vyinit = latSpeed[0] initstate = [xinit, yinit, vxinit, vyinit] #print(initstate) initcovariance = 1.0e-3 * np.eye(4) transistionCov = 1.0e-3 * np.eye(4) observationCov = np.eye(4) observationCov[0, 0] = 15 observationCov[1, 1] = 15 observationCov[2, 2] = 0.01 observationCov[3, 3] = 0.01 #print('kman') kf = KalmanFilter(transition_matrices=Transition_Matrix, observation_matrices=Observation_Matrix, initial_state_mean=initstate, initial_state_covariance=initcovariance, transition_covariance=transistionCov, observation_covariance=observationCov) output = kf.smooth(coord)[0] result = usbl result['KfUsblNorthing'] = np.append(output[:, 1], output[-1, 1]) result['KfUsblEasting'] = np.append(output[:, 0], output[-1, 0]) myProj = Proj( "+proj=utm +zone=55F, +south +ellps=WGS84 +datum=WGS84 +units=m +no_defs" ) result['KfUsblLongitude'], result['KfUsblLatitude'] = myProj( result['KfUsblEasting'].values, result['KfUsblNorthing'].values, inverse='True') return result
def smooth(dr): '''Smoothing with Kalman Filter''' kf = KalmanFilter() for t in range(1,201): if not os.path.exists('drivers/%d/%d_smooth.csv'%(dr,t)): d = np.genfromtxt('drivers/%d/%d.csv'%(dr,t), delimiter=',', skip_header=True) d[:,0] = kf.smooth(d[:,0])[0].T[0] d[:,1] = kf.smooth(d[:,1])[0].T[0] np.savetxt('drivers/%d/%d_smooth.csv'%(dr,t), d, delimiter=',', header='x,y', fmt='%.1f')
def kalman_filter(obs, obs_cov=1, trans_cov=0.1): obs = list(obs) kf = KalmanFilter(initial_state_mean=obs[0], initial_state_covariance=obs_cov, observation_covariance=obs_cov, observation_matrices=1, transition_covariance=trans_cov, transition_matrices=1) pred_state, state_cov = kf.smooth(obs) return (pred_state)
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 kalman_smooth(observations, **kwargs): ''' Smooth shit ''' kwargs.setdefault('initial_state_mean', BASE_SCORE) kwargs.setdefault('transition_covariance', 0.01 * np.eye(1)) kf = KalmanFilter(**kwargs) states_pred = kf.smooth(observations)[0] return states_pred[:, 0]
def test_kalman_predict(): 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) x_smooth = kf.smooth(X=data.observations)[0] assert_array_almost_equal( x_smooth[:501], data.smoothed_state_means[:501], decimal=7 )
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 pyKalman4DTest(params, greater, samples): kp = params['kparams'] #kp['initial_state_mean']=[quadsummary([s['unusual_packet'] for s in samples]), # quadsummary([s['other_packet'] for s in samples]), # numpy.mean([s['unusual_tsval'] for s in samples]), # numpy.mean([s['other_tsval'] for s in samples])] kf = KalmanFilter(n_dim_obs=4, n_dim_state=4, **kp) smooth,covariance = kf.smooth([(s['unusual_packet'],s['other_packet'],s['unusual_tsval'],s['other_tsval']) for s in samples]) m = numpy.mean(smooth) if greater: if m > params['threshold']: return 1 else: return 0 else: if m < params['threshold']: return 1 else: return 0
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()
random_state=0 ) states, observations_all = kf.sample( n_timesteps, initial_state=initial_state_mean ) # label half of the observations as missing observations_missing = np.ma.array( observations_all, mask=np.zeros(observations_all.shape) ) for t in range(n_timesteps): if t % 5 != 0: observations_missing[t] = np.ma.masked # estimate state with filtering and smoothing smoothed_states_all = kf.smooth(observations_all)[0] smoothed_states_missing = kf.smooth(observations_missing)[0] # draw estimates pl.figure() lines_true = pl.plot(states, color='b') lines_smooth_all = pl.plot(smoothed_states_all, color='r') lines_smooth_missing = pl.plot(smoothed_states_missing, color='g') pl.legend( (lines_true[0], lines_smooth_all[0], lines_smooth_missing[0]), ('true', 'all', 'missing'), loc='lower right' ) pl.show()
#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]]) start_index = 0 end_index = 800
def speakerTracker(self): # Clean up cv2.destroyAllWindows() if self.inputPath is not None: # If a path to a file was given, assume it is a single video file if os.path.isfile(self.inputPath): cap = cv2.VideoCapture(self.inputPath) clip = VideoFileClip(self.inputPath,audio=False) fps = cap.get(cv2.cv.CV_CAP_PROP_FPS) self.numFrames = cap.get(cv2.cv.CV_CAP_PROP_FRAME_COUNT) print "[speakerTracker] Number of frames" , self.numFrames pathBase = os.path.basename(self.inputPath) pathDirectory = os.path.dirname(self.inputPath) baseName = pathDirectory + '/' + os.path.splitext(pathBase)[0] + '_' #Skip first frames if required if self.skip is not None: cap.set(cv2.cv.CV_CAP_PROP_POS_FRAMES, self.skip) # Otherwise assume it is a format string for reading images else: cap = cmtutil.FileVideoCapture(self.inputPath) #Skip first frames if required if self.skip is not None: cap.frame = 1 + self.skip else: # If no input path was specified, open camera device sys.exit("[speakerTracker] Error: no input path was specified") # The number of pixels from the center of object till the border of cropped image marginPixels = 300 # Read first frame status, im0 = cap.read() imGray0 = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY) imDraw = np.copy(im0) (tl0, br0) = cmtutil.get_rect(imDraw) print '[speakerTracker] Using', tl0, br0, 'as initial bounding box for the speaker' # First initialization to get the center self.CMT.initialise(imGray0, tl0, br0) # The first x and y coordinates of the object of interest self.inity = tl0[1] - self.CMT.center_to_tl[1] self.initx = tl0[0] - self.CMT.center_to_tl[0] # Crop the first frame imGray0_initial = imGray0[self.inity - marginPixels : self.inity + marginPixels, self.initx - marginPixels : self.initx + marginPixels] # Calculate the translation vector from main image to the cropped frame self.originFromMainImageY = self.inity - marginPixels self.originFromMainImageX = self.initx - marginPixels # Calculate the position of the selected rectangle in the cropped frame tl = (tl0[0] - self.originFromMainImageX , tl0[1] - self.originFromMainImageY) br = (br0[0] - self.originFromMainImageX , br0[1] - self.originFromMainImageY) #print '[speakerTracker] Using', tl, br, 'as initial bounding box for the speaker' # initialization and keypoint calculation self.CMT.initialise(imGray0_initial, tl, br) # Center of object in cropped frame self.currentY = tl[1] - self.CMT.center_to_tl[1] self.currentX = tl[0] - self.CMT.center_to_tl[0] # Center of object in main frame self.currentYMainImage = self.currentY + self.originFromMainImageY self.currentXMainImage = self.currentX + self.originFromMainImageX measuredTrack=np.zeros((self.numFrames+10,2))-1 count =0 # loop to read all frames, # crop them with the center of last frame, # calculate keypoints and center of the object for frame in clip.iter_frames(): # Read the frame and convert it to gray scale im_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Corner correction (Height) if (self.currentYMainImage + marginPixels >= im_gray.shape[0]): self.currentYMainImage = im_gray.shape[0] - marginPixels -1 else: self.currentYMainImage = self.currentYMainImage if (self.currentXMainImage + marginPixels >= im_gray.shape[1]): self.currentXMainImage = im_gray.shape[1] - marginPixels -1 else: self.currentXMainImage = self.currentXMainImage if (self.currentYMainImage - marginPixels <= 0): self.currentYMainImage = 0 + marginPixels +1 else: self.currentYMainImage = self.currentYMainImage if (self.currentXMainImage - marginPixels <= 0): self.currentXMainImage = 0 + marginPixels +1 else: self.currentXMainImage = self.currentXMainImage # Crop it by previous coordinates im_gray_crop = im_gray[self.currentYMainImage - marginPixels : self.currentYMainImage + marginPixels, self.currentXMainImage - marginPixels : self.currentXMainImage + marginPixels] #plt.imshow(im_gray_crop, cmap = cm.Greys_r) #plt.show() #print "self.currentYMainImage:", self.currentYMainImage #print "self.currentXMainImage:", self.currentXMainImage #print im_gray_crop.shape # Compute all keypoints in the cropped frame self.CMT.process_frame(im_gray_crop) #print 'frame: {2:4d}, Center: {0:.2f},{1:.2f}'.format(self.CMT.center[0], self.CMT.center[1] , count) if not (math.isnan(self.CMT.center[0]) or math.isnan(self.CMT.center[1]) or (self.CMT.center[0] <= 0) or (self.CMT.center[1] <= 0)): # Compute the center of the object with respect to the main image self.diffY = self.CMT.center[0] - self.currentY self.diffX = self.CMT.center[1] - self.currentX self.currentYMainImage = self.diffY + self.currentYMainImage self.currentXMainImage = self.diffX + self.currentXMainImage self.currentY = self.CMT.center[0] self.currentX = self.CMT.center[1] # Save the center of frames in an array for further process measuredTrack[count,0] = self.currentYMainImage measuredTrack[count,1] = self.currentXMainImage else: self.currentYMainImage = self.currentYMainImage self.currentXMainImage = self.currentXMainImage print 'frame: {2:4d}, Center: {0:.2f},{1:.2f}'.format(self.currentYMainImage, self.currentXMainImage , count) count += 1 numMeas=measuredTrack.shape[0] markedMeasure=np.ma.masked_less(measuredTrack,0) # Kalman Filter Parameters deltaT = 1.0/clip.fps transitionMatrix=[[1,0,deltaT,0],[0,1,0,deltaT],[0,0,1,0],[0,0,0,1]] #A observationMatrix=[[1,0,0,0],[0,1,0,0]] #C xinit = markedMeasure[0,0] yinit = markedMeasure[0,1] vxinit = markedMeasure[1,0]-markedMeasure[0,0] vyinit = markedMeasure[1,1]-markedMeasure[0,1] initState = [xinit,yinit,vxinit,vyinit] #mu0 initCovariance = 1.0e-3*np.eye(4) #sigma0 transistionCov = 1.0e-4*np.eye(4) #Q observationCov = 1.0e-1*np.eye(2) #R # Kalman Filter bias kf = KalmanFilter(transition_matrices = transitionMatrix, observation_matrices = observationMatrix, initial_state_mean = initState, initial_state_covariance = initCovariance, transition_covariance = transistionCov, observation_covariance = observationCov) self.measuredTrack = measuredTrack # Kalman Filter (self.filteredStateMeans, self.filteredStateCovariances) = kf.filter(markedMeasure) # Kalman Smoother (self.filterStateMeanSmooth, self.filterStateCovariancesSmooth) = kf.smooth(markedMeasure) newClip = clip.fl_image( self.crop ) return newClip
def speakerTracker(self): # Clean up cv2.destroyAllWindows() if self.inputPath is not None: # If a path to a file was given, assume it is a single video file if os.path.isfile(self.inputPath): cap = cv2.VideoCapture(self.inputPath) clip = VideoFileClip(self.inputPath,audio=False) fps = cap.get(cv2.cv.CV_CAP_PROP_FPS) self.numFrames = cap.get(cv2.cv.CV_CAP_PROP_FRAME_COUNT) print "[speakerTracker] Number of frames" , self.numFrames pathBase = os.path.basename(self.inputPath) pathDirectory = os.path.dirname(self.inputPath) baseName = pathDirectory + '/' + os.path.splitext(pathBase)[0] + '_' # Otherwise assume it is a format string for reading images else: cap = cmtutil.FileVideoCapture(self.inputPath) else: # If no input path was specified, open camera device sys.exit("[speakerTracker] Error: no input path was specified") # Read first frame status, im0 = cap.read() imGray0 = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY) imDraw = np.copy(im0) (tl, br) = cmtutil.get_rect(imDraw) print '[speakerTrackering] Using', tl, br, 'as initial bounding box for the speaker' self.CMT.initialise(imGray0, tl, br) #self.inity = tl[1] - self.CMT.center_to_tl[1] measuredTrack=np.zeros((self.numFrames+10,2))-1 count =0 for frame in clip.iter_frames(): im_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) self.CMT.process_frame(im_gray) print 'frame: {2:4d}, Center: {0:.2f},{1:.2f}'.format(self.CMT.center[0], self.CMT.center[1] , count) #print self.inity if not (math.isnan(self.CMT.center[0]) or (self.CMT.center[0] <= 0)): measuredTrack[count,0] = self.CMT.center[0] measuredTrack[count,1] = self.CMT.center[1] count += 1 numMeas=measuredTrack.shape[0] markedMeasure=np.ma.masked_less(measuredTrack,0) # Kalman Filter Parameters deltaT = 1.0/clip.fps transitionMatrix=[[1,0,deltaT,0],[0,1,0,deltaT],[0,0,1,0],[0,0,0,1]] #A observationMatrix=[[1,0,0,0],[0,1,0,0]] #C xinit = markedMeasure[0,0] yinit = markedMeasure[0,1] vxinit = markedMeasure[1,0]-markedMeasure[0,0] vyinit = markedMeasure[1,1]-markedMeasure[0,1] initState = [xinit,yinit,vxinit,vyinit] #mu0 initCovariance = 1.0e-3*np.eye(4) #sigma0 transistionCov = 1.0e-4*np.eye(4) #Q observationCov = 1.0e-1*np.eye(2) #R kf = KalmanFilter(transition_matrices = transitionMatrix, observation_matrices = observationMatrix, initial_state_mean = initState, initial_state_covariance = initCovariance, transition_covariance = transistionCov, observation_covariance = observationCov) self.measuredTrack = measuredTrack (self.filteredStateMeans, self.filteredStateCovariances) = kf.filter(markedMeasure) (self.filterStateMeanSmooth, self.filterStateCovariancesSmooth) = kf.smooth(markedMeasure) self.inity = np.mean(self.filterStateMeanSmooth[:][1], axis=0) newClip = clip.fl_image( self.crop ) return newClip
# 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) xobservations = 20 * (np.sin(x) + 0.5 * rnd.randn(n_timesteps)) observations['noise'] = x
def trainPyKalman4D(db, unusual_case, greater, num_observations): global _pykalman4d_params global _pykalman4d_good_threshold db.resetOffsets() if _pykalman4d_params == None: train = db.subseries('train',unusual_case, offset=0) null = db.subseries('train_null',unusual_case, offset=0) train_array = numpy.asarray([(s['unusual_packet'],s['other_packet'],s['unusual_tsval'],s['other_tsval']) for s in train]) null_array = numpy.asarray([(s['unusual_packet'],s['other_packet'],s['unusual_tsval'],s['other_tsval']) for s in null]) kf = KalmanFilter(n_dim_obs=4, n_dim_state=4) #initial_state_mean=[quadsummary([s['unusual_packet'] for s in train]), # quadsummary([s['other_packet'] for s in train]), # numpy.mean([s['unusual_tsval'] for s in train]), # numpy.mean([s['other_tsval'] for s in train])]) kf = kf.em(train_array+null_array[0:50000], n_iter=10, em_vars=('transition_matrices', 'observation_matrices', 'transition_offsets', 'observation_offsets', 'transition_covariance', 'observation_covariance', 'initial_state_covariance')) _pykalman4d_params = {'transition_matrices': kf.transition_matrices.tolist(), 'observation_matrices': kf.observation_matrices.tolist(), 'transition_offsets': kf.transition_offsets.tolist(), 'observation_offsets': kf.observation_offsets.tolist(), 'transition_covariance': kf.transition_covariance.tolist(), 'observation_covariance': kf.observation_covariance.tolist(), 'initial_state_mean': kf.initial_state_mean.tolist(), 'initial_state_covariance': kf.initial_state_covariance.tolist()} print(_pykalman4d_params) kf = KalmanFilter(n_dim_obs=4, n_dim_state=4, **_pykalman4d_params) smoothed,covariance = kf.smooth(train_array) null_smoothed,covariance = kf.smooth(null_array) kp = _pykalman4d_params.copy() #kp['initial_state_mean']=[quadsummary([s['unusual_packet'] for s in train]), # quadsummary([s['other_packet'] for s in train]), # numpy.mean([s['unusual_tsval'] for s in train]), # numpy.mean([s['other_tsval'] for s in train])] #kf = KalmanFilter(n_dim_obs=4, n_dim_state=4, **kp) #null_smoothed,covariance = kf.smooth(null_array) _pykalman4d_good_threshold = (numpy.mean([m[0]-m[1] for m in smoothed])+numpy.mean([m[0]-m[1] for m in null_smoothed]))/2.0 print(_pykalman4d_good_threshold) def trainAux(params, num_trials): estimator = functools.partial(pyKalman4DTest, params, greater) estimates = bootstrap3(estimator, db, 'train', unusual_case, num_observations, num_trials) null_estimates = bootstrap3(estimator, db, 'train_null', unusual_case, num_observations, num_trials) bad_estimates = len([e for e in estimates if e != 1]) bad_null_estimates = len([e for e in null_estimates if e != 0]) false_negatives = 100.0*bad_estimates/num_trials false_positives = 100.0*bad_null_estimates/num_trials return false_positives,false_negatives params = {'threshold':_pykalman4d_good_threshold, 'kparams':_pykalman4d_params} wt = WorkerThreads(2, trainAux) num_trials = 50 performance = [] for t in range(-80,100,20): thresh = _pykalman4d_good_threshold + abs(_pykalman4d_good_threshold)*(t/100.0) params['threshold'] = thresh wt.addJob(thresh, (params.copy(),num_trials)) wt.wait() while not wt.resultq.empty(): job_id,errors = wt.resultq.get() fp,fn = errors #performance.append(((fp+fn)/2.0, job_id, fn, fp)) performance.append((abs(fp-fn), job_id, fn, fp)) performance.sort() #pprint.pprint(performance) best_threshold = performance[0][1] #print("best_threshold:", best_threshold) params['threshold']=best_threshold wt.stop() return {'trial_type':"train", 'num_observations':num_observations, 'num_trials':num_trials, 'params':json.dumps(params, sort_keys=True), 'false_positives':performance[0][3], 'false_negatives':performance[0][2]}
def main(): #Paramètres du modèle #Indiquer à quoi correspond chaque variable osigma=0.1; transition_matrix = np.array([[1., 0.,0.,0.,0.,0.],[1., 1.,0.,0.,0.,0.],[0.,0,1,0.,0.,0.]]) transition_covariance = np.zeros((6,6)); observation_matrix = np.array([[0., 1.,0.],[0., 0.,1.]]) observation_covariance = np.eye(2)*osigma initial_state_mean = np.array([1,0,10]) # m(0) initial_state_covariance = np.eye(3);# p(0) #Observations observations = csv.reader(open('voitureObservations.csv','r'),delimiter=' ') # Filtrage à la main # Quels sont les paramètres du constructeur ? kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, ) # Que conserverons les variables suivantes ? hand_state_estimates=[initial_state_mean] hand_state_cov_estimates=[initial_state_covariance] for anObs in observations: # Quelles étapes du filtrage sont réalisées par la ligne suivante (aMean,aCov) = kf.filter_update(hand_state_estimates[-1],hand_state_cov_estimates[-1],anObs) hand_state_estimates.append(aMean) hand_state_cov_estimates.append(aCov) hand_state_estimates=np.array(hand_state_estimates) # A quoi sert la ligne suivante ? hand_positions=np.dot(hand_state_estimates,observation_matrix.T ) # obtenir les observation (H*Mk-1 dans le cours, rouge) projet mon espace etat ver mon espace observation plt.figure(1) plt.plot(observations[:,0],observations[:,1], 'r+') plt.plot(hand_positions[:,0],hand_positions[:,1], 'b') plt.savefig('illustration_filtrage_main_voiture.pdf') plt.close() # Filtrage complet # Que fait cette section ? # Quels sont les paramètres supplémentaires donnés au constructeur ? # Quels sont les autres paramètres possibles ? kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance, ) (filtered_state_estimates,filtered_state_cov_estimates) = kf.filter(observations) filtered_positions=np.dot(filtered_state_estimates,observation_matrix.T ) plt.figure(1) plt.plot(observations[:,0],observations[:,1], 'r+') plt.plot(filtered_positions[:,0],filtered_positions[:,1], 'b') plt.savefig('illustration_filtrage_voiture.pdf') plt.close() # Lissage # Que fait cette section ? # Quel est la différence avec le filtrage ? # Puis-je faire un lissage "à la main" observation par observation ? kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance, ) (smoothed_state_estimates,smoothed_state_cov_estimates) = kf.smooth(observations) # au lieu de filtre, on appel smooth smoothed_positions=np.dot(smoothed_state_estimates,observation_matrix.T ) plt.figure(2) plt.plot(observations[:,0],observations[:,1], 'r+') plt.plot(smoothed_positions[:,0],smoothed_positions[:,1], 'b') plt.savefig('illustration_lissage_voiture.pdf') plt.close()
# + kf.transition_offsets[t] # ) # Estimate the hidden states using observations up to and including # time t for t in [0...n_timesteps-1]. This method outputs the mean and # covariance characterizing the Multivariate Normal distribution for # P(x_t | z_{1:t}) filtered_state_estimates = kf.filter(data.observations)[0] # Estimate the hidden states using all observations. These estimates # will be 'smoother' (and are to be preferred) to those produced by # simply filtering as they are made with later observations in mind. # Probabilistically, this method produces the mean and covariance # characterizing, # P(x_t | z_{1:n_timesteps}) smoothed_state_estimates = kf.smooth(data.observations)[0] # Draw the true, blind,e filtered, and smoothed state estimates for all 5 # dimensions. pl.figure(figsize=(16, 6)) lines_true = pl.plot(data.states, linestyle='-', color='b') # lines_blind = pl.plot(blind_state_estimates, linestyle=':', color='m') lines_observations = pl.plot(data.observations, linestyle=':', color='m') lines_filt = pl.plot(filtered_state_estimates, linestyle='--', color='g') lines_smooth = pl.plot(smoothed_state_estimates, linestyle='-.', color='r') pl.legend( (lines_true[0], lines_filt[0], lines_smooth[0], lines_observations[0]), ('true', 'filtered', 'smoothed', 'observations') ) pl.xlabel('time') pl.ylabel('state')
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();
transition_covariance = 0.05 # Q observation_covariance = 0.5 # R initial_state_mean = 0.0 initial_state_covariance = 1.0 # sample from model kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, transition_offset, observation_offset, initial_state_mean, initial_state_covariance, ) #kf = kf.em(y, n_iter=5) # Expectation Maximization y_filtered['pykalman-zero-order'] = kf.filter(y)[0].flatten() y_filtered['pykalman_smoothed-zero-order'] = kf.smooth(y)[0][:,0] #-------------------------------------------------------------------------------------------- # Kalman filter 2nd order (mass-spring-damper) (PyKalman) #-------------------------------------------------------------------------------------------- m = 1000.0 # mass k_s = 0.05 # spring (the larger, the harder) k_d = 0.5 # damper (the larger, the harder) d = k_d / k_s print("d: " + str(d) + ", Damped: " + str(d > 1)) q = 0.1 # Variance of process noise, i.e. state uncertainty r = 0.01 # Variance of measurement error
'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() print 'lds RMSE: %.5f' % np.sqrt(((N_SV - pred_Y_SV + E_V)**2).mean()) # comment in to plot the forecasts for each dimension # for v in xrange(V): # plt.plot(np.arange(T+S), np.append(N_TV[:, v], N_SV[:, v]), 'o-') # plt.plot(np.arange(T, T+S), pred_Y_SV[:, v] + E_V[v], 'o-', color='r') # plt.axvline(T-1, color='g', linestyle='--') # plt.show()
def main(): #Paramètres du modèle #Indiquer à quoi correspond chaque variable osigma=0.1; transition_matrix = np.array([[1., 0.,0.],[1., 1.,0.],[0.,0,0.9]]) transition_covariance = np.zeros((3,3)); observation_matrix = np.array([[0., 1.,0.],[0., 0.,1.]]) observation_covariance = np.eye(2)*osigma initial_state_mean = np.array([1,0,10]) initial_state_covariance = np.eye(3); #Observations observations=np.array([ [1.1,9.2], [1.9,8.1], [2.8,7.2], [4.2,6.6], [5.0,5.9], [6.1,5.32], [7.2,4.7], [8.1,4.3], [9.0,3.9]]) # Filtrage à la main # Quels sont les paramètres du constructeur ? kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, ) # Que conserverons les variables suivantes ? hand_state_estimates=[initial_state_mean] hand_state_cov_estimates=[initial_state_covariance] for anObs in observations: # Quelles étapes du filtrage sont réalisées par la ligne suivante (aMean,aCov) = kf.filter_update(hand_state_estimates[-1],hand_state_cov_estimates[-1],anObs) hand_state_estimates.append(aMean) hand_state_cov_estimates.append(aCov) hand_state_estimates=np.array(hand_state_estimates) # A quoi sert la ligne suivante ? hand_positions=np.dot(hand_state_estimates,observation_matrix.T ) plt.figure(1) plt.plot(observations[:,0],observations[:,1], 'r+') plt.plot(hand_positions[:,0],hand_positions[:,1], 'b') plt.savefig('illustration_filtrage_main.pdf') plt.close() # Filtrage complet # Que fait cette section ? # Quels sont les paramètres supplémentaires donnés au constructeur ? # Quels sont les autres paramètres possibles ? kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance, ) (filtered_state_estimates,filtered_state_cov_estimates) = kf.filter(observations) filtered_positions=np.dot(filtered_state_estimates,observation_matrix.T ) plt.figure(1) plt.plot(observations[:,0],observations[:,1], 'r+') plt.plot(filtered_positions[:,0],filtered_positions[:,1], 'b') plt.savefig('illustration_filtrage.pdf') plt.close() # Lissage # Que fait cette section ? # Quel est la différence avec le filtrage ? # Puis-je faire un lissage "à la main" observation par observation ? kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance, ) (smoothed_state_estimates,smoothed_state_cov_estimates) = kf.smooth(observations) smoothed_positions=np.dot(smoothed_state_estimates,observation_matrix.T ) plt.figure(2) plt.plot(observations[:,0],observations[:,1], 'r+') plt.plot(smoothed_positions[:,0],smoothed_positions[:,1], 'b') plt.savefig('illustration_lissage.pdf') plt.close()
x = np.linspace(0, 30 * np.pi, n_timesteps) observations = 20 * (np.sin(x) + 0.5 * rnd.randn(n_timesteps)) # 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. masked_observations = np.ma.masked_where(observations < -5, observations) # observation_covariances = np.where(observations <-5, np.ones_like(observations),np.zeros_like(observations)) kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]), observation_covariance=[[10]]) masked_observations = np.ma.masked_where(observations < -5, observations) # You can use the Kalman Filter immediately without fitting, but its estimates # may not be as good as if you fit first. masked_pred = kf.smooth(masked_observations)[0] kf = KalmanFilter(transition_matrices=np.array([[1, x[1]], [0, 1]]), observation_covariance=[[10]]) states_pred = kf.smooth(observations)[0] print "fitted model: %s" % (kf,) # Plot lines for the observations without noise, the estimated position of the # target before fitting, and the estimated position after fitting. pl.figure(figsize=(16, 6)) obs_scatter = pl.scatter(x, observations, marker="x", color="b", label="observations") obs_scatter = pl.scatter(x, masked_observations, marker="x", color="m", label="ma observations") position_line = pl.plot(x, states_pred[:, 0], linestyle="-", marker="o", color="b", label="position est.") position_masked = pl.plot(x, masked_pred[:, 0], linestyle="-", marker="o", color="m", label="ma position est.")
class KalmanSmoother2D: def __init__(self, x_noise, y_noise, smoothness_x=1, smoothness_y=1): dt = 1 # model F = np.eye(4) F[0, 2] = dt F[1, 3] = dt H = np.zeros((2, 4)) H[0, 0] = 1 H[1, 1] = 1 R = np.zeros((2, 2)) R[0, 0] = x_noise * x_noise R[1, 1] = y_noise * y_noise sigma_ax, sigma_ay = 1, 1 G = np.zeros((4, 1)) G[2] = sigma_ax * dt G[3] = sigma_ay * dt Q = np.transpose(G) * G Q[0, 1] = 0 Q[1, 0] = 0 Q[0, 3] = 0 Q[3, 0] = 0 Q[1, 2] = 0 Q[2, 1] = 0 Q[2, 3] = 0 Q[3, 2] = 0 # initialize filter self.kf = KalmanFilter() self.kf.transition_matrices = F self.kf.observation_matrices = H self.kf.transition_covariance = Q self.kf.observation_covariance = R # default initial state # TODO: maybe use first measurement as default? self.kf.initial_state_mean = np.zeros((4,)) self.kf.initial_state_covariance = np.zeros((4, 4)) # TODO: get innovations? def set_initial_state(self, initial_mean, initial_covariance=np.zeros((4, 4))): if initial_mean.shape[0] == 2: print "initial velocity unspecified, assuming v0 = 0" initial_mean = np.array([initial_mean[0], initial_mean[1], 0, 0]) self.kf.initial_state_mean = initial_mean self.kf.initial_state_covariance = initial_covariance def set_measurements(self, measurements): self.smooth_means, self.smooth_covs = self.kf.smooth(measurements) def get_smoothed_measurements(self): return self.smooth_means[:, 0:2] def get_velocities(self): return self.smooth_means[:, 2:] def get_covariances(self): return self.smooth_covs
for cnum, cpos in posDF.groupby("id"): if len(cpos) == 1: continue ft = np.arange(cpos["frame"].values[0], cpos["frame"].values[-1] + 1) # obs = np.vstack((cpos['x'].values, cpos['y'].values)).T obs = np.zeros((len(ft), 2)) obs = np.ma.array(obs, mask=np.zeros_like(obs)) for f in range(len(ft)): if len(cpos[cpos["frame"] == ft[f]].x.values) > 0: obs[f][0] = cpos[cpos["frame"] == ft[f]].x.values[0] * px_to_m obs[f][1] = cpos[cpos["frame"] == ft[f]].y.values[0] * px_to_m else: obs[f] = np.ma.masked kf.initial_state_mean = [cpos["x"].values[0] * px_to_m, cpos["y"].values[0] * px_to_m, 0, 0, 0, 0] sse = kf.smooth(obs)[0] ani = cpos["animal"].values[0] xSmooth = sse[:, 0] ySmooth = sse[:, 1] xv = sse[:, 2] / 0.1 yv = sse[:, 3] / 0.1 xa = sse[:, 4] / 0.01 ya = sse[:, 5] / 0.01 headings = np.zeros_like(xSmooth) dx = np.zeros_like(xSmooth) dy = np.zeros_like(xSmooth) for i in range(len(headings)): start = max(0, i - 5) stop = min(i + 5, len(headings)) - 1