class KalmanMovingAverage(object):
"""
Estimates the moving average of a price process via Kalman Filtering, using pykalman
"""
def __init__(self, asset, observation_covariance=1.0, initial_value=0,
initial_state_covariance=1.0, transition_covariance=0.05,
initial_window=30, maxlen=300, freq='1d'):
self.asset = asset
self.freq = freq
self.initial_window = initial_window
self.kf = KalmanFilter(transition_matrices=[1],
observation_matrices=[1],
initial_state_mean=initial_value,
initial_state_covariance=initial_state_covariance,
observation_covariance=observation_covariance,
transition_covariance=transition_covariance)
self.state_means = pd.Series([self.kf.initial_state_mean], name=self.asset)
self.state_covs = pd.Series([self.kf.initial_state_covariance], name=self.asset)
def update(self, observations):
for dt, observation in observations[self.asset].iterkv():
self._update(dt, observation)
def _update(self, dt, observation):
mu, cov = self.kf.filter_update(self.state_means.iloc[-1],
self.state_covs.iloc[-1],
observation)
self.state_means[dt] = mu.flatten()[0]
self.state_covs[dt] = cov.flatten()[0]
def calc_slope_intercept_using_kalman_filter(etfs, prices):
"""
Utilise the Kalman Filter from the pyKalman package
to calculate the slope and intercept of the regressed
ETF prices.
"""
delta = 1e-5
trans_cov = delta / (1 - delta) * np.eye(2)
obs_mat = np.vstack(
[prices[etfs[0]], np.ones(prices[etfs[0]].shape)]
).T[:, np.newaxis]
kf = KalmanFilter(
n_dim_obs=1,
n_dim_state=2,
initial_state_mean=np.zeros(2),
initial_state_covariance=np.ones((2, 2)),
transition_matrices=np.eye(2),
observation_matrices=obs_mat,
observation_covariance=1.0,
transition_covariance=trans_cov
)
state_means, state_covs = kf.filter(prices[etfs[1]].values)
return state_means, state_covs
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 main():
with open(sys.argv[1], "r") as fin:
tracks = cPickle.load(fin)
print "%d tracks loaded."%len(tracks)
index = 5
measurements = []
track = tracks[index]
t0 = track.utm[0][2]/1e6
for pt in track.utm:
t = pt[2]/1e6 - t0
measurements.append([pt[0], pt[1]])
measurements = np.asarray(measurements)
kf = KalmanFilter(n_dim_obs=2, n_dim_state=2).em(measurements, n_iter=100)
results = kf.smooth(measurements)[0]
fig = plt.figure(figsize=(9,9))
ax = fig.add_subplot(111, aspect='equal')
ax.plot([pt[0] for pt in results],
[pt[1] for pt in results],
'r-', linewidth=2)
ax.plot([pt[0] for pt in track.utm],
[pt[1] for pt in track.utm],
'x-')
#ax.set_xlim([const.SF_small_RANGE_SW[0], const.SF_small_RANGE_NE[0]])
#ax.set_ylim([const.SF_small_RANGE_SW[1], const.SF_small_RANGE_NE[1]])
plt.show()
def 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 test_kalman_filter_update():
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)
# use Kalman Filter
(x_filt, V_filt) = kf.filter(X=data.observations)
# use online Kalman Filter
n_timesteps = data.observations.shape[0]
n_dim_state = data.transition_matrix.shape[0]
x_filt2 = np.zeros((n_timesteps, n_dim_state))
V_filt2 = np.zeros((n_timesteps, n_dim_state, n_dim_state))
for t in range(n_timesteps - 1):
if t == 0:
x_filt2[0] = data.initial_state_mean
V_filt2[0] = data.initial_state_covariance
(x_filt2[t + 1], V_filt2[t + 1]) = kf.filter_update(
x_filt2[t], V_filt2[t], data.observations[t + 1],
transition_offset=data.transition_offsets[t]
)
assert_array_almost_equal(x_filt, x_filt2)
assert_array_almost_equal(V_filt, V_filt2)
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 on_update(self, data):
result = {}
result["timestamp"] = data["timestamp"]
if self.input.size() >= self.length:
independent_var = self.input.get_by_idx_range(key=None, start_idx=0, end_idx=-1)
symbol_set = set(self.input.keys)
depend_symbol = symbol_set.difference(self.input.default_key)
depend_var = self.input.get_by_idx_range(key=depend_symbol, start_idx=0, end_idx=-1)
obs_mat = np.vstack([independent_var.values, np.ones(independent_var.values.shape)]).T[:, np.newaxis]
model = KalmanFilter(
n_dim_obs=1,
n_dim_state=2,
initial_state_mean=np.zeros(2),
initial_state_covariance=np.ones((2, 2)),
transition_matrices=np.eye(2),
observation_matrices=obs_mat,
observation_covariance=1.0,
transition_covariance=self.trans_cov,
)
state_means, state_covs = model.filter(depend_var.values)
slope = state_means[:, 0][-1]
result[Indicator.VALUE] = slope
result[KalmanFilteringPairRegression.SLOPE] = slope
result[KalmanFilteringPairRegression.SLOPE] = state_means[:, 1][-1]
self.add(result)
else:
result[Indicator.VALUE] = np.nan
result[KalmanFilteringPairRegression.SLOPE] = np.nan
result[KalmanFilteringPairRegression.SLOPE] = np.nan
self.add(result)
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 smooth(dr):
'''Smoothing with Kalman Filter'''
kf = KalmanFilter()
for t in range(1,201):
if not os.path.exists('drivers/%d/%d_smooth.csv'%(dr,t)):
d = np.genfromtxt('drivers/%d/%d.csv'%(dr,t), delimiter=',', skip_header=True)
d[:,0] = kf.smooth(d[:,0])[0].T[0]
d[:,1] = kf.smooth(d[:,1])[0].T[0]
np.savetxt('drivers/%d/%d_smooth.csv'%(dr,t), d, delimiter=',', header='x,y', fmt='%.1f')
def kalman_smooth(observations, **kwargs):
'''
Smooth shit
'''
kwargs.setdefault('initial_state_mean', BASE_SCORE)
kwargs.setdefault('transition_covariance', 0.01 * np.eye(1))
kf = KalmanFilter(**kwargs)
states_pred = kf.smooth(observations)[0]
return states_pred[:, 0]
def 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 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 test_kalman_sampling():
kf = KalmanFilter(
data.transition_matrix,
data.observation_matrix,
data.transition_covariance,
data.observation_covariance,
data.transition_offsets,
data.observation_offset,
data.initial_state_mean,
data.initial_state_covariance)
(x, z) = kf.sample(100)
assert_true(x.shape == (100, data.transition_matrix.shape[0]))
assert_true(z.shape == (100, data.observation_matrix.shape[0]))
class KalmanRegression(object):
"""
Uses a Kalman Filter to estimate regression parameters
in an online fashion.
Estimated model: y ~ beta * x + alpha
"""
def __init__(self, initial_y, initial_x, delta=1e-5, maxlen=3000):
self._x = initial_x.name
self._y = initial_y.name
self.maxlen = maxlen
trans_cov = delta / (1 - delta) * np.eye(2)
obs_mat = np.expand_dims(
np.vstack([[initial_x], [np.ones(initial_x.shape[0])]]).T, axis=1)
self.kf = KalmanFilter(n_dim_obs=1, n_dim_state=2,
initial_state_mean=np.zeros(2),
initial_state_covariance=np.ones((2, 2)),
transition_matrices=np.eye(2),
observation_matrices=obs_mat,
observation_covariance=1.0,
transition_covariance=trans_cov)
state_means, state_covs = self.kf.filter(initial_y.values)
self.means = pd.DataFrame(state_means,
index=initial_y.index,
columns=['beta', 'alpha'])
self.state_cov = state_covs[-1]
def update(self, observations):
x = observations[self._x]
y = observations[self._y]
mu, self.state_cov = self.kf.filter_update(
self.state_mean, self.state_cov, y,
observation_matrix=np.array([[x, 1.0]]))
mu = pd.Series(mu, index=['beta', 'alpha'],
name=observations.name)
self.means = self.means.append(mu)
if self.means.shape[0] > self.maxlen:
self.means = self.means.iloc[-self.maxlen:]
def get_spread(self, observations):
x = observations[self._x]
y = observations[self._y]
return y - (self.means.beta[-1] * x + self.means.alpha[-1])
@property
def state_mean(self):
return self.means.iloc[-1]
def trackKalman():
l_publ = rospy.Publisher("tracking", tracking, queue_size = 10)
rate = rospy.Rate(10) # 10hz
initstate = [current_measurement[0], current_measurement[1], 0, 0]
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]]
initcovariance=1.0e-3*np.eye(4)
transistionCov=1.0e-4*np.eye(4)
observationCov=1.0e-1*np.eye(2)
kf=KalmanFilter(transition_matrices=Transition_Matrix,
observation_matrices =Observation_Matrix,
initial_state_mean=initstate,
initial_state_covariance=initcovariance,
transition_covariance=transistionCov,
observation_covariance=observationCov)
start = 1
t = 1
while not rospy.is_shutdown():
#cv2.rectangle(image,(face[0]-50,face[1]-50),(face[0]+50,face[1]+50),(255,0,0),2)
##cv2.rectangle(image,(face_center_pixels[0]-50,face_center_pixels[1]-50),(face_center_pixels[0]+50,face_center_pixels[1]+50),(255,0,0),2)
#cv2.imshow("Calibrated Boundary", image)
#cv2.waitKey(1)
if (start == 1):
start = 0
filtered_state_means = initstate
filtered_state_covariances = initcovariance
print 'current measurement: ', current_measurement
(pred_filtered_state_means, pred_filtered_state_covariances) = kf.filter_update(filtered_state_means, filtered_state_covariances, current_measurement);
t += 1
filtered_state_means = pred_filtered_state_means
filtered_state_covariances = pred_filtered_state_covariances
print 'predicted: ', filtered_state_means[0], filtered_state_means[1]
#print type(current_measurement[0])
#print type(filtered_state_means[0])
location = tracking()
location.x0 = current_measurement[0]
location.y0 = current_measurement[1]
location.x1 = filtered_state_means[0]
location.y1 = filtered_state_means[1]
l_publ.publish(location)
print '\n'
rate.sleep()
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]
class StateEstimationAltitudeCam:
def __init__(self):
self.state = [0, 0]
self.covariance = np.eye(2)
self.observation_covariance = np.array([[1]])
self.transition_covariance = np.array([[0.001, 0.001], [0.001, 0.001]])
self.kf = KalmanFilter(
transition_covariance=self.transition_covariance, # H
observation_covariance=self.observation_covariance, # Q
)
self.previous_update = None
def update(self, observations):
if not self.previous_update:
self.previous_update = time.time()
dt = time.time() - self.previous_update
self.state, self.covariance = self.kf.filter_update(
self.state,
self.covariance,
observations,
transition_matrix=np.array([[1, dt], [0, 1]]),
observation_matrix=np.array([[1, 0]]),
# observation_offset = np.array([, 0, 0])
# observation_covariance=np.array(0.1*np.eye(1))
)
self.previous_update = time.time()
def getAltitude(self):
return self.state[0]
def __init__(self):
self._observations = pickle.load(open("dumps/marker_observations.dump"))
transition_covariance = np.array([[0.025, 0.005], [0.0005, 0.01]])
self.kf = KalmanFilter(
transition_covariance=transition_covariance, observation_covariance=np.eye(1) * 1 # H # Q
)
self.altitude = []
self.co = []
self.state = [0, 0]
self.covariance = np.eye(2)
test = "test_9"
self._baro = pickle.load(open("data/duedalen/%s/barometer.dump" % test))
self._throttle = pickle.load(open("data/duedalen/%s/throttle.dump" % test))
self._sonar = pickle.load(open("data/12.11.13/%s/sonar.dump" % test))
self
self.acceleration = pickle.load(open("data/12.11.13/%s/acceleration.dump" % test))
self.z_velocity = [a.z_velocity for a in self.acceleration]
self.baro = [i[1] for i in self._baro]
self.throttle = [(i[1] - 1000.0) / (1000.0) for i in self._throttle]
print self.throttle
self.sonar = [i[1] for i in self._sonar]
self.cam_alt = [marker.z if marker else np.ma.masked for marker in self._observations]
for i in xrange(len(self._baro) - 1):
dt = self._baro[i + 1][0] - self._baro[i][0]
print dt
def __init__(self, df, dependent=None, independent=None, delta=None, trans_cov=None, obs_cov=None):
if not dependent:
dependent = df.columns[1]
if not independent:
independent = df.columns[2]
self.x = df[independent]
self.x.index = df.index
self.y = df[dependent]
self.y.index = df.index
self.dependent = dependent
self.independent = independent
self.delta = delta or 1e-5
self.trans_cov = trans_cov or self.delta / (1 - self.delta) * np.eye(2)
self.obs_mat = np.expand_dims(
np.vstack([[self.x.values], [np.ones(len(self.x))]]).T,
axis=1
)
self.obs_cov = obs_cov or 1
self.kf = KalmanFilter(n_dim_obs=1, n_dim_state=2,
initial_state_mean=np.zeros(2),
initial_state_covariance=np.ones((2, 2)),
transition_matrices=np.eye(2),
observation_matrices=self.obs_mat,
observation_covariance=self.obs_cov,
transition_covariance=self.trans_cov)
self.state_means, self.state_covs = self.kf.filter(self.y.values)
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))
class StateEstimationAccel:
def __init__(self):
"""
# x,y,z, Xvelo,Yvelo, Zvelo Xaccel, Yaccel, Zaccel, roll, pitch, yaw
"""
self.state = [0, 0, 0, 0, 0] # x, y ,z
self.covariance = np.eye(5)
transition_covariance = np.array(
[[0.01, 0, 0, 0, 0], [0, 0.1, 0, 0, 0], [0, 0, 0.001, 0, 0], [0, 0, 0, 0.001, 0], [0, 0, 0, 0, 0.001]]
)
self.observation_covariance = np.eye(3) * 0.1
# self.transition_covariance = np.eye(5) * 0.001
print transition_covariance
self.kf = KalmanFilter(
transition_covariance=transition_covariance, observation_covariance=self.observation_covariance # H # Q
)
def update(self, dt, observations):
self.state, self.covariance = self.kf.filter_update(
self.state,
self.covariance,
observations,
transition_matrix=np.array(
[[1, dt, 0.5 * (dt ** 2), 0, 0], [0, 1, dt, 0, 0], [0, 0, 1, 0, 0], [0, 0, 0, 1, 0], [0, 0, 0, 0, 1]]
),
observation_matrix=np.array([[0, 0, 1, 0, 0], [0, 0, 0, 1, 0], [0, 0, 0, 0, 1]]),
# observation_offset=np.array([[0]]),
)
def _KalmanFilterRegression( self ): """ Use Kalman Filter to obtain first-order auto-regression parameters r_t = beta_0 + beta_1 * r_(t-1) """ returns = self.returns; _trans_cov_delta = 1e-3; # Transition matrix and covariance trans_mat = np.eye(2); # Assume beta is not to change from t-1 to t _delta = _trans_cov_delta; trans_cov = _delta / (1 - _delta) * np.eye(2); # This _delta and trans_cov seem to have great impact on the result # form Observation Matrix data = returns.values[:-1,:]; _, num_stocks = data.shape; data = np.expand_dims( data, axis = 2 ); # T-by-2-by-1 array obs_mat = np.insert( data, 1, 1, axis = 2 ); # Insert column of ones T-2-2 array obs_cov = np.eye( num_stocks ); # assume zero correlation among noises in observed stock returns #print "Shape of observation matrix is ", obs_mat.shape; #print "Example of obs_mat is ", obs_mat[:2,:,:]; # Observed stock returns: r_t index = returns.index[1:]; # index for beta_1(t) observations = returns.values[1:,:] # matrix of r_t # Form Kalman Filter and then filter! kf = KalmanFilter( n_dim_obs = num_stocks, n_dim_state = 2, # 2 regression parameters initial_state_mean = np.zeros(2), initial_state_covariance = np.ones((2,2)), transition_matrices = trans_mat, transition_covariance = trans_cov, observation_matrices = obs_mat, observation_covariance = obs_cov, ); state_means, state_covs = kf.filter( observations ); slope = pd.Series( state_means[:,0], index ); intercept = pd.Series( state_means[:,1], index ); kf_coefficients_df = pd.DataFrame( [ intercept, slope ], index = [ "coeff_0", "coeff_1" ] ); self.kf_coefficients_df = kf_coefficients_df.transpose(); return (intercept, slope);
def check_dims(n_dim_state, n_dim_obs, kwargs):
kf = KalmanFilter(**kwargs)
(transition_matrices, transition_offsets, transition_covariance,
observation_matrices, observation_offsets, observation_covariance,
initial_state_mean, initial_state_covariance) = (
kf._initialize_parameters()
)
assert_true(transition_matrices.shape == (n_dim_state, n_dim_state))
assert_true(transition_offsets.shape == (n_dim_state,))
assert_true(transition_covariance.shape == (n_dim_state, n_dim_state))
assert_true(observation_matrices.shape == (n_dim_obs, n_dim_state))
assert_true(observation_offsets.shape == (n_dim_obs,))
assert_true(observation_covariance.shape == (n_dim_obs, n_dim_obs))
assert_true(initial_state_mean.shape == (n_dim_state,))
assert_true(
initial_state_covariance.shape == (n_dim_state, n_dim_state)
)
def test_kalman_predict():
kf = KalmanFilter(
data.transition_matrix,
data.observation_matrix,
data.transition_covariance,
data.observation_covariance,
data.transition_offsets,
data.observation_offset,
data.initial_state_mean,
data.initial_state_covariance)
x_smooth = kf.smooth(X=data.observations)[0]
assert_array_almost_equal(
x_smooth[:501],
data.smoothed_state_means[:501],
decimal=7
)
def 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 __init__(self):
rospy.loginfo("Starting matlab engine...")
self.eng = matlab.engine.start_matlab()
rospy.loginfo("Done")
self.DEBUG = True
# needle points in each image
self.left_points = None
self.right_points = None
# used to convert ROS image messages to format usable by opencv
self.cv_bridge = cv_bridge.CvBridge()
self.left_image = None
self.right_image = None
self.init_subscribers()
self.init_publishers()
self.info = {'l': None, 'r': None, 'b': None, 'd': None}
if self.DEBUG:
self.init_debug_publishers()
rospy.loginfo("Initialized needle tracker.")
# generate models for needle
self.model = models.generate_2d_unit_arc(12, 3.0/8.0, "needle_registration/input/arc_2d")
# more densely interpolated model
self.dense_model = models.generate_2d_unit_arc(100, 3.0/8.0, "needle_registration/input/arc_2d")
# setup kalman filter update
self.left_needle_pose = np.zeros(6)
self.left_needle_covar = np.identity(6)
self.left_kf = KalmanFilter(initial_state_mean=self.left_needle_pose, n_dim_obs=6)
self.right_needle_pose = np.zeros(6)
self.right_needle_covar = np.identity(6)
self.right_kf = KalmanFilter(initial_state_mean=self.left_needle_pose, n_dim_obs=6)
# set up exit handler
signal.signal(signal.SIGINT, self.exit_handler)
def __init__(self):
self.state = [0, 0]
self.covariance = np.eye(2)
self.observation_covariance = np.array([[1]])
self.transition_covariance = np.array([[0.001, 0.001], [0.001, 0.001]])
self.kf = KalmanFilter(
transition_covariance=self.transition_covariance, # H
observation_covariance=self.observation_covariance, # Q
)
self.previous_update = None
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 makeKalman(self, pts):
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=pts[0,0]
yinit=pts[0,1]
vxinit=pts[1,0]-pts[0,0]
vyinit=pts[1,1]-pts[0,1]
initstate=[xinit,yinit,vxinit,vyinit]
initcovariance=1.0e-3*np.eye(4)
transistionCov=1.0e-4*np.eye(4)
observationCov=1.0e-1*np.eye(2)
self.kf=KalmanFilter(transition_matrices=Transition_Matrix,
observation_matrices =Observation_Matrix,
initial_state_mean=initstate,
initial_state_covariance=initcovariance,
transition_covariance=transistionCov,
observation_covariance=observationCov)
self.means, self.covariances = self.kf.filter(pts)
self.v = self.means[-1][2:]
def plot_with_kalman_smoothing(cpu_data):
kalman_data = cpu_data[['temperature', 'cpu_percent', 'sys_load_1']]
# Preparing Kalman filter parameters
initial_state = kalman_data.iloc[0]
observation_covariance = np.diag([0.5, 0.5, 0.5]) ** 2
transition_covariance = np.diag([0.2, 0.2, 0.2]) ** 2
transition = [[1, -1, 0.7], [0, 0.6, 0.03], [0, 1.3, 0.8]]
# Creating Kalman filter
kf = KalmanFilter(
initial_state_mean=initial_state,
initial_state_covariance=observation_covariance,
observation_covariance=observation_covariance,
transition_covariance=transition_covariance,
transition_matrices=transition
)
kalman_smoothed, _ = kf.smooth(kalman_data)
plt.plot(cpu_data['timestamp'], kalman_smoothed[:, 0], 'g-')
def getPredictedValuesVer2(measurements, standard_deviation = None):
#this method use PyKalman module
#Apply this method only for Gains.
if standard_deviation == None:
#if standard deviation is not specified, then get it only from measurements list
standard_deviation = getStandardDeviation(measurements)
#adapt the transition_covariance. Temporary
if standard_deviation < 1e-9:
transition_covariance = 1e-21
else:
transition_covariance = 5e-17
kf = KalmanFilter(transition_matrices = [1], observation_matrices = [1], transition_covariance = transition_covariance, observation_covariance = math.pow(standard_deviation, 2))
tmp= kf.filter(measurements)[0].tolist()
for i in range(0, len(tmp)):
tmp[i] = tmp[i][0]
return tmp
def Spread_KalmanFilterRegression(df,pair1,pair2):
x=KalmanFilterAverage(df[pair1])
y=KalmanFilterAverage(df[pair2])
delta = 1e-3
trans_cov = delta / (1 - delta) * np.eye(2) # How much random walk wiggles
obs_mat = np.expand_dims(np.vstack([[x], [np.ones(len(x))]]).T, axis=1)
kf = KalmanFilter(n_dim_obs=1, n_dim_state=2, # y is 1-dimensional, (alpha, beta) is 2-dimensional
initial_state_mean=[0,0],
initial_state_covariance=np.ones((2, 2)),
transition_matrices=np.eye(2),
observation_matrices=obs_mat,
observation_covariance=2,
transition_covariance=trans_cov)
# Get running estimates and errors for the state parameters
state_means, state_covs = kf.filter(y.values)
#Build the spread
spread=pd.DataFrame()
spread['{}_{}'.format(pair1,pair2)]=df[pair1]-state_means[:,0]*df[pair2]
spread.dropna(inplace=True)
return(spread)
def kalman_filter(data_1, niter):
kalman_param = np.zeros((data_1.shape[0], 2))
kalman_smooth = np.zeros((data_1.shape[0], data_1.shape[1]))
""" EM algorithm is slow for Kalman initialization"""
for i in range(data_1.shape[0]):
kf = KalmanFilter(em_vars=[
'transition_matrices', 'observation_matrices',
'transition_covariance', 'observation_covariance',
'observation_offsets', 'initial_state_mean',
'initial_state_covariance'
])
kf = kf.em(X=data_1[i], n_iter=niter)
kalman_smooth[i, ] = np.ravel(kf.smooth(data_1[i])[0])
kalman_param[i, 0] = np.max(kalman_smooth[i, ])
kalman_param[i, 1] = np.std(kalman_smooth[i, ])
""" Eigen Value for correlated smoothed series """
eigen_corr, t = np.linalg.eig(np.corrcoef(kalman_smooth))
eigen_corr.sort()
eigen_corr = eigen_corr[::-1]
return eigen_corr, kalman_param
def calc_slope_intercept_kalman(etfs, prices):
"""
Utilise the Kalman Filter from the PyKalman package
to calculate the slope and intercept of the regressed
ETF prices.
"""
delta = 1e-5
trans_cov = delta / (1 - delta) * np.eye(2)
obs_mat = np.vstack([prices[etfs[0]],
np.ones(prices[etfs[0]].shape)]).T[:, np.newaxis]
kf = KalmanFilter(n_dim_obs=1,
n_dim_state=2,
initial_state_mean=np.zeros(2),
initial_state_covariance=np.ones((2, 2)),
transition_matrices=np.eye(2),
observation_matrices=obs_mat,
observation_covariance=1.0,
transition_covariance=trans_cov)
state_means, state_covs = kf.filter(prices[etfs[1]].values)
return state_means, state_covs
def init_filter(self):
random_state = np.random.RandomState(0)
transition_matrix = [[1, 0.1], [0, 1]]
transition_offset = [-0.1, 0.1]
observation_matrix = np.eye(2) + random_state.randn(2, 2) * 0.1
observation_offset = [1.0, -1.0]
transition_covariance = np.eye(2)
observation_covariance = np.eye(2) + random_state.randn(2, 2) * 0.1
initial_state_mean = [5, -5]
initial_state_covariance = [[1, 0.1], [-0.1, 1]]
# sample from model
self.kf = KalmanFilter(transition_matrix,
observation_matrix,
transition_covariance,
observation_covariance,
transition_offset,
observation_offset,
initial_state_mean,
initial_state_covariance,
random_state=random_state)
def __init__(self, initial_y, initial_x, delta=1e-5, maxlen=3000):
self._x = initial_x.name
self._y = initial_y.name
self.maxlen = maxlen
trans_cov = delta / (1 - delta) * np.eye(2)
obs_mat = np.expand_dims(
np.vstack([[initial_x], [np.ones(initial_x.shape[0])]]).T, axis=1)
self.kf = KalmanFilter(n_dim_obs=1, n_dim_state=2,
initial_state_mean=np.zeros(2),
initial_state_covariance=np.ones((2, 2)),
transition_matrices=np.eye(2),
observation_matrices=obs_mat,
observation_covariance=1.0,
transition_covariance=trans_cov)
state_means, state_covs = self.kf.filter(initial_y.values)
self.means = pd.DataFrame(state_means,
index=initial_y.index,
columns=['beta', 'alpha'])
self.state_cov = state_covs[-1]
def kalman(x, n_dim_state):
#parameter: time series,
kf = KalmanFilter(n_dim_state=n_dim_state, n_dim_obs=x.shape[0])
x = x.T
#loglikelihoods = range(1, 51)
#estimates = np.ones(50)
#for i in range(len(loglikelihoods)):
#
# kf = kf.em(X=x, n_iter= 1, em_vars = 'all')
# estimates[i] = kf.loglikelihood(x)
# print(i)
converged = False
tol = 1e-8
LL = []
i = 0
while converged == False:
kf = kf.em(X=x, n_iter=1, em_vars='all')
LL.append(kf.loglikelihood(x))
LLold = LL[i]
if i <= 2:
LLbase = LL[i]
elif ((LL[i] - LLbase) < (1 + tol) * (LLold - LLbase)):
converged = True
i = i + 1
return i - 1
def KF_filter(full_data):
data_i=[]
filter_i=[]
for i in range(16):
data_i.append(full_data[:,i*3:(i+1)*3])
for d_i in data_i:
d=np.asarray(d_i)
a=1/50
A=np.asarray([[1, 0, 0, a, 0, 0, 0.5*(a**2), 0, 0],
[0, 1, 0, 0, a, 0, 0, 0.5*(a**2), 0],
[0, 0, 1, 0, 0, a, 0, 0, 0.5*(a**2)],
[0, 0, 0, 1, 0 ,0, a, 0, 0],
[0, 0, 0, 0, 1, 0, 0, a, 0],
[0, 0, 0, 0, 0, 1, 0, 0, a],
[0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1]])
C=C = np.asarray([
[1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0]])
R=np.identity(3,dtype=int)
Q=np.identity(9,dtype=int)
velocity=np.gradient(d)[1]
acceleration=np.gradient(velocity)[1]
tot=np.concatenate((d,velocity,acceleration),axis=1)
mean=np.mean(tot,axis=0)
covariance=np.cov(tot,rowvar = False)
kf = KalmanFilter(transition_matrices = A, observation_matrices = C, transition_covariance = Q, observation_covariance = R,
initial_state_mean = mean, initial_state_covariance = covariance)
filter=kf.filter(d)[0]
filter_i.append(filter[:,:3])
return np.concatenate(filter_i,axis=1)
class KFMulti():
"""
Tracks multiple objects (without association) using pykalman
"""
def __init__(self, measurements):
#shared KF (same dynamics used by all objects, we update tr and obs)
self.num_tracks = len(measurements)
self.observation_matrix = [[1, 0, 0, 0], [0, 0, 1, 0]]
self.transition_matrix = [[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1],
[0, 0, 0, 1]]
self.xs = [] #state estimates
self.Ps = [] #cov matrices
self.kf = KalmanFilter(transition_matrices=self.transition_matrix,
observation_matrices=self.observation_matrix)
self.createKFs(measurements)
def createKFs(self, meas_all):
#create a KF object for each tracked object, sharing tr and obs, but using individual states
for meas in meas_all:
init_state = [meas[0][0], 0, meas[0][1], 0] #in form [x,xv,y,yv]
kf = KalmanFilter(transition_matrices=self.transition_matrix,
observation_matrices=self.observation_matrix,
initial_state_mean=init_state)
kf = kf.em(np.array(meas), n_iter=5)
x, P = kf.smooth(np.array(meas))
self.xs.append(x[-1, :]) #save the last state estimate
self.Ps.append(P[-1, :]) #save last cov matrix estimate
def update(self, obs_all=None):
#update each state and cov estimate using latest obs (or None for Constant Vel)
#if obs_all is set, length must be same as objects we have init on
#return just the [x,y] for each tracked object (ie no vel estimate)
if obs_all is not None:
if len(obs_all) != self.num_tracks:
##Incorrect length of observations
return None
for i in range(self.num_tracks):
obs = None
if obs_all is not None:
obs = obs_all[i]
(x_new, P_new) = self.kf.filter_update(
filtered_state_mean=self.xs[i],
filtered_state_covariance=self.Ps[i],
observation=obs)
self.xs[i] = x_new
self.Ps[i] = P_new
new_pred = []
for obj in self.xs:
new_pred.append([obj[0], obj[2]])
return new_pred
def KalmanFilterRegression(x, y):
delta = 1e-3
trans_cov = delta / (1 - delta) * np.eye(
2) # How much random walk wiggles
obs_mat = np.expand_dims(np.vstack([[x], [np.ones(len(x))]]).T, axis=1)
kf = KalmanFilter(
n_dim_obs=1,
n_dim_state=2, # y is 1-dimensional, (alpha, beta) is 2-dimensional
initial_state_mean=[0, 0],
initial_state_covariance=np.ones((2, 2)),
transition_matrices=np.eye(2),
observation_matrices=obs_mat,
observation_covariance=2,
transition_covariance=trans_cov)
# Use the observations y to get running estimates and errors for the state parameters
state_means, state_covs = kf.filter(y.values)
slope = state_means[:, 0]
intercept = state_means[:, 1]
std_q = np.array([np.sqrt(cov[0][0]) for cov in state_covs])
return slope, intercept, std_q
def hedge_ratio(Y, X):
"""
Use the Kalman Filter from the pyKalman package
to calculate the slope and intercept of the regressed
prices
"""
delta = 1e-5 # To Optimize
trans_cov = delta / (1 - delta) * np.eye(2)
obs_mat = np.vstack([X, np.ones(X.shape)]).T[:, np.newaxis]
kf = KalmanFilter(n_dim_obs=1,
n_dim_state=2,
initial_state_mean=np.zeros(2),
initial_state_covariance=np.ones((2, 2)),
transition_matrices=np.eye(2),
observation_matrices=obs_mat,
observation_covariance=2.0,
transition_covariance=trans_cov)
state_means, state_covs = kf.filter(Y)
return state_means[:, 1]
def __init__(self):
self.state = [0, 0]
self.covariance = np.eye(2)
# self.transition_covariance = np.array([
# [0.0000025, 0.000005],
# [0.0000005, 0.0000001],
# ])
self.observation_covariance = np.array([
[0.01, 0],
[0, 0.5],
])
self.transition_covariance = np.array([
[0.003, 0.05],
[0, 0.02],
])
self.kf = KalmanFilter(
transition_covariance=self.transition_covariance, # H
observation_covariance=self.observation_covariance, # Q
)
self.previous_update = None
def kalman_filter(data, error):
data = np.ma.masked_less(data, -900)
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 = data[0, 0]
yinit = data[0, 1]
vxinit = data[1, 0] - data[0, 0]
vyinit = data[1, 1] - data[0, 1]
initstate = [xinit, yinit, vxinit, vyinit]
initcovariance = error * np.eye(4)
transistionCov = error * np.eye(4)
observationCov = error * np.eye(2)
kf = KalmanFilter(transition_matrices=Transition_Matrix,
observation_matrices=Observation_Matrix,
initial_state_mean=initstate,
initial_state_covariance=initcovariance,
transition_covariance=transistionCov,
observation_covariance=observationCov)
(filtered_state_means, _) = kf.filter(data)
return np.array(filtered_state_means)
def __init__(self):
"""
# x,y,z, Xvelo,Yvelo, Zvelo Xaccel, Yaccel, Zaccel, roll, pitch, yaw
"""
self.state = [0, 0, 0, 0, 0] # x, y ,z
self.covariance = np.eye(5)
transition_covariance = np.array([
[0.01, 0, 0, 0, 0],
[0, 0.1, 0, 0, 0],
[0, 0, 0.001, 0, 0],
[0, 0, 0, 0.001, 0],
[0, 0, 0, 0, 0.001],
])
self.observation_covariance = np.eye(3) * 0.1
# self.transition_covariance = np.eye(5) * 0.001
print transition_covariance
self.kf = KalmanFilter(
transition_covariance=transition_covariance, # H
observation_covariance=self.observation_covariance, # Q
)
def setup_kf(imean, a=[[1, 0, 0.5, 0], [0, 1, 0, 0.5], [0, 0, 1, 0], [0, 0, 0, 1]], o=[[1, 0, 0, 0], [0, 1, 0, 0]]):
"""
Initialize Kalman filter object for each new tracks.
The transfermation matrix (a) and observation matrix (o) can be tuned to better suit with a specific motion pattern,
but to preserve generality we are using a simple constant speed model here.
Args:
imean (2x1 array): 1x2 array or list of the location of centroid.
a (array): transformation matrix that governs state transition to the next time step. Size varies with model.
o (array): observation matrix that defines the observable states. Size varies with model.
"""
return KalmanFilter(transition_matrices=a, observation_matrices=o, initial_state_mean=[*imean,0,0])
def smooth_measurements(measurements):
measurements = np.asarray(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=10)
(smoothed_state_means, smoothed_state_covariances) = kf1.smooth(measurements)
return smoothed_state_means
def __init__(self): self.pos = [0,0,0] self.predicted = False self.idLocal = -1 self.idGlobal = -1 self.timestamp = time() self.idCam = -1 self.ocurrencias = 0 self.vol = -1 # Volumen self.seen = False # Flag update kalman self.kf = KalmanFilter( transition_matrices=transition_matrix, observation_matrices=observation_matrix, transition_offsets=transition_offset, observation_offsets=observation_offset, random_state=0 ) self.Vx = 0 self.Vy = 0 self.maskMultiDel = False self.dist = 0
def update_smoo_hists(self):
"""
update self.smoo_X_hist, self.smoo_Y_hist, self.smoo_R3_hist according to filter_type
"""
if self.dps_settings.filter_type == 'none':
self.smoo_X_hist = self.X_hist
self.smoo_Y_hist = self.Y_hist
self.smoo_R3_hist = self.R3_hist
self.smoo_velX_hist = self.velX_hist
self.smoo_velY_hist = self.velY_hist
self.smoo_velR3_hist = self.velR3_hist
elif self.dps_settings.filter_type == 'kalman':
self.update_sampled_hists()
kf = KalmanFilter(
initial_state_mean=0,
n_dim_obs=1,
)
self.smoo_X_hist, _ = kf.filter(self.sampled_X_hist)
self.smoo_Y_hist, _ = kf.filter(self.sampled_Y_hist)
self.smoo_R3_hist, _ = kf.filter(self.sampled_R3_hist)
def pyKalman4DTest(params, greater, samples):
kp = params['kparams']
#kp['initial_state_mean']=[quadsummary([s['unusual_packet'] for s in samples]),
# quadsummary([s['other_packet'] for s in samples]),
# numpy.mean([s['unusual_tsval'] for s in samples]),
# numpy.mean([s['other_tsval'] for s in samples])]
kf = KalmanFilter(n_dim_obs=4, n_dim_state=4, **kp)
smooth, covariance = kf.smooth([(s['unusual_packet'], s['other_packet'],
s['unusual_tsval'], s['other_tsval'])
for s in samples])
m = numpy.mean(smooth)
if greater:
if m > params['threshold']:
return 1
else:
return 0
else:
if m < params['threshold']:
return 1
else:
return 0
def _log_likelihood(self, theta):
Gamma0, Gamma1, Psi, Pi, C_in, obs_matrix, obs_offset = self._eval_matrix(
theta, to_array=True)
for mat in [Gamma0, Gamma1, Psi, Pi, C_in]:
if isnan(mat).any() or isinf(mat).any():
return -inf
G1, C_out, impact, fmat, fwt, ywt, gev, eu, loose = gensys(
Gamma0, Gamma1, C_in, Psi, Pi)
if eu[0] == 1 and eu[1] == 1:
# TODO add observation covariance to allow for measurment errors
kf = KalmanFilter(G1, obs_matrix, impact @ impact.T, None,
C_out.reshape(self.n_state),
obs_offset.reshape(self.n_obs))
L = kf.loglikelihood(self.data)
else:
L = -inf
return L
def __init__(self,
dt,
conv_speed,
x0=np.zeros((1, 4)),
covar0=5 * np.eye(4).reshape((1, 4, 4))):
self.dt = dt
self.conv_speed = conv_speed
self.state_dim = 4
# --- parameters ---
self.A = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 0, 0],
[0, 0, 0, 0]])
self.B = np.array([[0, 0], [0, 0], [1, 0], [0, 1]])
self.u = np.array([
conv_speed[1] * np.cos(conv_speed[0]),
conv_speed[1] * np.sin(conv_speed[0])
]) #conveyer speed factored into x and y
self.C = np.eye(4)
self.d = np.zeros(4, )
self.R = 0.7 * np.eye(4) # state (process) noise covariance
self.Q = 1.0 * np.eye(4) # observation noise covariance
self.x0 = x0
self.covar0 = covar0
transition_matrix = self.A
transition_offset = np.matmul(self.B, self.u) # B*u = b*v_conv
observation_matrix = self.C
observation_offset = self.d
transition_covariance = self.R
observation_covariance = self.Q
initial_state_mean = self.x0
initial_state_covariance = self.covar0
self.kf = KalmanFilter(transition_matrix, observation_matrix,
transition_covariance, observation_covariance,
transition_offset, observation_offset,
initial_state_mean, initial_state_covariance)
self.states_means = self.x0 # list of predictions of states means from KF
self.states_covars = self.covar0 # list of predictions of states covariances from KF
self.observations = self.x0 # list of the observations
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 _process_update(self, source: str, timestamp: int, data: Dict[str,
float]):
result = {}
if input.size() >= self.length:
independent_var = self.first_input.get_by_idx_range(key=None,
start_idx=0,
end_idx=-1)
symbol_set = set(self.first_input.keys)
depend_symbol = symbol_set.difference(self.first_input.default_key)
depend_var = self.first_input.get_by_idx_range(key=depend_symbol,
start_idx=0,
end_idx=-1)
obs_mat = np.vstack([
independent_var.values,
np.ones(independent_var.values.shape)
]).T[:, np.newaxis]
model = KalmanFilter(n_dim_obs=1,
n_dim_state=2,
initial_state_mean=np.zeros(2),
initial_state_covariance=np.ones((2, 2)),
transition_matrices=np.eye(2),
observation_matrices=obs_mat,
observation_covariance=1.0,
transition_covariance=self.trans_cov)
state_means, state_covs = model.filter(depend_var.values)
slope = state_means[:, 0][-1]
result[Indicator.VALUE] = slope
result[KalmanFilteringPairRegression.SLOPE] = slope
result[KalmanFilteringPairRegression.SLOPE] = state_means[:, 1][-1]
self.add(timestamp=timestamp, data=result)
else:
result[Indicator.VALUE] = np.nan
result[KalmanFilteringPairRegression.SLOPE] = np.nan
result[KalmanFilteringPairRegression.SLOPE] = np.nan
self.add(timestamp=timestamp, data=result)
def _process_kalman(df):
kf1 = KalmanFilter(transition_matrices=[1],
observation_matrices=[1],
initial_state_mean=0,
initial_state_covariance=1,
observation_covariance=1,
transition_covariance=.01)
x_state_means, _ = kf1.filter(df.x.values)
y_state_means, _ = kf1.filter(df.y.values)
df['smooth_x'] = x_state_means
df['smooth_y'] = y_state_means
obs_mat = np.vstack([df.smooth_x,
np.ones(df.smooth_x.shape)]).T[:, np.newaxis]
delta = 1e-5
trans_cov = delta / (1 - delta) * np.eye(2)
kf2 = KalmanFilter(n_dim_obs=1,
n_dim_state=2,
initial_state_mean=np.zeros(2),
initial_state_covariance=np.ones((2, 2)),
transition_matrices=np.eye(2),
observation_matrices=obs_mat,
observation_covariance=1.0,
transition_covariance=trans_cov)
reg_state, _ = kf2.filter(df.smooth_y.values)
df['beta'], df['alpha'] = reg_state[:, 0], reg_state[:, 1]
df['new_x'] = df['smooth_x'] * df['beta'] + df['alpha']
df['spread'] = df['smooth_y'] - df['new_x']
df['stdev'] = df.expanding().spread.std()
df['z'] = df['spread'] / df['stdev']
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 __init__(self,
init_state,
transition_matrix,
observation_matrix,
n_iter=5,
train_size=15):
"""
Create the Kalman filter.
:param init_state: Initial state of the Kalman filter. Should be equal to first element in first_train_batch.
:param transition_matrix: adjacency matrix representing state transition from t to t+1 for any time t
Example: http://campar.in.tum.de/Chair/KalmanFilter
Most likely, this will be an NxN eye matrix the where N is the number of variables
being estimated
:param observation_matrix: translation matrix from measurement coordinate system to desired coordinate system
See: https://dsp.stackexchange.com/a/27488
Most likely, this will be an NxN eye matrix the where N is the number of variables
being estimated
:param n_iter: Number of times to repeat the parameter estimation function (estimates noise)
"""
init_state = np.array(init_state)
transition_matrix = np.array(transition_matrix)
observation_matrix = np.array(observation_matrix)
self._expected_shape = init_state.shape
self._filter = KalmanFilter(
transition_matrices=transition_matrix,
observation_matrices=observation_matrix,
initial_state_mean=init_state,
)
self._calibration_countdown = 0
self._calibration_observations = []
self._em_iter = n_iter
self.calibrate(train_size, n_iter)
self._x_now = np.zeros(3)
self._p_now = np.zeros(3)
def KalmanSequence(size, a, rand):
# X_{t+1} = a*X_t + noise
##########
# specify parameters
random_state = np.random.RandomState(rand)
transition_matrix = [[a]]
transition_offset = [0]
observation_matrix = np.eye(1) #+ random_state.randn(2, 2) * 0.1
observation_offset = [0]
transition_covariance = np.eye(1)
observation_covariance = np.eye(1) #+ random_state.randn(2, 2) * 0.1
initial_state_mean = [0]
initial_state_covariance = [[1]]
# for 2-dim
# random_state = np.random.RandomState(0)
# transition_matrix = [[a, 0], [0, a]]
# transition_offset = [0, 0]
# observation_matrix = np.eye(2) #+ random_state.randn(2, 2) * 0.1
# observation_offset = [0, 0]
# transition_covariance = np.eye(2)
# observation_covariance = np.eye(2) #+ random_state.randn(2, 2) * 0.1
# initial_state_mean = [5, -5]
# initial_state_covariance = [[1, 0], [0, 1]]
# 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
)
states, observations = kf.sample(n_timesteps=size, initial_state=initial_state_mean)
# estimate state with filtering and smoothing
filtered_state_estimates = kf.filter(states)[0]
# filtered_state_estimates = kf.filter(observations)[0]
# smoothed_state_estimates = kf.smooth(observations)[0]
return states, observations, filtered_state_estimates
def checkMLDS(T, A, C, Q, S):
"""
Quick sanity check to test multivariate LDS
Generate Dx latent dynamics and fit Dy observation
"""
Dx = A.shape[0]
Dy = C.shape[0]
obs = np.zeros([T, Dy])
hid = np.zeros([T, Dx])
for i in range(1, T):
hid[i] = np.dot(A, hid[i - 1]) + np.dot(Q, np.random.randn(Dx))
obs[i] = np.dot(C, hid[i]) + np.dot(S, np.random.randn(Dy))
plt.figure()
plt.title("Simulated Data")
plt.plot(hid, c='blue')
plt.plot(obs, c='red')
plt.legend(['hidden', 'observed'])
plt.show()
mykf = KalmanFilter(initial_state_mean=np.zeros(Dx),
n_dim_state=Dx,
n_dim_obs=Dy,
em_vars=[
'transition_matrices', 'observation_matrices',
'transition_covariance', 'observation_covariance'
])
mykf.em(obs, n_iter=100)
plt.figure()
myZ, mySig = mykf.smooth(obs)
plt.title("Estimated States vs Ground Truth")
plt.plot(myZ, c='red')
plt.plot(hid, c='blue')
plt.legend(['smoothed', 'true'])
plt.show()
return mykf.transition_matrices, mykf.observation_matrices, mykf.transition_covariance, mykf.observation_covariance
def next(obj, field):
objvals = self._message_df[self._message_df['object_id'] == obj][field]
if len(self._message_df[self._message_df['object_id'] == obj]) < 2:
self._result = message.get(field, None)
else:
self.initial_state_mean = objvals.iat[0]
self.kf = KalmanFilter(
transition_matrices=self.F,
transition_covariance=self.Q,
observation_matrices=self.H,
observation_covariance=self.R,
initial_state_mean=self.initial_state_mean,
initial_state_covariance=self.P)
state_means, _ = self.kf.filter(objvals.values)
state_means = pd.Series(state_means.flatten())
self._result = state_means.iloc[-1]
if len(objvals) < 10:
pass
else:
self._message_df[self._message_df['object_id'] == obj].drop(
self._message_df[self._message_df['object_id'] == obj].head(1).index)
return self._result
