def test_kalman2():
from pykalman import UnscentedKalmanFilter
ukf = UnscentedKalmanFilter(lambda x, w: x + np.sin(w),
lambda x, v: x + v,
transition_covariance=0.1)
(filtered_state_means, filtered_state_covariances) = ukf.filter([0, 1, 2])
(smoothed_state_means, smoothed_state_covariances) = ukf.smooth([0, 1, 2])
def kalman_feat(df, cols):
for col in cols:
ukf = UnscentedKalmanFilter(lambda x, w: x + np.sin(w),
lambda x, v: x + v,
observation_covariance=0.1)
(filtered_state_means,
filtered_state_covariances) = ukf.filter(df[col])
(smoothed_state_means,
smoothed_state_covariances) = ukf.smooth(df[col])
df[col + "_UKFSMOOTH"] = smoothed_state_means.flatten()
df[col + "_UKFFILTER"] = filtered_state_means.flatten()
return df
def main():
# sample from model
kf = UnscentedKalmanFilter(
transition_function, observation_function,
observation_covariance,
)
# states, observations = kf.sample(50, initial_state_mean)
observations = []
import os
dir = os.path.dirname(os.path.realpath(__file__))
with open(dir+'/data.csv', 'rb') as csvfile:
para = csv.reader(csvfile, delimiter=',', quotechar='|')
for row in para:
observations.append(row)
states = []
cov = []
for i in range(len(observations)):
states.append([0, 0])
cov.append([[0, 0], [0, 0]])
states[0] = initial_state_mean
print states[0]
cov[0] = initial_state_covariance
for i in range(len(observations)-1):
states[i], cov[i] = kf.filter(observations[i+1])
states[i+1], cov[i+1] = kf.filter_update(states[i], cov[i])
# 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', ls='-')
# lines_smooth = pl.plot(smoothed_state_estimates, color='g', ls='-.')
#pl.legend((lines_true[0], lines_filt[0], lines_smooth[0]),
# ('true', 'filt', 'smooth'),
# loc='lower left'
#)
pl.show()
def kalman_filter(video_file, dep0, v0):
# get feature points from video
points, init_image = get_feature_points_from_video(video_file)
i, o = feature_points_to_observation_and_initilization(points, v0, dep0)
# number of the features points
observations = np.asarray(o)
n = observations.shape[1]
print('initilization shape is ', i.shape)
print('observation shape is ', observations.shape)
# initialize the vovariance and mean
transition_covariance = np.eye(2*n+6)
random_state = np.random.RandomState(0)
observation_covariance = np.eye(n) + abs(random_state.randn(n, n) * 0.005)
initial_state_mean = i
covariance_init = random_state.randn(2*n+6, 2*n+6) * 0.2
covariance_init[0:3,0:3] = 0.005
initial_state_covariance = np.eye(2*n+6) + abs(covariance_init)
# set Unscented kalman filter
kf = UnscentedKalmanFilter(
transition_function, observation_function,
transition_covariance, observation_covariance,
initial_state_mean, initial_state_covariance,
random_state=random_state
)
"""
kf = AdditiveUnscentedKalmanFilter(
additive_transition_function, additive_observation_function,
transition_covariance, observation_covariance,
initial_state_mean, initial_state_covariance
)
"""
# get result
filtered_state_estimates = kf.filter(observations)
smoothed_state_estimates = kf.smooth(observations)
return filtered_state_estimates, smoothed_state_estimates
random_state=None)
n = 100
while n:
n -= 1
# Find the means and covariances
warped_obs = np.zeros(hidden_time_length)
for obs in observed_trajectories:
# warp trajectory to make it to the same length of hidden trajectory
warped_obs += get_time_warped_series(obs, hidden_trajectory)
warped_obs = warped_obs / hidden_time_length
smooth_means, smooth_cov = ukf.smooth(warped_obs)
filter_means, filter_cov = ukf.filter(warped_obs)
# Find new Q and R
Q = ukf.transition_covariance
R = ukf.observation_covariance
new_Q = 0
new_R = 0
for t in range(hidden_time_length-1):
d_mu = smooth_means[t+1] - _transition(smooth_means[t])
A = jacobian_transition(smooth_means[i])
L = filter_cov[t] * A.T / filter_cov[t+1]
P = smooth_cov[t+1] - smooth_cov[t+1] * L.T * A.T - A.T * L.T * smooth_cov[t+1]
new_Q += d_mu * d_mu.T + A * smooth_cov[t] * A.T + P
d_y = warped_obs[t] - _observation(smooth_means[t]) # TODOOOO
Initial_state = [0, 0]
intial_covariance = [[1, 0.1], [-0.1, 1]]
# UKF
kf = UnscentedKalmanFilter(transition_function,
observation_function,
transition_covariance,
observation_covariance,
Initial_state,
intial_covariance,
random_state=noise_generator)
sample = 200
states, observations = kf.sample(sample, Initial_state)
# estimate state with filtering and smoothing
filtered_state_estimates = kf.filter(observations)[0]
smoothed_state_estimates = kf.smooth(observations)[0]
# True line
t = np.linspace(0, sample * 0.1, sample)
y = np.sin(t)
plt.plot(filtered_state_estimates[:, 0],
filtered_state_estimates[:, 1],
color='r',
ls='-',
label='UKF')
plt.plot(smoothed_state_estimates[:, 0],
smoothed_state_estimates[:, 1],
color='g',
ls='-.',
initial_state_covariance = [[1, 0.1], [0.1, 1]]
# sample from model
ukf = UnscentedKalmanFilter(transition_function,
observation_function,
transition_covariance,
observation_covariance,
initial_state_mean,
initial_state_covariance,
random_state=random_state)
akf = AdditiveUnscentedKalmanFilter(additive_transition_function,
additive_observation_function,
transition_covariance,
observation_covariance, initial_state_mean,
initial_state_covariance)
states, observations = ukf.sample(50, initial_state_mean)
# estimate state with filtering
ukf_state_estimates = ukf.filter(observations)[0]
akf_state_estimates = akf.filter(observations)[0]
# draw estimates
pl.figure()
lines_true = pl.plot(states, color='b')
lines_ukf = pl.plot(ukf_state_estimates, color='r')
lines_akf = pl.plot(akf_state_estimates, color='g')
pl.legend((lines_true[0], lines_ukf[0], lines_akf[0]),
('true', 'UKF', 'AddUKF'),
loc='upper left')
pl.show()
initial_state_covariance = [[1, 0.1], [-0.1, 1]]
# sample from model
ukf = UnscentedKalmanFilter(
transition_function, observation_function,
transition_covariance, observation_covariance,
initial_state_mean, initial_state_covariance,
random_state=random_state
)
akf = AdditiveUnscentedKalmanFilter(
additive_transition_function, additive_observation_function,
transition_covariance, observation_covariance,
initial_state_mean, initial_state_covariance
)
states, observations = ukf.sample(50, initial_state_mean)
# estimate state with filtering
ukf_state_estimates = ukf.filter(observations)[0]
akf_state_estimates = akf.filter(observations)[0]
# draw estimates
pl.figure()
lines_true = pl.plot(states, color='b')
lines_ukf = pl.plot(ukf_state_estimates, color='r')
lines_akf = pl.plot(akf_state_estimates, color='g')
pl.legend((lines_true[0], lines_ukf[0], lines_akf[0]),
('true', 'UKF', 'AddUKF'),
loc='upper left'
)
pl.show()
def test_unscented_kalman(self):
ukf = UnscentedKalmanFilter(lambda x, w: x + np.sin(w), lambda x, v: x + v, transition_covariance=0.1)
(filtered_state_means, filtered_state_covariances) = ukf.filter([0, 1, 2])
(smoothed_state_means, smoothed_state_covariances) = ukf.smooth([0, 1, 2])
return filtered_state_means
from pykalman import UnscentedKalmanFilter
import numpy as np
ukf = UnscentedKalmanFilter(lambda x, w: x + np.sin(w),
lambda x, v: x + v,
observation_covariance=0.1)
(filtered_state_means, filtered_state_covariance) = ukf.filter([0, 0, 0])
(smoothed_state_means, smoothed_state_covariance) = ukf.smooth([0, 0, 0])
def smothingData(args):
delta = ukf.smooth(args)[0]
return delta
def filteringData(args):
alpha = ukf.filter(args)
return alpha
transition_covariance = np.eye(2)
random_state = np.random.RandomState(0)
observation_covariance = np.eye(2) + random_state.randn(2, 2) * 0.1
initial_state_mean = [0, 0]
initial_state_covariance = [[1, 0.1], [-0.1, 1]]
# sample from model
kf = UnscentedKalmanFilter(
transition_function, observation_function,
transition_covariance, observation_covariance,
initial_state_mean, initial_state_covariance,
random_state=random_state
)
states, observations = kf.sample(50, initial_state_mean)
# 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', ls='-')
lines_smooth = pl.plot(smoothed_state_estimates, color='g', ls='-.')
pl.legend((lines_true[0], lines_filt[0], lines_smooth[0]),
('true', 'filt', 'smooth'),
loc='lower left'
)
pl.show()
def filtering(timeseries=None, filterType='highPassRealTime'):
'''
filterType can be
highPassRealTime
highPassBetweenRuns
UnscentedKalmanFilter_filter # documentation: https://pykalman.github.io/
UnscentedKalmanFilter_smooth
KalmanFilter_filter
KalmanFilter_smooth
noFilter
'''
timeseries = timeseries.astype(np.float)
oldShape = timeseries.shape
timeseries = timeseries.reshape(timeseries.shape[0], -1)
if filterType == 'highPassRealTime':
# from highpassFunc import highPassRealTime, highPassBetweenRuns
from highpass import highpass
def highPassRealTime(A_matrix, TR, cutoff):
full_matrix = np.transpose(
highpass(np.transpose(A_matrix), cutoff / (2 * TR), True))
return full_matrix[-1, :]
filtered_timeseries = []
for currTR in range(timeseries.shape[0]):
filtered_timeseries.append(
highPassRealTime(timeseries[:(currTR + 1)], 1.5, 56))
filtered_timeseries = np.asarray(filtered_timeseries)
elif filterType == 'highPassBetweenRuns':
# from highpassFunc import highPassRealTime, highPassBetweenRuns
from highpass import highpass
def highPassBetweenRuns(A_matrix, TR, cutoff):
return np.transpose(
highpass(np.transpose(A_matrix), cutoff / (2 * TR), False))
filtered_timeseries = highPassBetweenRuns(timeseries, 1.5, 56)
elif filterType == 'UnscentedKalmanFilter_filter':
from pykalman import UnscentedKalmanFilter
ukf = UnscentedKalmanFilter(lambda x, w: x + np.sin(w),
lambda x, v: x + v,
observation_covariance=0.1)
filtered_timeseries = np.zeros(timeseries.shape)
for curr_voxel in range(timeseries.shape[1]):
(filtered_timeseries_state_means,
filtered_timeseries_state_covariances) = ukf.filter(
timeseries[:, curr_voxel])
filtered_timeseries[:,
curr_voxel] = filtered_timeseries_state_means.reshape(
-1)
elif filterType == 'UnscentedKalmanFilter_smooth':
from pykalman import UnscentedKalmanFilter
ukf = UnscentedKalmanFilter(lambda x, w: x + np.sin(w),
lambda x, v: x + v,
observation_covariance=0.1)
filtered_timeseries = np.zeros(timeseries.shape)
for curr_voxel in range(timeseries.shape[1]):
(smoothed_state_means,
smoothed_state_covariances) = ukf.smooth(data)
filtered_timeseries[:,
curr_voxel] = smoothed_state_means.reshape(
-1)
elif filterType == 'KalmanFilter_filter':
from pykalman import KalmanFilter
kf = KalmanFilter(transition_matrices=None,
observation_matrices=None)
filtered_timeseries = np.zeros(timeseries.shape)
for curr_voxel in range(timeseries.shape[1]):
measurements = np.asarray(timeseries[:, curr_voxel])
kf = kf.em(measurements, n_iter=5)
(filtered_state_means,
filtered_state_covariances) = kf.filter(measurements)
filtered_timeseries[:,
curr_voxel] = filtered_state_means.reshape(
-1)
elif filterType == 'KalmanFilter_smooth':
from pykalman import KalmanFilter
kf = KalmanFilter(transition_matrices=None,
observation_matrices=None)
filtered_timeseries = np.zeros(timeseries.shape)
for curr_voxel in range(timeseries.shape[1]):
measurements = np.asarray(timeseries[:, curr_voxel])
kf = kf.em(measurements, n_iter=5)
(smoothed_state_means,
smoothed_state_covariances) = kf.smooth(measurements)
filtered_timeseries[:,
curr_voxel] = smoothed_state_means.reshape(
-1)
elif filterType == 'noFilter':
filtered_timeseries = timeseries
else:
raise Exception('filterType wrong')
filtered_timeseries = filtered_timeseries.reshape(oldShape)
return filtered_timeseries
