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
