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