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]
class AngleKalman:
def __init__(self, start_angle, time_delta):
angle_stdev = 0.05
A = numpy.asarray([[1, time_delta, 0.5 * time_delta * time_delta],
[0, 1, time_delta], [0, 0, 1]])
H = numpy.asarray([[1, 0, 0]])
r = angle_stdev
r2 = r * r
R = numpy.asarray([
[r],
])
Q = numpy.asarray([[r2, 0, 0], [0, r2, 0], [0, 0, r2]])
initial_state = numpy.asarray([start_angle, 0, 0])
self._kf = KalmanFilter(transition_matrices=A,
observation_matrices=H,
transition_covariance=Q,
observation_covariance=R,
initial_state_mean=initial_state)
self._means = initial_state
self._covs = numpy.zeros((3, 3))
self._last_a = start_angle
def step(self, a):
diff = abs(a - self._last_a)
if diff > math.pi:
corr = math.copysign(2 * math.pi, a)
self._means[0] += corr
self._last_a += corr
(self._means, self._covs) = \
self._kf.filter_update(self._means, self._covs, observation=numpy.asarray([a]))
self._last_a = a
def missing_step(self):
(self._means, self._covs) = \
self._kf.filter_update(self._means, self._covs, observation=None)
def get_means(self):
(a, o, e) = self._means
new_a = (a + math.pi) % (2 * math.pi) - math.pi
return float(new_a), float(o), float(e)
class _KalmanFilter_(object):
"""Kalman Filter"""
def __init__(self, xinit, yinit):
self.Transition_Matrix = [[1, 0, 1, 0, 0.5, 0], [0, 1, 0, 1, 0, 0.5],
[0, 0, 1, 0, 1, 0], [0, 0, 0, 1, 0, 1],
[0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]]
self.Observation_Matrix = [[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]]
self.xinit = xinit
self.yinit = yinit
self.vxinit = 0.0
self.vyinit = 0.0
self.axinit = 0.0
self.ayinit = 0.0
self.initstate = [
self.xinit, self.yinit, self.vxinit, self.vyinit, self.axinit,
self.ayinit
]
self.initcovariance = 1.0e-3 * np.eye(6)
self.transistionCov = 1.0e-4 * np.eye(6)
self.observationCov = 1.0e-1 * np.eye(2)
self.mean = None
self.covariance = None
self.kf = KalmanFilter(transition_matrices=self.Transition_Matrix,
observation_matrices=self.Observation_Matrix,
initial_state_mean=self.initstate,
initial_state_covariance=self.initcovariance,
transition_covariance=self.transistionCov,
observation_covariance=self.observationCov)
def _filterUpdate_(self, new_x_measurement, new_y_meansurement):
if (self.mean is None) or (self.covariance is None):
self.mean, self.covariance = self.kf.filter_update(
self.initstate, self.initcovariance,
[new_x_measurement, new_y_meansurement])
else:
self.mean, self.covariance = self.kf.filter_update(
self.mean, self.covariance,
[new_x_measurement, new_y_meansurement])
return self.mean[0], self.mean[1]
class KalmanSensorNode(object):
def __init__(self, node_id, state_dims, process, fusion_algorithm,
transition_matrix, transition_covariance,
measurement_covariance):
self.node_id = node_id
self.state_dims = state_dims
self.measurement_covariance = measurement_covariance
self.mean = np.zeros(shape=(state_dims, ))
self.cov = np.identity(state_dims)
self.kf = KalmanFilter(transition_matrices=transition_matrix,
transition_covariance=transition_covariance,
observation_covariance=measurement_covariance)
self.process = process
self.fusion_algorithm = fusion_algorithm
self.local_estimates = []
self.fused_estimates = []
def estimate(self):
measurement_noise = np.random.multivariate_normal(
[0 for dim in range(self.state_dims)], self.measurement_covariance)
noisy_measurement = np.squeeze(np.asarray(
self.process.current_state)) + measurement_noise
self.mean, self.cov = self.kf.filter_update(self.mean, self.cov,
noisy_measurement)
self.local_estimates.append((self.mean, self.cov))
return self.mean, self.cov
def fuse_in(self, mean, cov):
self.mean, self.cov = self.fusion_algorithm.fuse(
self.mean, self.cov, mean, cov)
self.fused_estimates.append((self.mean, self.cov))
return self.mean, self.cov
class OurFilter():
def __init__(self, newX, prevX, newCaptureTime, prevCaptureTime):
_, Q = getTransitionMats(dtForInitCov)
dt = (newCaptureTime - prevCaptureTime)
if dt > nominalDt * 3:
print("dt was huge", dt)
#pdb.set_trace()
initV = (newX - prevX) / dt
self.prevStateMean = np.array([newX, initV])
self.prevStateCovariance = Q
self.prevStateTime = newCaptureTime
self.filter = KalmanFilter()
def predict(self, predTime):
A, Q = getTransitionMats(predTime - self.prevStateTime)
return A @ self.prevStateMean, A @ self.prevStateCovariance @ A.T + Q
def update(self, measX, measTime):
dt = measTime - self.prevStateTime
A, Q = getTransitionMats(dt)
self.prevStateMean, self.prevStateCovariance = self.filter.filter_update( \
self.prevStateMean,
self.prevStateCovariance,
observation = measX,
transition_matrix = A,
transition_covariance = Q,
observation_matrix = np.array([[1,0]]),
observation_covariance=np.array([[measurementVariance]])
)
self.prevStateTime = measTime
class PointMassTracker(object):
def __init__(self, frequency, transition_variances, observation_variances, init_pose, init_variances, change_thresholds):
delta_t = 1.0 / frequency
transition_mat = np.array([[1, 0, delta_t, 0],
[0, 1, 0, delta_t],
[0, 0, 1, 0],
[0, 0, 0, 1]])
transition_cov = np.array([[transition_variances[0], 0, 0, 0],
[0, transition_variances[1], 0, 0],
[0, 0, transition_variances[2], 0],
[0, 0, 0, transition_variances[3]]])
observation_mat = np.array([[1, 0, 0, 0],
[0, 1, 0, 0]])
observation_cov = np.array([[observation_variances[0], 0],
[0, observation_variances[1]]])
init_state = np.array([init_pose[0], init_pose[1], 0, 0])
init_cov = np.array([[init_variances[0], 0, 0, 0],
[0, init_variances[1], 0, 0],
[0, 0, init_variances[2], 0],
[0, 0, 0, init_variances[3]]])
self.change_thresholds = change_thresholds
self.kf = KalmanFilter(transition_matrices=transition_mat, observation_matrices=observation_mat, transition_covariance=transition_cov,
observation_covariance=observation_cov, initial_state_mean=init_state, initial_state_covariance=init_cov)
self.filtered_state_mean = init_state
self.filtered_state_cov = init_cov
def update_and_get_estimation(self, measurement):
if measurement is not None:
if abs(self.filtered_state_mean[0] - measurement[0]) > self.change_thresholds[0] or \
abs(self.filtered_state_mean[1] - measurement[1]) > self.change_thresholds[1]:
measurement = None
self.filtered_state_mean, self.filtered_state_cov = self.kf.filter_update(self.filtered_state_mean, self.filtered_state_cov, measurement)
return self.filtered_state_mean
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)
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 fitKCA(t,z,q,fwd=0):
#
#Inputs:
# t: Iterable with time indices
# z: Iterable with measurements
# q: Scalar that multiplies the seed states covariance
# fwd: number of steps to forecast (optional, default=0)
#Output:
#x[0]: smoothed state means of position velocity and acceleration
#x[1]: smoothed state covar of position velocity and acceleration
#Dependencies: numpy, pykalman
#
#1) Set up matrices A,H and a seed for Q
h=(t[-1]-t[0])/t.shape[0]
A=np.array([[1,h,.5*h**2],
[0,1,h],
[0,0,1]])
Q=q*np.eye(A.shape[0])
#2) Apply the filter
kf=KalmanFilter(transition_matrices=A,transition_covariance=Q)
kf=kf.em(z)
#4) Smooth
x_mean,x_covar=kf.smooth(z)
#5) Forecast
for fwd_ in range(fwd):
x_mean_,x_covar_=kf.filter_update(filtered_state_mean=x_mean[-1], filtered_state_covariance=x_covar[-1])
x_mean=np.append(x_mean,x_mean_.reshape(1,-1),axis=0)
x_covar_=np.expand_dims(x_covar_,axis=0)
x_covar=np.append(x_covar,x_covar_,axis=0)
#6) Std series
x_std=(x_covar[:,0,0]**.5).reshape(-1,1)
for i in range(1,x_covar.shape[1]):
x_std_=x_covar[:,i,i]**.5
x_std=np.append(x_std,x_std_.reshape(-1,1),axis=1)
return x_mean,x_std,x_covar
def kalman_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)
class KalmanRegression(object):
def __init__(self, x_state_mean, y_state_mean, delta=1e-5):
trans_cov = delta / (1 - delta) * np.eye(2)
obs_mat = np.expand_dims(np.vstack([[x_state_mean],
[np.ones(len(x_state_mean))]]).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)
means, cov = self.kf.filter(y_state_mean)
self.mean = means[-1]
self.cov = cov[-1]
def update(self, x, y):
self.mean, self.cov = self.kf.filter_update(
self.mean, self.cov, y, observation_matrix=np.array([[x, 1.0]]))
@property
def state_mean(self):
return self.mean
class KalmanMovingAverage(object):
def __init__(self,
asset,
observation_covariance=1.0,
initial_value=0,
initial_state_covariance=1.0,
transition_covariance=0.05):
self.asset = asset
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 = [self.kf.initial_state_mean]
self.state_vars = [self.kf.initial_state_covariance]
def update_all(self, observations):
for observation in observations:
self.update(observation)
def update(self, observation):
mu, cov = self.kf.filter_update(self.state_means[-1],
self.state_vars[-1], observation)
self.state_means.append(mu.flatten()[0])
self.state_vars.append(cov.flatten()[0])
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]]),
)
class anglefilter(object):
def __init__(self):
self.A_a = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
self.C_a = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
self.Q_a = np.asarray([[1e-3, 1e-4, 1e-4], [1e-4, 1e-2, 1e-4],
[1e-4, 1e-4, 1e-2]]) / 50
self.R_a = np.asarray([[0.1, 1e-3, 1e-3], [1e-3, 0.1, 1e-3],
[1e-3, 1e-3, 0.1]]) / 50
self.state_mean = np.zeros(3)
self.state_covariance = self.Q_a
self.filter = KalmanFilter(transition_matrices=self.A_a,
observation_matrices=self.C_a,
transition_covariance=self.Q_a,
observation_covariance=self.R_a,
initial_state_mean=np.asarray([0, 0, 0]))
def update(self, angle_list):
measurement = np.asarray(angle_list[:3])
self.state_mean, self.state_covariance = self.filter.filter_update(
filtered_state_mean=self.state_mean,
filtered_state_covariance=self.state_covariance,
observation=measurement)
new_spot_list = [
self.state_mean[0], self.state_mean[1], self.state_mean[2]
] + angle_list[3:]
return new_spot_list
def kf_measure_with_intercept():
initial_state_mean = 0.8
initial_state_covariance = 1
# 构建一个卡尔曼滤波器
kf = KalmanFilter(transition_matrices=np.eye(2),
observation_matrices=[0.01, 1],
initial_state_mean=[initial_state_mean, 0],
initial_state_covariance=np.ones((2, 2)),
observation_covariance=1,
transition_covariance=np.eye(2) * 0.0001)
n_timesteps, n_dim_state, n_dim_obs = len(df), 2, 1
filtered_state_means = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances = np.zeros(
(n_timesteps, n_dim_state, n_dim_state))
filtered_state_means[0] = initial_state_mean
filtered_state_covariances[0] = initial_state_covariance
for i in tqdm(range(n_timesteps - 1)):
filtered_state_means[i + 1], filtered_state_covariances[
i + 1] = kf.filter_update(filtered_state_means[i],
filtered_state_covariances[i],
df.ic[i],
observation_matrix=np.array(
[[df.ih[i], 1]]))
def testKalmanFilter(self, data, data2):
#d2 = [math.degrees(r) for r d2in ]
xCorr = self.getCorrelation(data[:, 0], data2[:, 4])
#self.plotTwoSets(data, data2, m)
print(xCorr)
w, h = 1920, 1080
mns = [0, 0, 700, 0, 0, 0]
cs = 6 * [[192, 107, 100, math.pi, math.pi, math.pi]]
#print(cs)
#print(mns.shape, cs.shape)
kf = KalmanFilter(initial_state_mean=mns, initial_state_covariance=cs)
#m, v = kf.filter(data2)
#print(m)
m = np.zeros_like(data2)
mf = [[0], [0], [700], [0], [0], [0]]
cf = 6 * [[
192, 107, 100, math.pi / 1800, math.pi / 180000, math.pi / 1800
]]
for i, y in enumerate(data2):
mf, cf = kf.filter_update(mf, cf, y)
m[i] = mf[0]
print(m.shape)
data = data[:, 0]
data2 = [math.degrees(r) for r in data2[:, 4]]
m = [math.degrees(r) for r in m[:, 4]]
xCorr = self.getCorrelation(data, m)
print(xCorr)
self.plotTwoSets(data2, m)
class AccelerometerReader(Reader):
def __init__(self, type, model):
super(AccelerometerReader, self).__init__(type, model)
self.last_point = None
self.points = []
self.mag_threshold2 = pow(config.ACCEL_MAG_THRESHOLD, 2)
self.kf = KalmanFilter(initial_state_mean=0, n_dim_obs=1)
self.point = None
self.covariance = [ [0.0], [0.0], [0.0] ]
def _handle_point(self, raw_point):
if not self.point:
self.point = raw_point
if config.ACCEL_FILTER:
(self.point[0], self.covariance[0]) = self.kf.filter_update(self.point[0], self.covariance[0], raw_point[0])
(self.point[1], self.covariance[1]) = self.kf.filter_update(self.point[1], self.covariance[1], raw_point[1])
(self.point[2], self.covariance[2]) = self.kf.filter_update(self.point[2], self.covariance[2], raw_point[2])
point = [self.point[0][0].data[0], self.point[1][0].data[0], self.point[2][0].data[0]]
else:
point = raw_point
diff = ( abs(self.last_point[0] - point[0]), abs(self.last_point[1] - point[1]), abs(self.last_point[2] - point[2]) )
mag2 = pow(diff[0], 2) + pow(diff[1], 2) + pow(diff[2], 2)
s = sqrt(mag2)
# print mag2, self.mag_threshold2
if mag2 > self.mag_threshold2:
self._save_data("sleep", { 'mag' : sqrt(mag2) - config.ACCEL_MAG_THRESHOLD})
self.last_point = point
def read(self):
pt = list(self.sensor.read())
if not self.last_point:
self.last_point = pt
return
self._handle_point(pt)
class KLF(object):
def __init__(self):
self._trans_mat = np.eye(6)
self._trans_conv = scipy.linalg.block_diag(
np.eye(3) * 0.05,
np.eye(3) * 0.2)
self._trans_conv[2, 2] = 0.0872665
self._trans_conv[5, 5] = 0.349066
self._observation_mat = np.block([np.eye(3), np.eye(3) * 0])
self._observation_conv = np.eye(3) * 0.1
self._observation_conv[2, 2] = 0.0872665
self._filter = None
#self._initialize()
def _initialize(self, cur_state):
#reinitialize kalman filter
self._filter = KalmanFilter(
observation_matrices=self._observation_mat,
transition_covariance=self._trans_conv,
observation_covariance=self._observation_conv,
transition_offsets=np.array([0, 0, 0, 0, 0, 0]),
observation_offsets=np.array([0, 0, 0]),
n_dim_state=6,
n_dim_obs=3)
self._filtered_state = np.hstack([cur_state, [0, 0, 0]])
self._filtered_covariance = np.eye(6) * 0.1
def filter(self, cur_state, dt):
#first time, we just initialize stuff
if (self._filter is None):
self._initialize(cur_state)
return cur_state
#if dt is certain time, restart filter
if (dt > 1.25):
#reinitialize
print("KLF timed out, dt{}".format(dt))
self._initialize(cur_state)
return cur_state
#do kalman update
self._trans_mat[:3, 3:] = np.eye(3) * dt
#update
nxt_state, nxt_conv = self._filter.filter_update(
self._filtered_state,
self._filtered_covariance,
cur_state,
transition_matrix=self._trans_mat)
self._filtered_state = nxt_state
self._filtered_covariance = nxt_conv
return nxt_state
def kalmanFilterPositionChange(initial_state_mean, initial_state_covariance,
initial_AccX_Value, new_AccX_Value):
AccX_Value = np.array([initial_AccX_Value, new_AccX_Value])
AccX_Variance = 0.0007
# time step
dt = 1
# transition_matrix
F = [[1, dt, 0.5 * dt**2], [0, 1, dt], [0, 0, 1]]
# observation_matrix
H = [0, 0, 1]
# transition_covariance
Q = [[0.2, 0, 0], [0, 0.1, 0], [0, 0, 10e-4]]
# observation_covariance
R = AccX_Variance
# initial_state_mean
X0 = [0, 0, AccX_Value[0]]
# initial_state_covariance
P0 = [[0, 0, 0], [0, 0, 0], [0, 0, AccX_Variance]]
n_timesteps = AccX_Value.shape[0]
n_dim_state = 3
filtered_state_means = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances = np.zeros(
(n_timesteps, n_dim_state, n_dim_state))
kf = KalmanFilter(transition_matrices=F,
observation_matrices=H,
transition_covariance=Q,
observation_covariance=R,
initial_state_mean=X0,
initial_state_covariance=P0)
# iterative estimation for the new measurement
for t in range(n_timesteps):
if t == 0:
filtered_state_means[t] = initial_state_mean
filtered_state_covariances[t] = initial_state_covariance
else:
filtered_state_means[t], filtered_state_covariances[t] = (
kf.filter_update(filtered_state_means[t - 1],
filtered_state_covariances[t - 1],
AccX_Value[t]))
return (filtered_state_means[1], filtered_state_covariances[1],
new_AccX_Value,
filtered_state_means[1, 0] - filtered_state_means[0, 0])
def test_online_update(self):
kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]])
measurements = np.asarray([[1,0], [0,0], [0,1]]) # 3 observations
measurements = np.ma.asarray(measurements)
measurements[1] = np.ma.masked # measurement at timestep 1 is unobserved
kf = kf.em(measurements, n_iter=5)
(filtered_state_means, filtered_state_covariances) = kf.filter(measurements)
for t in range(1, 3):
filtered_state_means[t], filtered_state_covariances[t] = \
kf.filter_update(filtered_state_means[t-1], filtered_state_covariances[t-1], measurements[t])
return filtered_state_means
def missingDataKalman(X, t):
# Filter Configuration
X = ma.where(X == -1, ma.masked, X)
# t step
dt = t[2] - t[1]
# transition_matrix
F = [[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]]
# observation_matrix
H = [1, 0, 0]
# transition_covariance
Q = [[1, 0, 0], [0, 1e-4, 0], [0, 0, 1e-6]]
# observation_covariance
R = [0.01] # max error = 0.6m
# initial_state_mean
if (X[0] == ma.masked):
X0 = [0, 0, 0]
else:
X0 = [X[0], 0, 0]
# initial_state_covariance
P0 = [[10, 0, 0], [0, 1, 0], [0, 0, 1]]
n_tsteps = len(t)
n_dim_state = 3
filtered_state_means = np.zeros((n_tsteps, n_dim_state))
filtered_state_covariances = np.zeros((n_tsteps, n_dim_state, n_dim_state))
# Kalman-Filter initialization
kf = KalmanFilter(transition_matrices=F,
observation_matrices=H,
transition_covariance=Q,
observation_covariance=R,
initial_state_mean=X0,
initial_state_covariance=P0)
# iterative estimation for each new measurement
for t in range(n_tsteps):
if t == 0:
filtered_state_means[t] = X0
filtered_state_covariances[t] = P0
else:
filtered_state_means[t], filtered_state_covariances[t] = (
kf.filter_update(filtered_state_means[t - 1],
filtered_state_covariances[t - 1],
observation=X[t]))
return filtered_state_means
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 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 kalman_estimation(self, array, index):
dt = self.time_step(index)
X = array.copy()
n_timesteps = len(index)
n_dim_state = 3
# transition_matrix
F = [[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]]
# observation_matrix
H = [1, 0, 0]
# transition_covariance
Q = [[1, 0, 0], [0, 1e-4, 0], [0, 0, 1e-6]]
# observation_covariance
R = [0.04]
# initial_state_mean
X0 = [0, 0, 0]
# initial_state_covariance
P0 = [[10, 0, 0], [0, 1, 0], [0, 0, 1]]
self.filtered_state_means = np.zeros((n_timesteps, n_dim_state))
self.filtered_state_covariances = np.zeros(
(n_timesteps, n_dim_state, n_dim_state))
# Kalman-Filter initialization
kf = KalmanFilter(transition_matrices=F,
observation_matrices=H,
transition_covariance=Q,
observation_covariance=R,
initial_state_mean=X0,
initial_state_covariance=P0)
# iterative estimation for each new measurement
for t in range(n_timesteps):
if t == 0:
self.filtered_state_means[t] = X0
self.filtered_state_covariances[t] = P0
else:
self.filtered_state_means[t], self.filtered_state_covariances[
t] = (kf.filter_update(self.filtered_state_means[t - 1],
self.filtered_state_covariances[t -
1],
observation=X[t]))
filtered_values = self.filtered_state_means[:, 0]
sigma = np.sqrt(self.filtered_state_covariances[:, 0, 0])
return filtered_values, sigma
def kf_predict(meassurements):
global i
newMeasurement = np.ma.asarray(-1)
initial_state_mean = [measurements[0, 0], 0, measurements[0, 1], 0]
#transition_matrix = [[1, 0, 0.4, 0],[1, 0, 0.1, 0],[0, 0.2, 1, 1],[0.2, 0.5, 0.3, 1]] #TWEAK THIS BASED ON EXPERIMENTATION
transition_matrix = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]
observation_matrix = [[1, 0, 0, 0], [0, 1, 0, 0]]
kf1 = KalmanFilter(transition_matrices=transition_matrix,
observation_matrices=observation_matrix,
initial_state_mean=initial_state_mean)
kf1 = kf1.em(measurements, n_iter=5)
(smoothed_state_means,
smoothed_state_covariances) = kf1.smooth(measurements)
#PREDICTION
x_now = smoothed_state_means[-1, :]
P_now = smoothed_state_covariances[-1, :]
#newMeasurement = np.ma.asarray(measurements[measurements.shape[0]-1])
#print "++++++++"
#print x_now
#print P_now
#print "+++++++++"
(x_now, P_now) = kf1.filter_update(filtered_state_mean=x_now,
filtered_state_covariance=P_now,
observation=None)
#PLOTS - Comment out later
plt.figure(i)
i = i + 1
plt.hold(True)
times = range(measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo', times, measurements[:, 1], 'ro',
times, smoothed_state_means[:, 0], 'b--', times,
smoothed_state_means[:, 2], 'r--')
#print np.array(smoothed_state_means.shape)
#print x_now[0]
#print x_now[1]
#print np.array(times).shape[0] + 1
plt.plot([np.array(times).shape[0] + 1], x_now[0], 'xb',
[np.array(times).shape[0] + 1], x_now[1], 'xr')
return (x_now)
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]
class LocationKalman:
def __init__(self, start_x, start_y, time_delta):
position_stdev = 0.05
A = numpy.asarray([[1, 0, time_delta, 0], [0, 1, 0, time_delta],
[0, 0, 1, 0], [0, 0, 0, 1]])
H = numpy.asarray([[1, 0, 0, 0], [0, 1, 0, 0]])
r = position_stdev
r2 = r * r
R = numpy.asarray([[r, 0], [0, r]])
Q = numpy.asarray([[r2, 0, 0, 0], [0, r2, 0, 0], [0, 0, r2, 0],
[0, 0, 0, r2]])
initial_state = numpy.asarray([start_x, start_y, 0, 0])
self._kf = KalmanFilter(transition_matrices=A,
observation_matrices=H,
transition_covariance=Q,
observation_covariance=R,
initial_state_mean=initial_state)
self._means = initial_state
self._covs = numpy.zeros((4, 4))
def step(self, x, y):
(self._means, self._covs) = \
self._kf.filter_update(self._means, self._covs, observation=numpy.asarray([x, y]))
def missing_step(self):
(self._means, self._covs) = \
self._kf.filter_update(self._means, self._covs, observation=None)
def get_means(self):
return tuple(self._means)
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 applyKalmanFilter(accelData):
# Time = [i * 0.015 for i in range(len(accelData))]
# time step
dt = 0.015
# transition_matrix
F = [[1, dt, 0.5 * dt ** 2], [0, 1, dt], [0, 0, 1]]
# observation_matrix
H = [0, 0, 1]
# transition_covariance
Q = [[0.5, 0, 0], [0, 0.5, 0], [0, 0, 0.5]]
# observation_covariance
R = np.var(accelData)
# initial_state_mean
X0 = [0, 0, 0]
# initial_state_covariance
P0 = [[0, 0, 0], [0, 0, 0], [0, 0, R]]
n_timesteps = len(accelData)
n_dim_state = 3
filtered_state_means = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances = np.zeros((n_timesteps, n_dim_state, n_dim_state))
kf = KalmanFilter(
transition_matrices=F,
observation_matrices=H,
transition_covariance=Q,
observation_covariance=R,
initial_state_mean=X0,
initial_state_covariance=P0,
)
# iterative estimation for each new measurement
for t in range(n_timesteps):
if t == 0:
filtered_state_means[t] = X0
filtered_state_covariances[t] = P0
else:
filtered_state_means[t], filtered_state_covariances[t] = kf.filter_update(
filtered_state_means[t - 1],
filtered_state_covariances[t - 1],
accelData[t],
)
return filtered_state_means[:, 2]
class Filter(object):
def __init__(self, first_obs):
self.transition_covariance = 0.8
self.observation_covariance = 0.8
self.state = first_obs
self.measurements = [first_obs]
self.covariance = None
self.transition_matrices = [0.8]
self.observation_matrices = [0.8]
self.initial_state_mean = [first_obs]
self.kf = KalmanFilter(
# self.transition_matrices,
# observation_matrices=self.observation_matrices,
initial_state_mean=0,
n_dim_state=1,
# self.transition_covariance,
observation_covariance=self.observation_covariance)
# self.state, self.covariance = self.kf.filter(self.measurements)
# self.state = self.state[0]
# self.covariance = self.covariance[0][0]
def update(self, rssi):
# self.state = self.kf.filter([rssi])[0][0][0]
self.measurements = self.measurements[-5:]
self.measurements.append(rssi)
means, covariances = self.kf.filter(self.measurements)
self.state, self.covariance = self.kf.filter_update(
means[-1], covariances[-1], [rssi])
self.state = self.state[0]
self.covariance = self.covariance[0][0]
# def update(self, rssi):
# self.state, self.covariance = self.kf.filter_update(
# [self.state], [self.covariance], [rssi])
# try:
# self.state = self.state[0][0]
# except:
# self.state = self.state[0]
# self.covariance = self.covariance[0][0]
def transition_function(self):
return
def observation_functions(self):
return
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 lambda_handler(event, context):
# Log the received event
# implement me!
try:
variance = get_variance( event )
if (PREVIOUS_STATE_KEY in event):
prev = event[PREVIOUS_STATE_KEY];
prevMean = np.array([ prev[LATITUDE_KEY], prev[LONGITUDE_KEY]])
prevCovar = np.multiply( prev[VARIANCE_KEY], COVARIANCE_MATRIX )
kf = KalmanFilter(
initial_state_mean=prevMean,
initial_state_covariance=prevCovar,
em_vars=['transition_covariance', 'observation_covariance'],
transition_matrices=TRANSITION_MATRIX)
curObs = np.array([ event[LATITUDE_KEY], event[LONGITUDE_KEY]]);
curCovar = np.multiply( event[VARIANCE_KEY], np.eye( len(curObs)))
nextMean, nextCovar = kf.filter_update(
prevMean,
prevCovar,
observation_matrix=OBSERVATION_MATRIX,
observation=curObs,
observation_covariance=curCovar)
return {
LATITUDE_KEY: nextMean[0],
LONGITUDE_KEY: nextMean[1],
VARIANCE_KEY: nextCovar[0][0],
}
else:
# if first point in sequence...
# just return the single location point, with corresponding accuracy
return {
LATITUDE_KEY: event[LATITUDE_KEY],
LONGITUDE_KEY: event[LONGITUDE_KEY],
VARIANCE_KEY: variance
}
except Exception as e:
print(e)
return { 'message': str(e) }
class KalmanSmoother:
def __init__(self, orice):
self.kf = KalmanFilter(transition_matrices=np.matrix('''
1. 0.;
0. 1.
'''), observation_matrices=np.matrix('''
1. 0.;
0. 1.
'''))
self.filtered_state_means = np.zeros((2))
self.filtered_state_covariances = np.matrix(np.eye(2))*1000
def get_distance_estimation(self, observation):
self.filtered_state_means, self.filtered_state_covariances = self.kf.filter_update(self.filtered_state_means, self.filtered_state_covariances, observation)
return self.filtered_state_means[0]
class StateEstimationAltitude2():
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 update(self, dt, observations):
if not self.previous_update:
self.previous_update = time.time()
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],
[0, 1],
]),
# observation_offset = np.array([, 0, 0])
# o1bservation_covariance=np.array(0.1*np.eye(1))
))
self.previous_update = time.time()
def getAltitude(self):
return self.state[0]
class KalmanMovingAverage(object):
"""
Estimates the moving average of a price process
via Kalman Filtering.
See http://pykalman.github.io/ for docs on the
filtering process.
"""
def __init__(self,
asset,
observation_covariance=1.0,
initial_value=0,
initial_state_covariance=1.0,
transition_covariance=0.05,
initial_window=20,
maxlen=3000,
freq='1d'):
self.asset = asset
self.freq = freq
self.initial_window = initial_window
self.maxlen = maxlen
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_vars = pd.Series([self.kf.initial_state_covariance],
name=self.asset)
def update(self, observations):
for dt, observation in observations[self.asset].iteritems():
self._update(dt, observation)
def _update(self, dt, observation):
mu, cov = self.kf.filter_update(self.state_means.iloc[-1],
self.state_vars.iloc[-1], observation)
self.state_means[dt] = mu.flatten()[0]
self.state_vars[dt] = cov.flatten()[0]
if self.state_means.shape[0] > self.maxlen:
self.state_means = self.state_means.iloc[-self.maxlen:]
if self.state_vars.shape[0] > self.maxlen:
self.state_vars = self.state_vars.iloc[-self.maxlen:]
class LightSensorReader(Reader):
def __init__(self, type, model):
super(LightSensorReader, self).__init__(type, model)
self.kf = KalmanFilter(initial_state_mean=0, n_dim_obs=1)
self.point = None
self.covariance = [0.0]
def read(self):
raw_point = [float(self.sensor.read())]
if not self.point:
self.point = raw_point
(self.point,
self.covariance) = self.kf.filter_update(self.point, self.covariance,
raw_point)
self._save_data("light", {'light': int(self.point[0])})
class Regression(object):
"""
y = beta * x + alpha
"""
def __init__(self, initial_x, initial_y, date, delta=1e-5):
trans_cov = delta / (1 - delta) * np.eye(2)
xList = []
for x in initial_x:
xList.append([[x,1.]])
obs_mat = np.vstack([xList])
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(np.array(initial_y))
self.means = pd.DataFrame(state_means,
columns=['beta', 'alpha'])
self.state_cov = state_covs[-1]
def update(self, observations, date):
x = observations[0]
y = observations[1]
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=date)
self.means = self.means.append(mu)
def get_spread(self, observations):
x = observations[0]
y = observations[1]
return y - (self.means.beta * x + self.means.alpha)
@property
def state_mean(self):
return self.means.iloc[-1]
def kalman_filtering(price_sequence):
h = 1 #time step
A = np.array([[1,h,.5*h**2],
[0,1,h],
[0,0,1]])
Q = np.eye(A.shape[0])
#2) Apply the filter
kf = KalmanFilter(transition_matrices = A , transition_covariance = Q)
means, covariances = kf.filter([price_sequence[0]])
filtered_price_sequence = [means[0,0]]
for i in range(1,len(price_sequence)):
#to track it (streaming)
new_mean, new_covariance = kf.filter_update(means[-1], covariances[-1], price_sequence[i])
means = np.vstack([means,new_mean])
covariances = np.vstack([covariances,new_covariance.reshape((1,3,3))])
filtered_price_sequence.append(means[i,0])
return filtered_price_sequence
def kalman_track(tser):
#get the tracking segments
segments = trigger_and_hold(tser,5,10)
#delta t
ts = 0.0002
#get the first time series slice
sig_slice = tser[:,0]/np.max(tser[:,0])
#the kalman filter to smooth in the angle domain
#array of angle indecies (change to actual dimentions?)
#set up initial conditions
#filter for time series
kf2 = KalmanFilter(transition_matrices=np.array([[1., ts, .5*ts**2, 0., 0, 0, ],
[0., 1., ts, 0., 0, 0, ],
[0., 0., 1., 0., 0, 0, ],
[0., 0., 0., 1., ts, 0.5*ts**2 ],
[0., 0., 0., 0., 1, ts ],
[0., 0., 0., 0., 0 , 1. ]]),
transition_covariance = np.array([[ts**2., 0., 0., 0., 0, 0.],
[0., 1., 0., 0., 0., 0.],
[0., 0, 1/ts**2, 0., 0., 0.],
[0., 0., 0., ts**2., 0., 0.],
[0., 0., 0., 0., 1., 0.],
[0., 0, 0., 0., 0., 1/ts**2]]),
observation_matrices= np.array([[1,0,0,0,0,0],
[0,0,0,1,0,0]]),
observation_covariance = np.array([[1, 0.,],
[0.,1]]))
#how much of the time series to filter - for debugging
testrange = len(segments)-1
#allocate memory
filtered_state_means = np.zeros((testrange+1,6))
filtered_state_cov = np.zeros((testrange+1,6,6))
#x = arange(len(sig_slice),dtype = float)
#kf1 = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]),observation_covariance = [[100000.0]])
#sm_slice = kf1.smooth(sig_slice)[0][:,0]
#p = [p_m1, p_m2, sd1, sd2, sc1, sc2]
#plsq = leastsq(res, p, args = (sm_slice/np.sum(sm_slice), x))
filtered_state_means[0,:] = [m1,0,0,m2,0,0]
filtered_state_cov[0,:] = np.zeros((6,6))
#get the valid tracking segments
#itteratively track
for t in range(testrange):
print t
if segments[t]:
sig_slice = tser[:,t]/np.max(tser[:,t])
observation = observe(sig_slice,filtered_state_means[t,0],filtered_state_means[t,3])
filtered_state_means[t+1,:],filtered_state_cov[t+1,:] = kf2.filter_update(filtered_state_means[t,:],
filtered_state_cov[t,:,:],
observation)
else:
filtered_state_means[t+1,:] = [m1,0,0,m2,0,0]
filtered_state_cov[t+1,:] = filtered_state_cov[t,:]
return filtered_state_means
def kf_metric_plot(metric):
assert isinstance(metric, Metric), "Not a metric: {0!s}".format(metric)
assert hasattr(metric, 'data'), "No Data"
import numpy as np
from bounos import DataPackage
from pykalman import KalmanFilter
data = DataPackage("/dev/shm/dat-2013-02-01-13-58-48.aietes.npz")
rnd = np.random.RandomState(0)
# generate a noisy sine wave to act as our fake observations
n_timesteps = data.tmax
x = range(0, n_timesteps)
records = metric.data
try:
obs_dim = len(records[0])
except TypeError as e:
obs_dim = 1
observations = np.ma.array(records) # to put it as(tmax,3)
masked = 0
for i in x:
try:
if rnd.normal(2, 2) >= 0:
observations[i] = np.ma.masked
masked += 1
except BaseException as e:
print(i)
raise e
print("{0:f}% Masked".format(((masked * 100.0) / data.tmax)))
print("Records: Shape: {0!s}, ndim: {1!s}, type: {2!s}".format(records.shape, records.ndim, type(records)))
# create a Kalman Filter by hinting at the size of the state and observation
# space. If you already have good guesses for the initial parameters, put them
# in here. The Kalman Filter will try to learn the values of all
# variables.
kf = KalmanFilter(n_dim_obs=obs_dim, n_dim_state=obs_dim)
# You can use the Kalman Filter immediately without fitting, but its estimates
# may not be as good as if you fit first.
# states_pred = kf.em(observations, n_iter=data.tmax).smooth(observations)
# print 'fitted model: %s' % (kf,)
# Plot lines for the observations without noise, the estimated position of the
# target before fitting, and the estimated position after fitting.
fig = plt.figure(figsize=(16, 6))
ax1 = fig.add_subplot(111)
filtered_state_means = np.zeros((n_timesteps, kf.n_dim_state))
filtered_state_covariances = np.zeros(
(n_timesteps, kf.n_dim_state, kf.n_dim_state))
for t in x:
if t == 0:
tmp = np.zeros(kf.n_dim_state)
tmp.fill(500)
filtered_state_means[t] = tmp
print(filtered_state_means[t])
continue
if masked and not observations.mask[t].any():
ax1.axvline(x=t, linestyle='-', color='r', alpha=0.1)
try:
filtered_state_means[t], filtered_state_covariances[t] = \
kf.filter_update(
filtered_state_means[t - 1], filtered_state_covariances[t - 1], observations[t])
except IndexError as e:
print(t)
raise e
(p, v) = (filtered_state_means, filtered_state_covariances)
errors = map(np.linalg.norm, p[:] - records[:])
pred_err = ax1.plot(x, p[:], marker=' ', color='b',
label='predictions-x')
obs_scatter = ax1.plot(x, records, linestyle='-', color='r',
label='observations-x', alpha=0.8)
ax1.set_ylabel(metric.label)
ax2 = ax1.twinx()
error_plt = ax2.plot(
x, errors, linestyle=':', color='g', label="Error Distance")
ax2.set_yscale('log')
ax2.set_ylabel("Error")
lns = pred_err + obs_scatter + error_plt
labs = [l.get_label() for l in lns]
plt.legend(lns, labs, loc='upper right')
plt.xlim(0, 500)
ax1.set_xlabel('time')
fig.suptitle(data.title)
print("Predicted ideal {0!s}: {1!s}".format(metric.label, str(p[-1])))
plt.show()
def main():
#Paramètres du modèle
#Indiquer à quoi correspond chaque variable
osigma=0.1;
transition_matrix = np.array([[1., 0.,0.],[1., 1.,0.],[0.,0,0.9]])
transition_covariance = np.zeros((3,3));
observation_matrix = np.array([[0., 1.,0.],[0., 0.,1.]])
observation_covariance = np.eye(2)*osigma
initial_state_mean = np.array([1,0,10])
initial_state_covariance = np.eye(3);
#Observations
observations=np.array([ [1.1,9.2],
[1.9,8.1],
[2.8,7.2],
[4.2,6.6],
[5.0,5.9],
[6.1,5.32],
[7.2,4.7],
[8.1,4.3],
[9.0,3.9]])
# Filtrage à la main
# Quels sont les paramètres du constructeur ?
kf = KalmanFilter(
transition_matrix, observation_matrix,
transition_covariance, observation_covariance,
)
# Que conserverons les variables suivantes ?
hand_state_estimates=[initial_state_mean]
hand_state_cov_estimates=[initial_state_covariance]
for anObs in observations:
# Quelles étapes du filtrage sont réalisées par la ligne suivante
(aMean,aCov) = kf.filter_update(hand_state_estimates[-1],hand_state_cov_estimates[-1],anObs)
hand_state_estimates.append(aMean)
hand_state_cov_estimates.append(aCov)
hand_state_estimates=np.array(hand_state_estimates)
# A quoi sert la ligne suivante ?
hand_positions=np.dot(hand_state_estimates,observation_matrix.T )
plt.figure(1)
plt.plot(observations[:,0],observations[:,1], 'r+')
plt.plot(hand_positions[:,0],hand_positions[:,1], 'b')
plt.savefig('illustration_filtrage_main.pdf')
plt.close()
# Filtrage complet
# Que fait cette section ?
# Quels sont les paramètres supplémentaires donnés au constructeur ?
# Quels sont les autres paramètres possibles ?
kf = KalmanFilter(
transition_matrix, observation_matrix,
transition_covariance, observation_covariance,
initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance,
)
(filtered_state_estimates,filtered_state_cov_estimates) = kf.filter(observations)
filtered_positions=np.dot(filtered_state_estimates,observation_matrix.T )
plt.figure(1)
plt.plot(observations[:,0],observations[:,1], 'r+')
plt.plot(filtered_positions[:,0],filtered_positions[:,1], 'b')
plt.savefig('illustration_filtrage.pdf')
plt.close()
# Lissage
# Que fait cette section ?
# Quel est la différence avec le filtrage ?
# Puis-je faire un lissage "à la main" observation par observation ?
kf = KalmanFilter(
transition_matrix, observation_matrix,
transition_covariance, observation_covariance,
initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance,
)
(smoothed_state_estimates,smoothed_state_cov_estimates) = kf.smooth(observations)
smoothed_positions=np.dot(smoothed_state_estimates,observation_matrix.T )
plt.figure(2)
plt.plot(observations[:,0],observations[:,1], 'r+')
plt.plot(smoothed_positions[:,0],smoothed_positions[:,1], 'b')
plt.savefig('illustration_lissage.pdf')
plt.close()
class Main:
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
# self.zvelo = [i.value for i in self._zvelo]
# print self.zvelo
# exit()
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]
# print kf.transition_covariance
# print kf.observation_covariance
def update_filter(self, value, control):
dt = 0.10
# if abs(value[0] - self.state[0]) > 10:
# value = Non
control = control / 200
print value
self.state, self.covariance = self.kf.filter_update(
self.state,
self.covariance,
value,
transition_matrix=np.array([[1, dt], [0, 1]]),
observation_matrix=np.array([[1, 0]]),
transition_offset=np.array([control]),
)
self.co.append(self.covariance[0])
self.altitude.append(self.state[0])
def run(self):
self.i = 0
for c in self.throttle:
self.update_filter([self.cam_alt[self.i]], c)
self.i += 1
def draw_fig(self):
pl.figure(dpi=80)
pl.plot(self.altitude, color="b")
pl.plot(self.cam_alt, color="r")
pl.plot(self.baro, color="g")
pl.show()
class KalmanFilterPairsTradingStrategy(StrategyBase):
"""
MeanReversionSpreadStrategy
"""
def __init__(
self, events_engine, data_board
):
super(KalmanFilterPairsTradingStrategy, self).__init__(events_engine, data_board)
self.symbols = ['EWA US Equity', 'EWC US Equity']
self.bollinger_scaler = 1.0
self.state_cov_multiplier = np.power(0.01, 2) # 0.1: spread_std=2.2, cov=16 ==> 0.01: 0.22, 0.16
self.observation_cov = 0.001
self.current_ewa_size = 0
self.current_ewc_size = 0
self._kf = None # Kalman Filter
self._current_state_means = None
self._current_state_covs = None
def on_bar(self, bar_event):
print(bar_event.bar_start_time)
A = self._data_board.get_hist_price(self.symbols[0], bar_event.bar_start_time)
B = self._data_board.get_hist_price(self.symbols[1], bar_event.bar_start_time)
x = A['Price'].iloc[-1]
y = B['Price'].iloc[-1]
observation_matrix_stepwise = np.array([[x, 1]])
observation_stepwise = y
spread = None
spread_std = None
if self._kf is None:
self._kf = KalmanFilter(n_dim_obs=1, n_dim_state=2,
initial_state_mean=np.ones(2), # initial value
initial_state_covariance=np.ones((2, 2)), # initial value
transition_matrices=np.eye(2), # constant
observation_matrices=observation_matrix_stepwise, # depend on x
observation_covariance=self.observation_cov, # constant
transition_covariance=np.eye(2) * self.state_cov_multiplier) # constant
P = np.ones((2, 2)) + np.eye(2)*self.state_cov_multiplier
spread = y - observation_matrix_stepwise.dot(np.ones(2))[0]
spread_std = np.sqrt(observation_matrix_stepwise.dot(P).dot(observation_matrix_stepwise.transpose())[0][0] + self.observation_cov)
state_means_stepwise, state_covs_stepwise = self._kf.filter(observation_stepwise) # depend on y
self._current_state_means = state_means_stepwise[0]
self._current_state_covs = state_covs_stepwise[0]
else:
state_means_stepwise, state_covs_stepwise = self._kf.filter_update(
self._current_state_means, self._current_state_covs,
observation=observation_stepwise,
observation_matrix=observation_matrix_stepwise)
P = self._current_state_covs + np.eye(2)*self.state_cov_multiplier # This has to be small enough
spread = y - observation_matrix_stepwise.dot(self._current_state_means)[0]
spread_std = np.sqrt(observation_matrix_stepwise.dot(P).dot(observation_matrix_stepwise.transpose())[0][0] + self.observation_cov)
self._current_state_means = state_means_stepwise
self._current_state_covs = state_covs_stepwise
# residual is assumed to be N(0, spread_std)
bollinger_ub = self.bollinger_scaler * spread_std
bollinger_lb = - self.bollinger_scaler * spread_std
coeff = self._current_state_means[0]
print(spread, spread_std)
if (spread > bollinger_ub) and (self.current_ewa_size >= 0):
print ('Hit upper band, short spread.')
o = OrderEvent()
o.full_symbol = self.symbols[0]
o.order_type = OrderType.MARKET
o.order_size = int(-1000*coeff) - self.current_ewa_size
self.place_order(o)
self.current_ewa_size = int(-1000*coeff)
o = OrderEvent()
o.full_symbol = self.symbols[1]
o.order_type = OrderType.MARKET
o.order_size = 1000 - self.current_ewc_size
self.place_order(o)
self.current_ewc_size = 1000
elif (spread < bollinger_lb) and (self.current_ewa_size <= 0):
print('Hit lower band, long spread.')
o = OrderEvent()
o.full_symbol = self.symbols[0]
o.order_type = OrderType.MARKET
o.order_size = int(1000 * coeff) - self.current_ewa_size
self.place_order(o)
self.current_ewa_size = int(1000 * coeff)
o = OrderEvent()
o.full_symbol = self.symbols[1]
o.order_type = OrderType.MARKET
o.order_size = -1000 - self.current_ewc_size
self.place_order(o)
self.current_ewc_size = -1000
elif (spread < 0) and (spread > bollinger_lb) and (self.current_ewa_size < 0):
print('spread crosses below average.cover short spread')
o = OrderEvent()
o.full_symbol = self.symbols[0]
o.order_type = OrderType.MARKET
o.order_size = - self.current_ewa_size
self.place_order(o)
self.current_ewa_size = 0
o = OrderEvent()
o.full_symbol = self.symbols[1]
o.order_type = OrderType.MARKET
o.order_size = - self.current_ewc_size
self.place_order(o)
self.current_ewc_size = 0
elif (spread > 0) and (spread < bollinger_ub) and (self.current_ewa_size > 0):
print('spread crosses above average.cover long spread')
o = OrderEvent()
o.full_symbol = self.symbols[0]
o.order_type = OrderType.MARKET
o.order_size = - self.current_ewa_size
self.place_order(o)
self.current_ewa_size = 0
o = OrderEvent()
o.full_symbol = self.symbols[1]
o.order_type = OrderType.MARKET
o.order_size = - self.current_ewc_size
self.place_order(o)
self.current_ewc_size = 0
from pykalman import KalmanFilter
kf = KalmanFilter(n_dim_state=1, n_dim_obs=1)
# print y
# output=kf.em(y).smooth(normalizedVals)[0]
##print output
# plot('kalman', output)
# plt.show()
# means, covariances = kf.filter(y[0])
import numpy as np
means = np.zeros((1, 1))
covariances = np.zeros((1, 1))
output = []
next_mean, next_covariance = kf.filter_update(means[-1], covariances[-1], y[0])
for new_measurement in y[1:]:
next_mean, next_covariance = kf.filter_update(next_mean, next_covariance, new_measurement)
print next_mean, next_covariance
output.append(next_mean[0])
print output
plot("Filtered", output)
plt.show()
class NeedleTracker(object):
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_subscribers(self):
# image subscribers
rospy.Subscriber(topics.LEFT_IMAGE, Image, self.left_image_callback, queue_size=1)
rospy.Subscriber(topics.RIGHT_IMAGE, Image, self.right_image_callback, queue_size=1)
# TODO: move topics to separate file
rospy.Subscriber("/AD/left/camera_info", CameraInfo, self.left_info_callback)
rospy.Subscriber("/AD/right/camera_info", CameraInfo, self.right_info_callback)
def init_publishers(self):
# needle pose publishers
self.needle_pose_left_pub = rospy.Publisher("/needle_tracker/needle_pose_left", PoseStamped)
self.needle_pose_right_pub = rospy.Publisher("/needle_tracker/needle_pose_right", PoseStamped)
def init_debug_publishers(self):
self.registered_points_pub = rospy.Publisher("/needle_tracker/debug/registered_points", Image)
self.color_seg_pub = rospy.Publisher("/needle_tracker/debug/color_seg_pub", Image)
self.points_3d_pub = rospy.Publisher('/needle_tracker/debug/points_3d', MarkerArray)
def left_info_callback(self, msg):
if self.info['l']:
return
self.info['l'] = msg
def right_info_callback(self, msg):
if self.info['r']:
return
self.info['r'] = msg
def image_publisher(self, image, image_publisher):
img_msg = self.cv_bridge.cv2_to_imgmsg(image)
image_publisher.publish(img_msg)
def left_image_callback(self, msg):
self.left_image = self.cv_bridge.imgmsg_to_cv2(msg, "bgr8")
self.left_points = self.process_image(self.left_image)
# do not continue until we have points from both images
if self.left_points is None or self.right_points is None:
return
# check the order of points and reverse if necessary
if self.left_points[0][0] < self.left_points[-1][0]:
self.left_points = self.left_points[::-1]
if self.right_points[0][0] < self.right_points[-1][0]:
self.right_points = self.right_points[::-1]
max_height = int(np.max(self.left_points[:,0]))
min_height = int(np.min(self.left_points[:,0]))
points_3d = []
for i in range(len(self.left_points)):
pt_left = self.left_points[i]
pt_right = self.right_points[i]
# if abs(pt_left[0] - pt_right[0]) > 20:
# continue
x = pt_left[0]
y = pt_left[1]
disparity = y - pt_right[1]
pt = Util.convertStereo(y,x,disparity, self.info)
points_3d.append(tfx.pose(pt))
# publish 3d point
if self.DEBUG:
markers = MarkerArray()
for i in range(len(points_3d)):
point = points_3d[i]
marker = Util.createMarker(point.msg.PoseStamped(), id=i+1, lifetime=2)
markers.markers.append(marker)
self.points_3d_pub.publish(markers)
# points_3d = [[a.point.x, a.point.y, a.point.z] for a in points_3d]
if points_3d[0].position[0] < points_3d[-1].position[0]:
pt_a = points_3d[0].position
pt_b = points_3d[1].position
pt_c = points_3d[-1].position
else:
pt_a = points_3d[-1].position
pt_b = points_3d[-2].position
pt_c = points_3d[0].position
x = np.hstack(np.array(pt_a - pt_b))
z = np.hstack(np.array(pt_a - pt_c))
y = np.cross(z,x)
z = np.cross(x,y)
x /= np.linalg.norm(x)
y /= np.linalg.norm(y)
z /= np.linalg.norm(z)
rotation_matrix = np.transpose(np.array([x,y,z]))
obs = tfx.pose(pt_a, rotation_matrix)
observation = [obs.position.x, obs.position.y, obs.position.z,
obs.tb_angles.yaw_deg, obs.tb_angles.pitch_deg, obs.tb_angles.roll_deg]
trans_covar = 0.01*np.eye(6)
(self.left_needle_pose, self.left_needle_covar) = self.left_kf.filter_update(self.left_needle_pose, self.left_needle_covar, transition_covariance = trans_covar, observation=observation)
self.left_needle_pose = self.left_needle_pose.data
rotation = tfx.tb_angles(*(self.left_needle_pose[3:]))
position = self.left_needle_pose[:3]
needle_pose = tfx.pose(position, rotation)
needle_pose.frame = 'left_AD' # TODO: dont' hard code this
self.needle_pose_left_pub.publish(needle_pose.msg.PoseStamped())
if points_3d[0].position[0] >= points_3d[-1].position[0]:
pt_a = points_3d[0].position
pt_b = points_3d[1].position
pt_c = points_3d[-1].position
else:
pt_a = points_3d[-1].position
pt_b = points_3d[-2].position
pt_c = points_3d[0].position
x = np.hstack(np.array(pt_a - pt_b))
z = np.hstack(np.array(pt_a - pt_c))
y = np.cross(z,x)
z = np.cross(x,y)
x /= np.linalg.norm(x)
y /= np.linalg.norm(y)
z /= np.linalg.norm(z)
rotation_matrix = np.transpose(np.array([x,y,z]))
obs = tfx.pose(pt_a, rotation_matrix)
observation = [obs.position.x, obs.position.y, obs.position.z,
obs.tb_angles.yaw_deg, obs.tb_angles.pitch_deg, obs.tb_angles.roll_deg]
trans_covar = 0.01*np.eye(6)
(self.right_needle_pose, self.right_needle_covar) = self.right_kf.filter_update(self.right_needle_pose, self.right_needle_covar, transition_covariance = trans_covar, observation=observation)
self.right_needle_pose = self.right_needle_pose.data
rotation = tfx.tb_angles(*(self.right_needle_pose[3:]))
position = self.right_needle_pose[:3]
needle_pose = tfx.pose(position, rotation)
needle_pose.frame = 'left_AD' # TODO: dont' hard code this
self.needle_pose_right_pub.publish(needle_pose.msg.PoseStamped())
def right_image_callback(self, msg):
self.right_image = self.cv_bridge.imgmsg_to_cv2(msg, "bgr8")
if self.right_points is None:
self.right_points = self.process_image(self.right_image)
def process_image(self, image):
return self.get_needle_points(image)
def get_needle_points(self, image):
""" Returns points along an ellipse that lines up the needle in the image """
erosion_kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3,3))
image = cv2.erode(image, erosion_kernel)
image = cv2.erode(image, erosion_kernel)
# image = cv2.erode(image, erosion_kernel)
# image = cv2.dilate(image, erosion_kernel)
# image = cv2.dilate(image, erosion_kernel)
# segment the needle from the background
hsv_image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
# segment needle based on its hue
segmented_image = cv2.inRange(hsv_image, np.array([20,90,50]),np.array([40,180,255]))
# publish segmented image
if self.DEBUG:
color_image = cv2.cvtColor(segmented_image,cv2.COLOR_GRAY2RGB)
self.image_publisher(color_image, self.color_seg_pub)
# create a file containing all 2d positions of segemented points
points = np.transpose(np.nonzero(segmented_image))
# np.savetxt("needle_registration/input/needle_seg.txt", points, delimiter=" ")
# result = gmmreg._core.run_ini("needle_registration/needle.ini")
# after_tps = result[2]
scene = matlab.double(points.tolist())
model = matlab.double(self.model.tolist())
result = self.eng.get_cpd_affine(model, scene)
# affine transformation A*x+b
A = np.array(result['R'])
b = np.array(result['t'])
x = np.transpose(self.dense_model)
transformed_pts = np.transpose(np.dot(A, x) + b)
# draw ellipse
if self.DEBUG:
color_image = image.copy()
for coord in np.asarray(transformed_pts, dtype=np.uint32):
coord = tuple(coord)[::-1]
# color_image[coord] = np.array([0, 0, 255])
cv2.circle(color_image, coord, 1, [0,0,255])
self.image_publisher(color_image, self.registered_points_pub)
return transformed_pts
def exit_handler(self, signal, frame):
rospy.loginfo("Shutting down needle tracking node...")
sys.exit(0)
print ''
#t = 0
mu = mu[-1]
sig = sig[-1]
for i in range(2, len(measurements)): # starting with third measurement...
if rospy.is_shutdown(): # easy interruption
break
t = i * 1/10.
#t += 1/10.
measurement = measurements[i]
#print measurement
#print mu
#print sig
mu, sig = kf.filter_update(mu, sig, observation=measurement)
#print mu
#print sig
#print ''
image_temp = image.copy()
cv2.circle(image_temp, (measurement.data[0], measurement.data[1]), 2, (255,0,0), -1)
cv2.circle(image_temp, (int(mu[0]), int(mu[1])), 5, (0,0,255))
spread = numpy.sqrt(sig.diagonal()) * 10
cv2.rectangle(image_temp, (int(mu[0] - spread[0]), int(mu[1] - spread[1])),
(int(mu[0] + spread[0]), int(mu[1] + spread[1])),
(0,0,255), 2)
cv2.imshow('2D Position & Variance', image_temp)
cv2.waitKey(1000/100) # display at 100Hz
n_dim_state = 2#kf.transition_matrix.shape[0]
filtered_state_means = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances = np.zeros((n_timesteps, n_dim_state, n_dim_state))
#filtered_state_means=measurementsRT
#filtered_state_covariances=measurementsRT
for t in range(n_timesteps - 1):
measurement=readbgblob()
measurementsRT[t,:]=measurement
if t == 0:
filtered_state_means[t,] = measurements[i,]#kf.initial_state_mean
filtered_state_covariances[t,] = [[70,70],[70,70]]#kf.initial_state_covariance
filtered_state_means[t + 1,], filtered_state_covariances[t + 1,] = (
kf.filter_update(
filtered_state_means[t,],
filtered_state_covariances[t,],
measurementsRT[t,]#,
#transition_offset=kf.transition_offsets[0], # originally data.transition_offsets[t]
#observation_offset=kf.observation_offsets
)
)
print filtered_state_means[t+1]
print measurementsRT[t]
#%% draw estimates
pl.figure()
lines_true = pl.plot(measurementsRT[:,0],measurementsRT[:,1], '-*',color='b')
lines_filt = pl.plot(filtered_state_means[:,0],filtered_state_means[:,1], color='r')
pl.legend((lines_true[0], lines_filt[0]), ('measured', 'filtered'))
pl.show()
#%% sockets for OE
def main():
#Paramètres du modèle
#Indiquer à quoi correspond chaque variable
osigma=0.1;
transition_matrix = np.array([[1., 0.,0.,0.,0.,0.],[1., 1.,0.,0.,0.,0.],[0.,0,1,0.,0.,0.]])
transition_covariance = np.zeros((6,6));
observation_matrix = np.array([[0., 1.,0.],[0., 0.,1.]])
observation_covariance = np.eye(2)*osigma
initial_state_mean = np.array([1,0,10]) # m(0)
initial_state_covariance = np.eye(3);# p(0)
#Observations
observations = csv.reader(open('voitureObservations.csv','r'),delimiter=' ')
# Filtrage à la main
# Quels sont les paramètres du constructeur ?
kf = KalmanFilter(
transition_matrix, observation_matrix,
transition_covariance, observation_covariance,
)
# Que conserverons les variables suivantes ?
hand_state_estimates=[initial_state_mean]
hand_state_cov_estimates=[initial_state_covariance]
for anObs in observations:
# Quelles étapes du filtrage sont réalisées par la ligne suivante
(aMean,aCov) = kf.filter_update(hand_state_estimates[-1],hand_state_cov_estimates[-1],anObs)
hand_state_estimates.append(aMean)
hand_state_cov_estimates.append(aCov)
hand_state_estimates=np.array(hand_state_estimates)
# A quoi sert la ligne suivante ?
hand_positions=np.dot(hand_state_estimates,observation_matrix.T ) # obtenir les observation (H*Mk-1 dans le cours, rouge) projet mon espace etat ver mon espace observation
plt.figure(1)
plt.plot(observations[:,0],observations[:,1], 'r+')
plt.plot(hand_positions[:,0],hand_positions[:,1], 'b')
plt.savefig('illustration_filtrage_main_voiture.pdf')
plt.close()
# Filtrage complet
# Que fait cette section ?
# Quels sont les paramètres supplémentaires donnés au constructeur ?
# Quels sont les autres paramètres possibles ?
kf = KalmanFilter(
transition_matrix, observation_matrix,
transition_covariance, observation_covariance,
initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance,
)
(filtered_state_estimates,filtered_state_cov_estimates) = kf.filter(observations)
filtered_positions=np.dot(filtered_state_estimates,observation_matrix.T )
plt.figure(1)
plt.plot(observations[:,0],observations[:,1], 'r+')
plt.plot(filtered_positions[:,0],filtered_positions[:,1], 'b')
plt.savefig('illustration_filtrage_voiture.pdf')
plt.close()
# Lissage
# Que fait cette section ?
# Quel est la différence avec le filtrage ?
# Puis-je faire un lissage "à la main" observation par observation ?
kf = KalmanFilter(
transition_matrix, observation_matrix,
transition_covariance, observation_covariance,
initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance,
)
(smoothed_state_estimates,smoothed_state_cov_estimates) = kf.smooth(observations) # au lieu de filtre, on appel smooth
smoothed_positions=np.dot(smoothed_state_estimates,observation_matrix.T )
plt.figure(2)
plt.plot(observations[:,0],observations[:,1], 'r+')
plt.plot(smoothed_positions[:,0],smoothed_positions[:,1], 'b')
plt.savefig('illustration_lissage_voiture.pdf')
plt.close()
data.initial_state_covariance,
random_state=0
)
# Estimate mean and covariance of hidden state distribution iteratively. This
# is equivalent to
#
# >>> (filter_state_means, filtered_state_covariance) = kf.filter(data)
n_timesteps = data.observations.shape[0]
n_dim_state = data.transition_matrix.shape[0]
filtered_state_means = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances = np.zeros((n_timesteps, n_dim_state, n_dim_state))
for t in range(n_timesteps - 1):
if t == 0:
filtered_state_means[t] = data.initial_state_mean
filtered_state_covariances[t] = data.initial_state_covariance
filtered_state_means[t + 1], filtered_state_covariances[t + 1] = (
kf.filter_update(
filtered_state_means[t],
filtered_state_covariances[t],
data.observations[t + 1],
transition_offset=data.transition_offsets[t],
)
)
# draw estimates
pk.figure()
lines_true = pk.plot(data.states, color='b')
lines_filt = pk.plot(filtered_state_means, color='r')
pk.legend((lines_true[0], lines_filt[0]), ('true', 'filtered'))
pk.show()
# spread_std = np.sqrt(observation_matrix_stepwise.dot(P).dot(observation_matrix_stepwise.transpose())[0][0] + observation_cov)
# print(spread, spread_std)
state_means_stepwise, state_covs_stepwise = kf.filter(observation_stepwise) # depend on y
# print(state_means_stepwise, state_covs_stepwise)
means_trace.append(state_means_stepwise[0])
covs_trace.append(state_covs_stepwise[0])
for step in range(1, data.shape[0]):
# print(step)
x = data[sym_a][step]
y = data[sym_b][step]
observation_matrix_stepwise = np.array([[x, 1]])
observation_stepwise = y
state_means_stepwise, state_covs_stepwise = kf.filter_update(
means_trace[-1], covs_trace[-1],
observation=observation_stepwise,
observation_matrix=observation_matrix_stepwise)
# print(state_means_stepwise, state_covs_stepwise)
# P = covs_trace[-1] + np.eye(2)*state_cov_multiplier # This has to be small enough
# spread = y - observation_matrix_stepwise.dot(means_trace[-1])[0]
# spread_std = np.sqrt(observation_matrix_stepwise.dot(P).dot(observation_matrix_stepwise.transpose())[0][0] + observation_cov)
# print(spread, spread_std)
means_trace.append(state_means_stepwise.data)
covs_trace.append(state_covs_stepwise)
# ------------------------------------ line evolvement --------------------------------------------#
# colormap
cm = plt.get_cmap('jet')
colors = np.linspace(0.1, 1, data.shape[0])
sc = plt.scatter(data[sym_a], data[sym_b], s=10, c=colors, cmap=cm, edgecolors='k', alpha=0.7)
