def __init__(self, dimState, delay, arm, knoiseU, controller):
'''
Initializes parameters to uses the function implemented below
inputs: -dimState: dimension of the state, here the state correspond to the muscular activation vector U, int
-dimObs: dimension of the observation, here the observation is the position of the arm given by the model, int
-delay: the delay with which we give the observation to the filter, int
-arm, armModel, class object
-rs, ReadSetupFile, class object
'''
self.name = "UnscentedKalmanFilter"
self.knoiseU = knoiseU
self.dimState = dimState
self.delay = delay
self.arm = arm
self.controller = controller
#initialization of some parameters for the filter
transition_covariance = np.eye(self.dimState) * 0.01
initial_state_mean = np.zeros(self.dimState)
observation_covariance = 1000 * np.eye(self.dimState)
initial_state_covariance = np.eye(self.dimState)
self.nextCovariance = np.eye(self.dimState) * 0.0001
self.ukf = UnscentedKalmanFilter(self.transitionFunctionUKF,
self.observationFunctionUKF,
transition_covariance,
observation_covariance,
initial_state_mean,
initial_state_covariance)
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_filter(seq):
def f(state, noise):
return state + np.sin(noise)
def g(state, noise):
return state + np.cos(noise)
# kf = KalmanFilter(initial_state_mean=0, n_dim_obs=2)
ukf = UnscentedKalmanFilter(f, g, 0.001)
return ukf.smooth(seq)[0].T[0]
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 __init__(self, name, rate_limiter):
super(Leader, self).__init__(name,
rate_limiter=rate_limiter,
laser_range=[np.pi / 3.5, np.pi / 3.5],
laser_dist=2)
self._epsilon = 0.1
self.leg_pub = rospy.Publisher('/' + name + '/legs',
Marker,
queue_size=5)
self.centroid_pub = rospy.Publisher('/' + name + '/centroids',
Marker,
queue_size=5)
self.v_pub = rospy.Publisher('/' + name + '/vel', Marker, queue_size=5)
self.follow = None
self._tf = TransformListener()
self.last_vel = np.array([0., 0.])
self.all_around_laser = Laser(name=name, laser_range=[np.pi, np.pi])
self.last_legs = None
def f(state, noise):
pass
def g(state, noise):
pass
self.ukf = UnscentedKalmanFilter(f, g)
self.kf = None
def __init__(self):
initialState = np.zeros((6, 1))
initialCov = np.diag(np.ones((6, 1))[:, 0])
self.filter = UnscentedKalmanFilter(
initial_state_mean=initialState,
initial_state_covariance=initialCov,
n_dim_obs=6)
def __init__(self, dimState, delay, arm, knoiseU, controller):
'''
Initializes parameters to uses the function implemented below
inputs: -dimState: dimension of the state, here the state correspond to the muscular activation vector U, int
-dimObs: dimension of the observation, here the observation is the position of the arm given by the model, int
-delay: the delay with which we give the observation to the filter, int
-arm, armModel, class object
-rs, ReadSetupFile, class object
'''
self.name = "UnscentedKalmanFilter"
self.knoiseU = knoiseU
self.dimState = dimState
self.delay = delay
self.arm = arm
self.controller = controller
#initialization of some parameters for the filter
transition_covariance = np.eye(self.dimState)*0.01
initial_state_mean = np.zeros(self.dimState)
observation_covariance = 1000*np.eye(self.dimState)
initial_state_covariance = np.eye(self.dimState)
self.nextCovariance = np.eye(self.dimState)*0.0001
self.ukf = UnscentedKalmanFilter(self.transitionFunctionUKF, self.observationFunctionUKF,
transition_covariance, observation_covariance,
initial_state_mean, initial_state_covariance)
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 kalmanFilter(X):
ukf = UnscentedKalmanFilter()
X_fil = np.zeros(np.shape(X))
for i in range(np.shape(X)[1]):
X_fil[:, i] = ukf.smooth(X[:, i])[0].ravel()
return X_fil
#if __name__ == "__main__":
#
# subject = 'qing_frontal_3.7'
# X,y= fetchDataset(subject)
#
# seq_x, seq,y = split_sequences(s, n_steps):
## X_train,X_test,y_train,y_test = split_train_test(X,y)
def __init__(self, x, v, w):
self.random_state = np.random.RandomState(0)
self.transition_covariance = np.array([[0.5, 0, 0, 0, 0],\
[0, 1, 0, 0, 0],\
[0, 0, 0.1, 0, 0],\
[0, 0, 0, 0.001, 0],\
[0, 0, 0, 0, 0.001],\
])
self.observation_covariance = np.array([[0.5, 0, 0, 0, 0],\
[0, 1, 0, 0, 0],\
[0, 0, 0.5, 0, 0],\
[0, 0, 0, 0.001, 0],\
[0, 0, 0, 0, 0.001],\
])
self.initial_state_mean = [x, 0.1, v, np.pi / 2, w]
# self.initial_state_mean = [0, 0, 20, 0, np.pi / 180]
self.transition_state = self.initial_state_mean
self.obs = self.initial_state_mean
self.pre_parabola_param = [0, 0, 1.75]
self.initial_state_covariance = np.array([[0.5, 0, 0, 0, 0],\
[0, 0.02, 0, 0, 0],\
[0, 0, 0.1, 0, 0],\
[0, 0, 0, 0.001, 0],\
[0, 0, 0, 0, 0.001],\
])
self.T = 0.5
self.estimate_state = [
self.initial_state_mean, self.initial_state_covariance
]
self.kf = UnscentedKalmanFilter(self.transition_function,
self.observation_function,
self.transition_covariance,
self.observation_covariance,
self.initial_state_mean,
self.initial_state_covariance,
random_state=self.random_state)
self.timestamp = time.time()
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
def additive_observation_function(state):
return observation_function(state, np.array([0, 0]))
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
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
class UnscentedKalmanFilterControl:
def __init__(self, dimState, delay, arm, knoiseU, controller):
'''
Initializes parameters to uses the function implemented below
inputs: -dimState: dimension of the state, here the state correspond to the muscular activation vector U, int
-dimObs: dimension of the observation, here the observation is the position of the arm given by the model, int
-delay: the delay with which we give the observation to the filter, int
-arm, armModel, class object
-rs, ReadSetupFile, class object
'''
self.name = "UnscentedKalmanFilter"
self.knoiseU = knoiseU
self.dimState = dimState
self.delay = delay
self.arm = arm
self.controller = controller
#initialization of some parameters for the filter
transition_covariance = np.eye(self.dimState)*0.01
initial_state_mean = np.zeros(self.dimState)
observation_covariance = 1000*np.eye(self.dimState)
initial_state_covariance = np.eye(self.dimState)
self.nextCovariance = np.eye(self.dimState)*0.0001
self.ukf = UnscentedKalmanFilter(self.transitionFunctionUKF, self.observationFunctionUKF,
transition_covariance, observation_covariance,
initial_state_mean, initial_state_covariance)
def initObsStore(self, state):
'''
Initialization of the observation storage
Input: -state: the state stored
'''
self.obsStore = []
for i in range(self.delay):
self.obsStore.append(state)
#print ("InitStore:", self.obsStore)
def storeObs(self, state):
'''
Stores the current state and returns the delayed state
Input: -state: the state to store
'''
for i in range (self.delay-1):
self.obsStore[self.delay-i-1]=self.obsStore[self.delay-i-2]
self.obsStore[0]=state
#print ("After store:", self.obsStore)
#print ("retour:", self.obsStore[self.delay-1])
return self.obsStore[self.delay-1]
def transitionFunctionUKF(self, state, transitionNoise = 0):
'''
Transition function used by the filter, function of the state and the transition noise at time t and produces the state at time t+1
Inputs: -stateU: the state at time t, numpy array
-transitionNoise: transition noise at time t, numpy array
Output: -nextStateUNoise: the next State with noise added, numpy array
'''
#print("UKF trans state :", state)
U = self.controller.computeOutput(state)
nextState = self.arm.computeNextState(U, state)
return nextState + transitionNoise
def observationFunctionUKF(self, state, observationNoise = 0):
'''
Observation function used by the filter, function of the state and the observation noise at time t and produces the observation at time t
Inputs: -stateU: the state at time t, numpy array
-observationNoise: the observation noise at time t, numpy array
Output: -nextObsNoise: observation at time t+1
'''
U = self.controller.computeOutput(state)
nextObs = self.arm.computeNextState(U, state)
nextObsNoise = nextObs + observationNoise
return nextObsNoise
def runUKF(self, state):
'''
Function used to compute the next state approximation with the filter
Inputs: -Unoisy: the state to feed the filter, here its the muscular activation vector U, numpy array of dimension (x, 1), here x = 6
-state: the state of the arm
Output: -stateApprox: the next state approximation, numpy array of dimension (x, 1), here x = 4
'''
#store the state of the arm to feed the filter with a delay on the observation
oldstate = self.storeObs(state)
#print("UKF old state :", oldstate)
#compute the nextState approximation ie here the next muscular activation
nextState, nextCovariance = self.ukf.filter_update(state, self.nextCovariance, oldstate)
#compute the nextState i.e. the next position vector from the approximation of the next muscular activation vector given by the filter
return nextState
def debugStore(self):
state = np.array([1,2,3,4])
self.initObsStore(state)
for i in range(5):
tmp = [rd.random() for x in range(4)]
self.storeObs(tmp)
def additive_transition_function(state):
return transition_function(state, np.array([0, 0]))
def additive_observation_function(state):
return observation_function(state, np.array([0, 0]))
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
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()
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
class Fusion(object):
def __init__(self, x, v, w):
self.random_state = np.random.RandomState(0)
self.transition_covariance = np.array([[0.5, 0, 0, 0, 0],\
[0, 1, 0, 0, 0],\
[0, 0, 0.1, 0, 0],\
[0, 0, 0, 0.001, 0],\
[0, 0, 0, 0, 0.001],\
])
self.observation_covariance = np.array([[0.5, 0, 0, 0, 0],\
[0, 1, 0, 0, 0],\
[0, 0, 0.5, 0, 0],\
[0, 0, 0, 0.001, 0],\
[0, 0, 0, 0, 0.001],\
])
self.initial_state_mean = [x, 0.1, v, np.pi / 2, w]
# self.initial_state_mean = [0, 0, 20, 0, np.pi / 180]
self.transition_state = self.initial_state_mean
self.obs = self.initial_state_mean
self.pre_parabola_param = [0, 0, 1.75]
self.initial_state_covariance = np.array([[0.5, 0, 0, 0, 0],\
[0, 0.02, 0, 0, 0],\
[0, 0, 0.1, 0, 0],\
[0, 0, 0, 0.001, 0],\
[0, 0, 0, 0, 0.001],\
])
self.T = 0.5
self.estimate_state = [
self.initial_state_mean, self.initial_state_covariance
]
self.kf = UnscentedKalmanFilter(self.transition_function,
self.observation_function,
self.transition_covariance,
self.observation_covariance,
self.initial_state_mean,
self.initial_state_covariance,
random_state=self.random_state)
self.timestamp = time.time()
def transition_function(self, state, noise):
t = self.T
if state[4] == 0:
a = state[2] * np.cos(state[3]) + state[0]
b = state[2] * np.sin(state[3]) + state[1]
c = state[2]
d = np.pi / 2 #state[4] * t + state[3]
e = state[4]
else:
r = state[2] / state[4]
a = r * np.sin(state[4] * t + state[3]) - r * np.sin(
state[3]) # + state[0]
b = -r * np.cos(state[4] * t + state[3]) + r * np.cos(
state[3]) + state[1]
c = state[2]
d = np.pi / 2 #state[4] * t + state[3]
e = state[4] # + np.sin(debug_index / 10) * 0.01
# e = states_i + np.sin(debug_index / 10) * 10 * np.pi / 180
#adjust x from parabola param(down-right coordinate)
pixl_b = b * (IMAGE_HEI / 40)
parabola_y1 = self.pre_parabola_param[0] * (
-pixl_b)**2 + self.pre_parabola_param[1] * (
-pixl_b) + self.pre_parabola_param[2]
dalta_y = parabola_y1 - self.pre_parabola_param[2]
# a = dalta_y * (3.5 / lane_wid) + a
pixl_y = parabola_y1 * (3.5 / lane_wid)
a = pixl_y - a
b = b - state[1]
if self.obs[0] - a > 3.5 / 2:
a += 3.5
elif self.obs[0] - a < -3.5 / 2:
a -= 3.5
self.transition_state = np.array([a, b, c, d, e]) + noise
# print ("self.transition_state:" + str(self.transition_state))
return self.transition_state
def observation_function(self, state, noise):
# C = np.array([[-1, 0.5], [0.2, 0.1]])
# C = np.array([[1, 0], [0, 1]])
C = np.eye(5)
# return np.dot(C, state) + noise
return state + noise
def update(self, obs):
self.estimate_state = self.kf.filter_update(
self.estimate_state[0], self.estimate_state[1], obs,
self.transition_function, self.transition_covariance,
self.observation_function, self.observation_covariance)
print("estimate X:" + str(self.estimate_state[0]))
return self.estimate_state
def update_step(self, x, v, w, t, parabola_param):
print("update_step1:({},{},{},{},{})".format(x, v, w, t,
str(parabola_param)))
self.T = t
self.parabola_param = parabola_param
# obs = [x, y, v, theta, w]
# we didn't have obs_y so use predict obs_y(we didn't use y)
if w == 0 or self.transition_state[4] == 0:
y = self.transition_state[2] * np.sin(
self.transition_state[3]) + self.transition_state[1]
else:
r = self.transition_state[2] / self.transition_state[4]
y = -r * np.cos(
self.transition_state[4] * t + self.transition_state[3]
) + r * np.cos(self.transition_state[3]) + self.transition_state[1]
self.obs = [x, y, v, np.pi / 2, w]
self.timestamp = time.time()
print("update_step2:({})".format(str(self.obs)))
self.update(self.obs)
self.pre_parabola_param = parabola_param
print("update_step3:({})".format(str(self.estimate_state[0])))
return self.estimate_state[0][0]
def get_estimate(self):
return self.estimate_state[0][0]
def get_predict(self):
return self.transition_state[0]
ym = np.sin(state[0]) + noise[1]
return np.array([xm, ym])
'''----INITIALIZE THE PARAMETERS----'''
transition_covariance = np.eye(2)
noise_generator = np.random.RandomState(0)
observation_covariance = np.eye(2) + noise_generator.randn(2, 2) * 0.1
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],
def observation_function(state, noise):
C = np.array([[-1, 0.5], [0.2, 0.1]])
return np.dot(C, state) + noise
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)
print(type(observations))
print(observations[0, 0])
print(observations.shape)
# 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='-')
return np.array([a, b])
def observation_function(state, noise):
C = np.array([[-1, 0.5], [0.2, 0.1]])
return np.dot(C, state) + noise
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'),
def main(argv):
config = read_parser(argv, Inputs, InputsOpt_Defaults)
if config['mode'] == 'fuse_kalman':
print('Select MASTER Features xls')
root = Tk()
root.withdraw()
root.update()
filepath = filedialog.askopenfilename()
root.destroy()
mydict = pd.read_excel(filepath)
rownames = list(mydict.index.values)
length_data = len(rownames)
mydict = mydict.to_dict(orient='list')
newdict = {}
for key, values in mydict.items():
newdict[key] = movil_avg(mydict[key], config['n_mov_avg'])
Features = []
for i in range(length_data):
example = []
for feature in config['feature_array']:
example.append(newdict[feature][i])
Features.append(example)
Features = np.array(Features)
plt.plot(Features)
plt.show()
scaler_model = StandardScaler()
scaler_model.fit(Features)
Features = scaler_model.transform(Features)
plt.plot(Features)
plt.show()
kf = UnscentedKalmanFilter(n_dim_state=1,
n_dim_obs=len(config['feature_array']),
random_state=1)
# kf = AdditiveUnscentedKalmanFilter(n_dim_state=1, n_dim_obs=6, random_state=1)
# kf = KalmanFilter(n_dim_state=1, n_dim_obs=len(config['feature_array']), random_state=1)
measurements = [i for i in Features]
# kf.em(measurements)
z = kf.smooth(measurements)
z = z[0]
plt.plot(z)
plt.show()
elif config['mode'] == 'testtt':
if config['mypath'] == None:
print('Select XLS Files with Feature')
root = Tk()
root.withdraw()
root.update()
Filepaths = filedialog.askopenfilenames()
root.destroy()
else:
# Filepath = config['mypath']
Filepaths = [
join(config['mypath'], f) for f in listdir(config['mypath'])
if isfile(join(config['mypath'], f)) if f[-4:] == 'xlsx'
]
Feature = []
for filepath in Filepaths:
mydict = pd.read_excel(filepath)
mydict = mydict.to_dict(orient='list')
Feature += mydict[config['feature']]
Feature = np.array(Feature)
for i in range(len(Feature)):
count = 0
while np.isnan(Feature[i]) == True:
count += 1
Feature[i] = Feature[i + count]
# Feature = np.nan_to_num(Feature)
Feature = movil_avg(Feature, config['n_mov_avg'])
plt.plot(Feature)
plt.show()
elif config['mode'] == 'fuse_factor':
print('Select MASTER Features xls')
root = Tk()
root.withdraw()
root.update()
filepath = filedialog.askopenfilename()
root.destroy()
mydict = pd.read_excel(filepath)
rownames = list(mydict.index.values)
length_data = len(rownames)
mydict = mydict.to_dict(orient='list')
newdict = {}
for key, values in mydict.items():
newdict[key] = movil_avg(mydict[key], config['n_mov_avg'])
Features = []
for i in range(length_data):
example = []
for feature in config['feature_array']:
example.append(newdict[feature][i])
Features.append(example)
Features = np.array(Features)
plt.plot(Features)
plt.show()
scaler_model = StandardScaler()
scaler_model.fit(Features)
Features = scaler_model.transform(Features)
plt.plot(Features)
plt.show()
fa = FactorAnalysis(n_components=1, random_state=1)
fa.fit(Features)
z = fa.transform(Features)
plt.plot(z)
plt.show()
elif config['mode'] == 'fuse_pca':
print('Select MASTER Features xls')
root = Tk()
root.withdraw()
root.update()
filepath = filedialog.askopenfilename()
root.destroy()
mydict = pd.read_excel(filepath)
rownames = list(mydict.index.values)
length_data = len(rownames)
mydict = mydict.to_dict(orient='list')
newdict = {}
for key, values in mydict.items():
newdict[key] = movil_avg(mydict[key], config['n_mov_avg'])
Features = []
for i in range(length_data):
example = []
for feature in config['feature_array']:
example.append(newdict[feature][i])
Features.append(example)
Features = np.array(Features)
plt.plot(Features)
plt.show()
scaler_model = StandardScaler()
scaler_model.fit(Features)
Features = scaler_model.transform(Features)
plt.plot(Features)
plt.show()
from sklearn.cluster import FeatureAgglomeration
from sklearn.decomposition import FastICA
from sklearn.decomposition import KernelPCA
FeatureAgglomeration
# model = PCA(n_components=len(config['feature_array']))
model = PCA(n_components=2)
# model = FeatureAgglomeration(n_clusters=1)
# model = FastICA(n_components=1)
# model = KernelPCA(n_components=1, kernel='rbf')
# model = KernelPCA(n_components=len(config['feature_array']), kernel='rbf')
model.fit(Features)
Features = model.transform(Features)
print(model.explained_variance_)
print(model.explained_variance_ratio_)
plt.plot(Features)
plt.show()
# TFeatures = np.transpose(Features)
# plt.plot(TFeatures[0])
# plt.show()
# mydict_out = {}
# mydict_out['FuFFT_ma1'] = TFeatures[0]
# writer = pd.ExcelWriter('MASTER_Features.xlsx')
# DataFr = pd.DataFrame(data=mydict_out, index=rownames)
# DataFr.to_excel(writer, sheet_name='AE_Features')
# writer.close()
elif config['mode'] == 'fuse_corr':
print('Select MASTER Features xls')
root = Tk()
root.withdraw()
root.update()
filepath = filedialog.askopenfilename()
root.destroy()
mydict = pd.read_excel(filepath)
rownames = list(mydict.index.values)
length_data = len(rownames)
mydict = mydict.to_dict(orient='list')
Features = {}
for key, values in mydict.items():
Features[key] = movil_avg(mydict[key], config['n_mov_avg'])
CorrCoefs = {}
for feature1 in config['feature_array']:
mylist = []
for feature2 in config['feature_array']:
value = np.corrcoef(Features[feature1],
Features[feature2])[0][1]
mylist.append(value)
CorrCoefs[feature1] = mylist
writer = pd.ExcelWriter('Corr_Features.xlsx')
DataFr = pd.DataFrame(data=CorrCoefs, index=config['feature_array'])
DataFr.to_excel(writer, sheet_name='Corr_Features')
writer.close()
elif config['mode'] == 'test_cluster':
print('Select MASTER Features xls')
root = Tk()
root.withdraw()
root.update()
filepath = filedialog.askopenfilename()
root.destroy()
mydict = pd.read_excel(filepath)
rownames = list(mydict.index.values)
length_data = len(rownames)
mydict = mydict.to_dict(orient='list')
newdict = {}
for key, values in mydict.items():
newdict[key] = movil_avg(mydict[key], config['n_mov_avg'])
Features = []
for i in range(length_data):
example = []
for feature in config['feature_array']:
example.append(newdict[feature][i])
Features.append(example)
Features = np.array(Features)
from sklearn.cluster import DBSCAN, AffinityPropagation
model = AffinityPropagation()
print(model.fit_predict(Features))
elif config['mode'] == 'fuse_mlp':
print('Select MASTER Features xls')
root = Tk()
root.withdraw()
root.update()
filepath = filedialog.askopenfilename()
root.destroy()
mydict = pd.read_excel(filepath)
rownames = list(mydict.index.values)
length_data = len(rownames)
mydict = mydict.to_dict(orient='list')
newdict = {}
for key, values in mydict.items():
newdict[key] = movil_avg(mydict[key], config['n_mov_avg'])
Features = []
for i in range(length_data):
example = []
for feature in config['feature_array']:
example.append(newdict[feature][i])
Features.append(example)
Features = np.array(Features)
plt.plot(Features)
plt.show()
scaler_model = StandardScaler()
scaler_model.fit(Features)
Features = scaler_model.transform(Features)
plt.plot(Features)
plt.show()
nn = MLPRegressor(hidden_layer_sizes=(5),
activation='tanh',
solver='lbfgs0',
alpha=1.e1)
nn.fit(X=Features, y=np.linspace(0, 1, length_data))
z = nn.predict(Features)
# Features = pca.transform(Features)
# corr = []
# TFeatures = np.transpose(Features)
# for feature_pca in TFeatures:
# corr.append(np.corrcoef(np.ravel(feature_pca), np.arange(len(feature_pca)))[0][1])
# z = TFeatures[np.argmax(np.absolute(corr))]
# print(corr)
plt.plot(z)
plt.show()
elif config['mode'] == 'predict_from_xls_mlp':
print('Select xls')
root = Tk()
root.withdraw()
root.update()
Filepaths = filedialog.askopenfilenames()
root.destroy()
Feature = []
for filepath in Filepaths:
mydict = pd.read_excel(filepath, sheetname=config['sheet'])
mydict = mydict.to_dict(orient='list')
# Feature += mydict[config['feature']][:-2]
Feature += mydict[config['feature']]
Feature = list(np.nan_to_num(Feature))
Feature = movil_avg(Feature, config['n_mov_avg'])
Feature = np.array(Feature)
# fig, ax = plt.subplots(nrows=2, ncols=1, sharex=True, sharey=True)
# ax[0].plot(Feature, 'r')
Feature = median_filter(data=Feature, points=5, same_length=True)
# ax[1].plot(Feature, 'b')
# plt.show()
x_Feature = np.arange(len(Feature))
Train = Feature[0:int(config['train'] * len(Feature))]
x_Train = np.arange(float(len(Train)))
x_Predict = np.linspace(len(Train),
len(Feature),
num=len(Feature) - len(Train),
endpoint=False)
# scaler = StandardScaler()
# scaler = RobustScaler()
# scaler.fit(Train)
# Train = scaler.transform(Train)
clf = MLPRegressor(solver=config['solver'],
alpha=config['alpha'],
hidden_layer_sizes=config['layers'],
random_state=config['rs'],
activation=config['activation'],
tol=config['tol'],
verbose=True,
max_iter=config['max_iter'])
# from sklearn.tree import DecisionTreeRegressor
# clf = DecisionTreeRegressor()
n_pre = int(config['n_pre'] * len(Train))
m_post = int(config['m_post'] * len(Train))
n_ex = len(Train) - n_pre - m_post
print('+++++++++++++Info: Input points n = ', n_pre)
print('+++++++++++++Info: Output points m = ', m_post)
print('+++++++++++++Info: Training examples = ', n_ex)
a = input('enter to continue...')
T_Inputs = []
T_Outputs = []
# plt.plot(Train, 'k')
# plt.show()
for k in range(n_ex + 1):
T_Inputs.append(Train[k:k + n_pre])
T_Outputs.append(Train[k + n_pre:k + n_pre + m_post])
# aa = np.arange(len(Train[k : k + n_pre]))
# plt.plot(aa, Train[k : k + n_pre], 'b')
# bb = np.max(aa) + np.arange(len(Train[k + n_pre : k + n_pre + m_post]))
# plt.plot(bb, Train[k + n_pre : k + n_pre + m_post], 'r')
# plt.show()
# sys.exit()
from sklearn.model_selection import KFold, GroupKFold
kf = KFold(n_splits=100, shuffle=True)
# X = np.array([[1, 2], [3, 4], [5, 6], [7, 8], [9, 10], [11, 12], [13, 14], [15, 16], [17, 19]])
# y = np.array([[3], [5], [7], [9], [11], [13], [15], [17], [19]])
# # X = T_Inputs
# # y = T_Outputs
# for i in range(3):
# for train_index, test_index in kf.split(X):
# print("TRAIN:", train_index, "TEST:", test_index)
# # X_train, X_test = X[train_index], X[test_index]
# # y_train, y_test = y[train_index], y[test_index]
# sys.exit()
T_Inputs = np.array(T_Inputs)
T_Outputs = np.array(T_Outputs)
count = 0
epochs = 10
for i in range(epochs):
print(
'+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++Epoch',
count)
numb = 0
for train_index, test_index in kf.split(T_Inputs):
print('+++++++++++Batch', numb)
T_Inputs_train, T_Inputs_test = T_Inputs[
train_index], T_Inputs[test_index]
T_Outputs_train, T_Outputs_test = T_Outputs[
train_index], T_Outputs[test_index]
clf.partial_fit(T_Inputs_test, T_Outputs_test)
numb += 1
count += 1
# clf.fit(T_Inputs, T_Outputs)
print(
'+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n'
)
Predict = []
It_Train = list(Train)
for k in range(len(x_Predict) + m_post - 1):
P_Input = It_Train[n_ex + k + 1:n_ex + n_pre + k + 1]
P_Output = clf.predict([P_Input])
P_Output = P_Output[0]
Predict.append(P_Output[-1])
It_Train.append(P_Output[-1])
Predict = Predict[:-(m_post - 1)]
fig, ax = plt.subplots()
ax.set_xlabel('Accumulated Operating Hours', fontsize=13)
ax.set_ylabel('Health Index', fontsize=13)
ax.set_title('Linear Regression', fontsize=13)
fact = 5. / 3600.
ax.plot(x_Feature * fact, Feature, 'b', label='Real')
ax.plot(x_Predict * fact, Predict, 'r', label='Prediction')
ax.plot(x_Train * fact, Train, 'k', label='Training')
ax.legend()
plt.show()
plt.plot(x_Feature, Feature, 'b', x_Predict, Predict, 'r', x_Train,
Train, 'k')
plt.show()
# elif config['mode'] == 'predict_from_xls_mlp':
# print('Select xls')
# root = Tk()
# root.withdraw()
# root.update()
# Filepaths = filedialog.askopenfilenames()
# root.destroy()
# Feature = []
# for filepath in Filepaths:
# mydict = pd.read_excel(filepath, sheetname=config['sheet'])
# mydict = mydict.to_dict(orient='list')
# # Feature += mydict[config['feature']][:-2]
# Feature += mydict[config['feature']]
# Feature = list(np.nan_to_num(Feature))
# Feature = movil_avg(Feature, config['n_mov_avg'])
# Feature = np.array(Feature)
# # fig, ax = plt.subplots(nrows=2, ncols=1, sharex=True, sharey=True)
# # ax[0].plot(Feature, 'r')
# Feature = median_filter(data=Feature, points=5, same_length=True)
# # ax[1].plot(Feature, 'b')
# # plt.show()
# x_Feature = np.arange(len(Feature))
# Train = Feature[0:int(config['train']*len(Feature))]
# x_Train = np.arange(float(len(Train)))
# x_Predict = np.linspace(len(Train), len(Feature), num=len(Feature) - len(Train), endpoint=False)
# # scaler = StandardScaler()
# # scaler = RobustScaler()
# # scaler.fit(Train)
# # Train = scaler.transform(Train)
# clf = MLPRegressor(solver=config['solver'], alpha=config['alpha'], hidden_layer_sizes=config['layers'], random_state=config['rs'], activation=config['activation'], tol=config['tol'], verbose=True, max_iter=config['max_iter'])
# # from sklearn.tree import DecisionTreeRegressor
# # clf = DecisionTreeRegressor()
# n_pre = int(config['n_pre']*len(Train))
# m_post = int(config['m_post']*len(Train))
# n_ex = len(Train) - n_pre - m_post
# print('+++++++++++++Info: Input points n = ', n_pre)
# print('+++++++++++++Info: Output points m = ', m_post)
# print('+++++++++++++Info: Training examples = ', n_ex)
# a = input('enter to continue...')
# T_Inputs = []
# T_Outputs = []
# # plt.plot(Train, 'k')
# # plt.show()
# for k in range(n_ex + 1):
# T_Inputs.append(Train[k : k + n_pre])
# T_Outputs.append(Train[k + n_pre : k + n_pre + m_post])
# # aa = np.arange(len(Train[k : k + n_pre]))
# # plt.plot(aa, Train[k : k + n_pre], 'b')
# # bb = np.max(aa) + np.arange(len(Train[k + n_pre : k + n_pre + m_post]))
# # plt.plot(bb, Train[k + n_pre : k + n_pre + m_post], 'r')
# # plt.show()
# # sys.exit()
# from sklearn.model_selection import KFold, GroupKFold
# kf = KFold(n_splits=100, shuffle=True)
# # X = np.array([[1, 2], [3, 4], [5, 6], [7, 8], [9, 10], [11, 12], [13, 14], [15, 16], [17, 19]])
# # y = np.array([[3], [5], [7], [9], [11], [13], [15], [17], [19]])
# # # X = T_Inputs
# # # y = T_Outputs
# # for i in range(3):
# # for train_index, test_index in kf.split(X):
# # print("TRAIN:", train_index, "TEST:", test_index)
# # # X_train, X_test = X[train_index], X[test_index]
# # # y_train, y_test = y[train_index], y[test_index]
# # sys.exit()
# T_Inputs = np.array(T_Inputs)
# T_Outputs = np.array(T_Outputs)
# count = 0
# epochs = 10
# for i in range(epochs):
# print('+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++Epoch', count)
# numb = 0
# for train_index, test_index in kf.split(T_Inputs):
# print('+++++++++++Batch', numb)
# T_Inputs_train, T_Inputs_test = T_Inputs[train_index], T_Inputs[test_index]
# T_Outputs_train, T_Outputs_test = T_Outputs[train_index], T_Outputs[test_index]
# clf.partial_fit(T_Inputs_test, T_Outputs_test)
# numb += 1
# count += 1
# # clf.fit(T_Inputs, T_Outputs)
# print('+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n')
# Predict = []
# It_Train = list(Train)
# for k in range(len(x_Predict) + m_post - 1):
# P_Input = It_Train[n_ex + k + 1 : n_ex + n_pre + k + 1]
# P_Output = clf.predict([P_Input])
# P_Output = P_Output[0]
# Predict.append(P_Output[-1])
# It_Train.append(P_Output[-1])
# Predict = Predict[:-(m_post-1)]
# fig, ax = plt.subplots()
# ax.set_xlabel('Accumulated Operating Hours', fontsize=13)
# ax.set_ylabel('Health Index', fontsize=13)
# ax.set_title('Linear Regression', fontsize=13)
# fact = 5./3600.
# ax.plot(x_Feature*fact, Feature, 'b', label='Real')
# ax.plot(x_Predict*fact, Predict, 'r', label='Prediction')
# ax.plot(x_Train*fact, Train, 'k', label='Training')
# ax.legend()
# plt.show()
# plt.plot(x_Feature, Feature, 'b', x_Predict, Predict, 'r', x_Train, Train, 'k')
# plt.show()
elif config['mode'] == 'predict_from_xls_svr':
print('Select xls')
root = Tk()
root.withdraw()
root.update()
Filepaths = filedialog.askopenfilenames()
root.destroy()
Feature = []
for filepath in Filepaths:
mydict = pd.read_excel(filepath, sheetname=config['sheet'])
mydict = mydict.to_dict(orient='list')
Feature += mydict[config['feature']]
Feature = list(np.nan_to_num(Feature))
Feature = movil_avg(Feature, config['n_mov_avg'])
Feature = np.array(Feature)
x_Feature = np.arange(len(Feature))
Train = Feature[0:int(config['train'] * len(Feature))]
x_Train = np.arange(float(len(Train)))
x_Predict = np.linspace(len(Train),
len(Feature),
num=len(Feature) - len(Train),
endpoint=False)
# scaler = StandardScaler()
# scaler = RobustScaler()
# scaler.fit(Train)
# Train = scaler.transform(Train)
# clf = NuSVR(nu=0.5, C=1.0, kernel='poly')
clf = SVR(kernel='poly', verbose=True, degree=6, C=0.5)
n_pre = int(config['n_pre'] * len(Train))
m_post = 1
# m_post = int(config['m_post']*len(Train))
n_ex = len(Train) - n_pre - m_post
print('+++++++++++++Info: Input points n = ', n_pre)
print('+++++++++++++Info: Output points m = ', m_post)
print('+++++++++++++Info: Training examples = ', n_ex)
a = input('enter to continue...')
T_Inputs = []
T_Outputs = []
for k in range(n_ex + 1):
T_Inputs.append(Train[k:k + n_pre])
T_Outputs.append(Train[k + n_pre:k + n_pre + m_post])
clf.fit(T_Inputs, T_Outputs)
print(
'+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n'
)
Predict = []
It_Train = list(Train)
for k in range(len(x_Predict) + m_post - 1):
P_Input = It_Train[n_ex + k + 1:n_ex + n_pre + k + 1]
P_Output = clf.predict([P_Input])
P_Output = P_Output[0]
Predict.append(P_Output)
It_Train.append(P_Output)
# Predict = Predict[:-(m_post-1)]
fig, ax = plt.subplots()
ax.set_xlabel('Accumulated Operating Hours', fontsize=13)
ax.set_ylabel('Health Index', fontsize=13)
ax.set_title('Linear Regression', fontsize=13)
fact = 5. / 3600.
ax.plot(x_Feature * fact, Feature, 'b', label='Real')
ax.plot(x_Predict * fact, Predict, 'r', label='Prediction')
ax.plot(x_Train * fact, Train, 'k', label='Training')
ax.legend()
plt.show()
# plt.plot(x_Feature, Feature, 'b', x_Predict, Predict, 'r', x_Train, Train, 'k')
# plt.show()
elif config['mode'] == 'predict_from_xls_lin':
print('Select xls')
root = Tk()
root.withdraw()
root.update()
Filepaths = filedialog.askopenfilenames()
root.destroy()
Feature = []
for filepath in Filepaths:
mydict = pd.read_excel(filepath, sheetname=config['sheet'])
mydict = mydict.to_dict(orient='list')
Feature += mydict[config['feature']]
Feature = list(np.nan_to_num(Feature))
Feature = movil_avg(Feature, config['n_mov_avg'])
# Feature = median_filter(data=Feature, points=5, same_length=True)
Feature = np.array(Feature)
x_Feature = np.arange(len(Feature))
Train = Feature[0:int(config['train'] * len(Feature))]
x_Train = np.arange(float(len(Train)))
slope, intercept, r_value, p_value, std_err = stats.linregress(
x_Train, Train)
x_Predict = np.linspace(len(Train),
len(Feature),
num=len(Feature) - len(Train),
endpoint=False)
Predict = slope * x_Predict + intercept
fig, ax = plt.subplots()
ax.set_xlabel('Accumulated Operating Hours', fontsize=13)
ax.set_ylabel('Health Index', fontsize=13)
# ax.set_title('Linear Regression', fontsize=13)
ax.set_title('Neural Network', fontsize=13)
fact = 5. / 3600.
ax.plot(x_Feature * fact, Feature, 'b', label='Real')
ax.plot(x_Predict * fact, Predict, 'r', label='Prediction')
ax.plot(x_Train * fact, Train, 'k', label='Training')
ax.legend()
plt.show()
print(len(Feature))
else:
print('unknown mode')
sys.exit()
return
GPScord = []
for d in data:
temp = d.split(',')
temp2 = []
temp2.append(float(temp[1][2:][:-1]))
temp2.append(float(temp[2][2:][:-1]))
temp2.append(float(temp[3][2:][:-2]))
GPScord.append(temp2)
#for record in GPScord:
# print record
from pykalman import KalmanFilter
from pykalman import UnscentedKalmanFilter
#kf = KalmanFilter(n_dim_obs=3)
kf = KalmanFilter(transition_matrices=[[1, 1], [0, 1]],
observation_matrices=[[0.1, 0.5], [-0.3, 0.0]])
kf2 = UnscentedKalmanFilter()
measurements = GPScord
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
lat = []
lon = []
alt = []
for sample in GPScord:
lat.append(sample[0])
lon.append(sample[1])
alt.append(sample[2])
ax.scatter(lat, lon, alt, c='r', marker='o', s=10)
ax.plot(lat, lon, alt)
ax.set_xlabel('X Label')
class UnscentedKalmanFilterControl:
def __init__(self, dimState, delay, arm, knoiseU, controller):
'''
Initializes parameters to uses the function implemented below
inputs: -dimState: dimension of the state, here the state correspond to the muscular activation vector U, int
-dimObs: dimension of the observation, here the observation is the position of the arm given by the model, int
-delay: the delay with which we give the observation to the filter, int
-arm, armModel, class object
-rs, ReadSetupFile, class object
'''
self.name = "UnscentedKalmanFilter"
self.knoiseU = knoiseU
self.dimState = dimState
self.delay = delay
self.arm = arm
self.controller = controller
#initialization of some parameters for the filter
transition_covariance = np.eye(self.dimState) * 0.01
initial_state_mean = np.zeros(self.dimState)
observation_covariance = 1000 * np.eye(self.dimState)
initial_state_covariance = np.eye(self.dimState)
self.nextCovariance = np.eye(self.dimState) * 0.0001
self.ukf = UnscentedKalmanFilter(self.transitionFunctionUKF,
self.observationFunctionUKF,
transition_covariance,
observation_covariance,
initial_state_mean,
initial_state_covariance)
def initObsStore(self, state):
'''
Initialization of the observation storage
Input: -state: the state stored
'''
self.obsStore = []
for i in range(self.delay):
self.obsStore.append(state)
#print ("InitStore:", self.obsStore)
def storeObs(self, state):
'''
Stores the current state and returns the delayed state
Input: -state: the state to store
'''
for i in range(self.delay - 1):
self.obsStore[self.delay - i - 1] = self.obsStore[self.delay - i -
2]
self.obsStore[0] = state
#print ("After store:", self.obsStore)
#print ("retour:", self.obsStore[self.delay-1])
return self.obsStore[self.delay - 1]
def transitionFunctionUKF(self, state, transitionNoise=0):
'''
Transition function used by the filter, function of the state and the transition noise at time t and produces the state at time t+1
Inputs: -stateU: the state at time t, numpy array
-transitionNoise: transition noise at time t, numpy array
Output: -nextStateUNoise: the next State with noise added, numpy array
'''
#print("UKF trans state :", state)
U = self.controller.computeOutput(state)
nextState = self.arm.computeNextState(U, state)
return nextState + transitionNoise
def observationFunctionUKF(self, state, observationNoise=0):
'''
Observation function used by the filter, function of the state and the observation noise at time t and produces the observation at time t
Inputs: -stateU: the state at time t, numpy array
-observationNoise: the observation noise at time t, numpy array
Output: -nextObsNoise: observation at time t+1
'''
U = self.controller.computeOutput(state)
nextObs = self.arm.computeNextState(U, state)
nextObsNoise = nextObs + observationNoise
return nextObsNoise
def runUKF(self, state):
'''
Function used to compute the next state approximation with the filter
Inputs: -Unoisy: the state to feed the filter, here its the muscular activation vector U, numpy array of dimension (x, 1), here x = 6
-state: the state of the arm
Output: -stateApprox: the next state approximation, numpy array of dimension (x, 1), here x = 4
'''
#store the state of the arm to feed the filter with a delay on the observation
oldstate = self.storeObs(state)
#print("UKF old state :", oldstate)
#compute the nextState approximation ie here the next muscular activation
nextState, nextCovariance = self.ukf.filter_update(
state, self.nextCovariance, oldstate)
#compute the nextState i.e. the next position vector from the approximation of the next muscular activation vector given by the filter
return nextState
def debugStore(self):
state = np.array([1, 2, 3, 4])
self.initObsStore(state)
for i in range(5):
tmp = [rd.random() for x in range(4)]
self.storeObs(tmp)
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
import numpy as np
import pandas as pd
from matplotlib import cm, pyplot as plt
from pykalman import UnscentedKalmanFilter
from hmmlearn.hmm import GaussianHMM
ukf = UnscentedKalmanFilter(initial_state_mean=0, n_dim_obs=1)
def smooth(df, colname):
df[colname + '_smoothed'] = ukf.smooth(df[colname].values)[0]
return df
def parse_fmftime(namestring):
fn = namestring.split('/')[-1]
exp_id, CAMN, DATE, TIME = fn.split('_', 3)
FLY_ID = exp_id + '_' + DATE + '_' + TIME
fmftime = pd.to_datetime(DATE + TIME)
return FLY_ID, fmftime, exp_id
treatments = ['00','11','15','65']
genotypes = ['DB072','DB185','DB213']
if __name__ == "__main__":
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--flymad_dir', type=str, required=True,
help='directory of flymad files')
parser.add_argument('--parameters', type=list, required=True,
help='parameters to define HMM')
identify the next state.
"""
print("In _transition: ", current_kalman_state, noise)
# current_kalman_state[:n_dim_state, :]
next_state = system_dyamics.subs(list(zip(state, current_kalman_state)))
return list(next_state)
def jacobian_transition(current_kalman_state, noise):
j = jacobian.subs(list(zip(state, current_kalman_state)))
return list(j)
ukf = UnscentedKalmanFilter(
transition_functions=_transition,
observation_functions=_observation,
# transition_covariance=None,
# observation_covariance=None,
# initial_state_mean=None,
# initial_state_covariance=None,
n_dim_state=n_dim_kalman_state,
n_dim_obs=n_dim_kalman_observed,
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)
def calc_offset(s0,
sr0,
s1,
sr1,
ax=None,
start_seconds=None,
length_seconds=None,
chroma_weight=1.0,
sf_weight=1.0,
cf_weight=1.0):
try:
# Acutally calc the offset
lookahead_ms = 100
lookbehind_ms = 600
hop_lengths = [256, 512, 1024, 2048]
times = np.arange(-lookahead_ms, lookbehind_ms, 10)
all_chroma_errors = []
all_sf_errors = []
all_cf_errors = []
for hop_length in hop_lengths:
hop_length_seconds = hop_length / SAMPLE_RATE
sf0, cf0, chroma_s0 = gen_features(
s0, sr0, hop_length_seconds=hop_length_seconds)
sf1, cf1, chroma_s1 = gen_features(
s1, sr1, hop_length_seconds=hop_length_seconds)
if start_seconds is not None:
start_frames = int(start_seconds // hop_length_seconds)
sf0 = sf0.copy()[start_frames:]
sf1 = sf1.copy()[start_frames:]
cf0 = cf0.copy()[start_frames:]
cf1 = cf1.copy()[start_frames:]
chroma_s0 = chroma_s0.copy()[start_frames:]
chroma_s1 = chroma_s1.copy()[start_frames:]
if length_seconds is not None:
length_frames = int(length_seconds // hop_length_seconds)
sf0 = sf0.copy()[:length_frames]
sf1 = sf1.copy()[:length_frames]
cf0 = cf0.copy()[:length_frames]
cf1 = cf1.copy()[:length_frames]
chroma_s0 = chroma_s0.copy()[:length_frames]
chroma_s1 = chroma_s1.copy()[:length_frames]
map_sf0 = gen_peak_map(sf0)
map_sf1 = gen_peak_map(sf1)
map_cf0 = gen_peak_map(cf0)
map_cf1 = gen_peak_map(cf1)
chroma_errors = calc_errors(chroma_s0,
chroma_s1,
times,
hop_length,
exact=True)
std = np.std(chroma_errors)
std = 1 if std == 0 else std
chroma_errors = (chroma_errors - np.mean(chroma_errors)) / std
chroma_errors *= chroma_weight
if np.isfinite(chroma_errors).all():
all_chroma_errors.append(chroma_errors)
sf_errors = calc_errors(sf0, sf1, times, hop_length)
std = np.std(sf_errors)
std = 1 if std == 0 else std
sf_errors = (sf_errors - np.mean(sf_errors)) / std
sf_errors *= sf_weight
if np.isfinite(sf_errors).all():
all_sf_errors.append(sf_errors)
cf_errors = calc_errors(cf0, cf1, times, hop_length)
std = np.std(cf_errors)
std = 1 if std == 0 else std
cf_errors = (cf_errors - np.mean(cf_errors)) / std
cf_errors *= cf_weight
if np.isfinite(cf_errors).all():
all_cf_errors.append(cf_errors)
# Normalise
to_stack = []
if len(all_chroma_errors):
all_chroma_errors = np.stack(all_chroma_errors)
kf = KalmanFilter(initial_state_mean=0,
n_dim_obs=all_chroma_errors.shape[0])
median_chroma_errors = kf.smooth(
all_chroma_errors.transpose())[0].flatten()
to_stack.append(median_chroma_errors)
if len(all_sf_errors):
all_sf_errors = np.stack(all_sf_errors)
kf = KalmanFilter(initial_state_mean=0,
n_dim_obs=all_sf_errors.shape[0])
median_sf_errors = kf.smooth(
all_sf_errors.transpose())[0].flatten()
to_stack.append(median_sf_errors)
if len(all_cf_errors):
all_cf_errors = np.stack(all_cf_errors)
kf = KalmanFilter(initial_state_mean=0,
n_dim_obs=all_cf_errors.shape[0])
median_cf_errors = kf.smooth(
all_cf_errors.transpose())[0].flatten()
to_stack.append(median_cf_errors)
total = np.sum(np.stack(to_stack), axis=0)
peaks, _ = calc_peaks(-total, height=1.0, prominence=1.0)
if len(peaks) > 0:
offsets = times[peaks]
offset_ms = offsets[0]
else:
offsets = []
offset_ms = 0
if ax:
if len(all_chroma_errors):
for chroma_errors in all_chroma_errors:
ax.plot(times, chroma_errors, color='r', alpha=0.3)
ax.plot(times, median_chroma_errors, label='Chroma', color='r')
if len(all_sf_errors):
for sf_errors in all_sf_errors:
ax.plot(times, sf_errors, color='g', alpha=0.3)
ax.plot(times,
median_sf_errors,
label='Spectral Flux',
color='g')
if len(all_cf_errors):
for cf_errors in all_cf_errors:
ax.plot(times, cf_errors, color='b', alpha=0.3)
ax.plot(times,
median_cf_errors,
label='Crest Factor',
color='b')
plt.plot(times, total, label='Overall', color='k', linewidth=5)
for offset in offsets:
alpha = 1 if offset == offset_ms else 0.2
ax.axvline(x=offset, color='r', alpha=alpha, linestyle='--')
ax.legend()
except Exception as e:
raise
print("Could not sync audio", e)
offset_ms = 0
return int(offset_ms)
