def smooth(data, Q, R, F=None, H=None, interpvals=0):
try:
os = data.shape[1] # observation size
except:
os = 1
ss = os * 2 # state size
if F is None:
if ss == 4:
F = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], dtype=np.float) # process update
elif ss == 2:
F = np.array([[1, 1], [0, 1]], dtype=np.float) # process update
if H is None:
if ss == 4:
H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], dtype=np.float) # observation matrix
elif ss == 2:
H = np.array([[1, 0]], dtype=np.float) # observation matrix
data = np.nan_to_num(data)
interpolated_data = np.zeros_like(data)
for c in range(os):
interpolated_data[:, c] = pm.interpolate(data[:, c], interpvals)
y = interpolated_data
initx = np.array([y[0, 0], y[1, 0] - y[0, 0]], dtype=np.float)
initV = 0 * np.eye(ss)
xsmooth, Vsmooth = adskalman.kalman_smoother(y, F, H, Q, R, initx, initV)
return xsmooth, Vsmooth
def ori_smooth(directions, frames_per_second=None, return_missing=False):
"""smooth orientations using an RTS smoother
This treats orientations as XYZ positions
"""
dt = 1.0 / frames_per_second
A = np.array([
[1, 0, 0, dt, 0, 0],
[0, 1, 0, 0, dt, 0],
[0, 0, 1, 0, 0, dt],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
])
Q = 1.0 * np.eye(6)
C = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0]])
R = 30.0 * np.eye(3)
init_x = np.hstack((directions[0, :], np.zeros((3, ))))
assert not np.any(
np.isnan(init_x)), "cannot start with missing orientation"
init_V = 2 * Q
y = directions
dirsmooth, V = adskalman.kalman_smoother(y, A, C, Q, R, init_x, init_V)
dirsmooth = dirsmooth[:, :3] # take only position information
dirsmooth_missing = np.array(dirsmooth, copy=True)
# remove results too distant from observations
avlen = 5
if len(y) >= avlen:
good = ~np.isnan(y[:, 0])
near_good = running_average(good, avlen)
pad = avlen // 2
near_good = np.hstack((np.zeros((pad, )), near_good, np.zeros(
(pad, ))))
good_cond = near_good > 0.2
bad_cond = ~good_cond
if bad_cond.shape != dirsmooth[:, 0].shape:
print("xxxx")
print(bad_cond.shape)
print(dirsmooth[:, 0].shape)
print(directions.shape)
dirsmooth[bad_cond, :] = np.nan
# normalize lengths to unit vectors
np.sum(dirsmooth**2, axis=1)
dirsmooth = (dirsmooth.T / np.sqrt(np.sum(dirsmooth**2, axis=1))).T
if return_missing:
return dirsmooth, dirsmooth_missing
else:
return dirsmooth
def kalman_smoother(data, F, H, Q, R, initx, initv, plot=False):
os = H.shape[0]
ss = F.shape[0]
xsmooth,Vsmooth = adskalman.kalman_smoother(data,F,H,Q,R,initx,initv)
if plot:
plt.plot(xsmooth[:])
plt.plot(data[:], '*')
return xsmooth,Vsmooth
def kalman_smoother(data, F, H, Q, R, initx, initv, plot=False):
os = H.shape[0]
ss = F.shape[0]
interpolated_data = np.zeros_like(data)
for c in range(os):
interpolated_data[:, c] = interpolate_nan(data[:, c])
y = interpolated_data
xsmooth, Vsmooth = adskalman.kalman_smoother(y, F, H, Q, R, initx, initv)
if plot:
plt.plot(xsmooth[:, 0])
plt.plot(y[:, 0], '*')
return xsmooth, Vsmooth
def kalman_smoother(data, F, H, Q, R, initx, initv, plot=False):
os = H.shape[0]
ss = F.shape[0]
interpolated_data = np.zeros_like(data)
for c in range(os):
interpolated_data[:, c] = floris_math.interpolate_nan(data[:, c])
y = interpolated_data
xsmooth, Vsmooth = adskalman.kalman_smoother(y, F, H, Q, R, initx, initv)
if plot:
plt.plot(xsmooth[:, 0])
plt.plot(y[:, 0], "*")
return xsmooth, Vsmooth
def calc_obj_motion(npmovie):
npmovie.kalmanobj = Object(npmovie)
ss = 4 # state size
os = 2 # observation size
F = numpy.array([[1,0,1,0], # process update
[0,1,0,1],
[0,0,1,0],
[0,0,0,1]],
dtype=numpy.float)
H = numpy.array([[1,0,0,0], # observation matrix
[0,1,0,0]],
dtype=numpy.float)
Q = 0.0001*numpy.eye(ss) # process noise
R = 100*numpy.eye(os) # observation noise
interpolated_positions_0 = interpolate(npmovie.obj.positions[:,0], 0)
interpolated_positions_1 = interpolate(npmovie.obj.positions[:,1], 0)
raw_x = np.nan_to_num(interpolated_positions_0)
indices = np.nonzero(raw_x)[0].tolist()
raw_x = raw_x[indices]
raw_y = np.nan_to_num(interpolated_positions_1)
raw_y = raw_y[indices]
y = np.vstack( (raw_x, raw_y) ).T
initx = numpy.array([y[0,0], y[0,1],0,0],dtype=numpy.float)
initV = 0*numpy.eye(ss)
xsmooth,Vsmooth = adskalman.kalman_smoother(y,F,H,Q,R,initx,initV)
npmovie.kalmanobj.positions[indices] = xsmooth[:,0:2]
npmovie.kalmanobj.velocities[indices] = xsmooth[:,2:4]
npmovie.kalmanobj.timestamps = [copy.copy(npmovie.uframes[i].timestamp) for i in indices]
npmovie.kalmanobj.indices = indices
npmovie.kalmanobj.errors = Vsmooth
npmovie.kalmanobj.speed = np.zeros([len(npmovie.uframes), 1])
for i, v in enumerate(npmovie.kalmanobj.velocities):
npmovie.kalmanobj.speed[i] = np.linalg.norm(v)
# need to fix/smooth missing angles
for i in range(len(npmovie.kalmanobj.indices)):
frame = indices[i]
npmovie.kalmanobj.long_axis[frame] = npmovie.obj.long_axis[frame] / np.linalg.norm(npmovie.obj.long_axis[frame])
for i in range(1,len(npmovie.kalmanobj.indices)):
frame = indices[i]
if npmovie.kalmanobj.long_axis[frame][0] == 1 or npmovie.kalmanobj.long_axis[frame][1] == 0:
future_axis = 1
future_frame = frame
while future_axis == 1:
future_frame += 1
if future_frame > npmovie.kalmanobj.indices[-1]:
future_frame = frame
break
future_axis = npmovie.kalmanobj.long_axis[future_frame][0]
delta_axis = (npmovie.kalmanobj.long_axis[future_frame] - npmovie.kalmanobj.long_axis[frame-1]) / float(future_frame- (frame-1) )
npmovie.kalmanobj.long_axis[frame] = npmovie.kalmanobj.long_axis[frame-1] + delta_axis
# fix angle orientation:
# flies don't spin around immediately, so generally body angle should be rouhgly the same from frame to frame, at least within 180 deg
npmovie.obj.dot_prev_ori = np.zeros(len(npmovie.kalmanobj.indices))
npmovie.obj.dot_vel = np.zeros(len(npmovie.kalmanobj.indices))
if 1:
npmovie.kalmanobj.long_axis[0] = npmovie.obj.long_axis[0]
for i in range(1,len(npmovie.kalmanobj.indices)):
if i < 2:
switching_threshold = 0
else:
switching_threshold = 0.2
frame = indices[i]
dot_prev_ori = np.dot(npmovie.kalmanobj.long_axis[frame-1], npmovie.obj.long_axis[frame])
dot_vel = np.dot(npmovie.kalmanobj.velocities[frame], npmovie.obj.long_axis[frame])
npmovie.obj.dot_prev_ori[i] = dot_prev_ori
npmovie.obj.dot_vel[i] = dot_vel
direction = 1.
if dot_vel < 0 and np.abs(dot_vel) > switching_threshold:
direction = -1
else:
if dot_prev_ori < 0: # not aligned with previous frame by > 90 deg
direction = -1
if dot_vel < 0: # orientation not aligned with velocity
if np.abs(dot_vel) > switching_threshold:
direction = -1
if dot_vel > 0: # orientation is aligned with velocity, but not with prev ori
if np.abs(dot_vel) > switching_threshold:
direction = 1
npmovie.kalmanobj.long_axis[frame] = npmovie.obj.long_axis[frame]*direction
return npmovie
def fuse_obj_ids(use_obj_ids, data_file,
dynamic_model_name = None,
frames_per_second=None):
"""take multiple obj_id tracks and fuse them into one long trajectory
Current implementation
======================
Load 'observations' (MLEs of fly positions) across all obj_ids,
and then do Kalman smoothing across all this, which fills in gaps
(but ignores single camera views).
"""
ca = core_analysis.get_global_CachingAnalyzer()
frames = []
xs = []
ys = []
zs = []
for obj_id in use_obj_ids:
#print
#print obj_id
kalman_rows = ca.load_dynamics_free_MLE_position( obj_id, data_file)
if len(frames):
tmp = kalman_rows['frame']
#print tmp
#(hmm, why do we care? this seems wrong, anyway...)
#assert tmp[0] > frames[-1][-1] # ensure new frames follow last
assert numpy.all((tmp[1:]-tmp[:-1]) > 0) # ensure new frames are ordered
this_x = kalman_rows['x']
full_obs_idx = ~numpy.isnan(this_x)
if 1:
warnings.warn('dropping last ML estimate of position in fuse_obj_ids because it is frequently noisy')
full_obs_idx = np.nonzero(full_obs_idx)[0]
full_obs_idx = full_obs_idx[:-1]
if not len(full_obs_idx):
warnings.warn('no data used for obj_id %d in fuse_obj_ids()'%obj_id)
continue # no data
frames.append( kalman_rows['frame'][full_obs_idx] )
xs.append( kalman_rows['x'][full_obs_idx] )
ys.append( kalman_rows['y'][full_obs_idx] )
zs.append( kalman_rows['z'][full_obs_idx] )
frames = numpy.hstack(frames)
xs = numpy.hstack(xs)
ys = numpy.hstack(ys)
zs = numpy.hstack(zs)
X = numpy.array([xs,ys,zs])
if 0:
import pylab
X=X.T
ax = pylab.subplot(3,1,1)
ax.plot( frames, X[:,0],'.' )
ax = pylab.subplot(3,1,2,sharex=ax)
ax.plot( frames, X[:,1],'.' )
ax = pylab.subplot(3,1,3,sharex=ax)
ax.plot( frames, X[:,2],'.' )
pylab.show()
sys.exit()
# convert to a single continuous masked array
frames_all = numpy.arange(frames[0],frames[-1]+1)
xs_all = numpy.ma.masked_array( data=numpy.ones( frames_all.shape ),
mask=numpy.ones( frames_all.shape, dtype=numpy.bool ))
ys_all = numpy.ma.masked_array( data=numpy.ones( frames_all.shape ),
mask=numpy.ones( frames_all.shape, dtype=numpy.bool ))
zs_all = numpy.ma.masked_array( data=numpy.ones( frames_all.shape ),
mask=numpy.ones( frames_all.shape, dtype=numpy.bool ))
idxs = frames_all.searchsorted( frames )
#idxs = find_first_idxs(frames,frames_all)
xs_all[idxs] = xs
ys_all[idxs] = ys
zs_all[idxs] = zs
orig_data_present = np.zeros( frames_all.shape, dtype=bool )
orig_data_present[idxs] = True
# "obs" == "observations" == ML estimates of position without dynamics
if 0:
# requires numpy >= r5284
obs = numpy.ma.masked_array( [xs_all, ys_all, zs_all] ).T # Nx3 array for N frames of data
else:
obs = numpy.ma.hstack( [xs_all[:,numpy.newaxis], ys_all[:,numpy.newaxis], zs_all[:,numpy.newaxis]] )
if 0:
import pylab
X=obs
frames = frames_all
ax = pylab.subplot(3,1,1)
ax.plot( frames, X[:,0],'.' )
ax = pylab.subplot(3,1,2,sharex=ax)
ax.plot( frames, X[:,1],'.' )
ax = pylab.subplot(3,1,3,sharex=ax)
ax.plot( frames, X[:,2],'.' )
pylab.show()
sys.exit()
if 1:
# convert from masked array to array with nan
obs[ obs.mask ] = numpy.nan
obs = numpy.ma.getdata(obs)
if 0:
obs = obs[:100,:]
print 'obs'
print obs
if 1:
# now do kalman smoothing across all obj_ids
model = flydra_core.kalman.dynamic_models.get_kalman_model(name=dynamic_model_name,
dt=(1.0/frames_per_second))
# initial state guess: postion = observation, other parameters = 0
ss = model['ss']
init_x = numpy.zeros( (ss,) )
init_x[:3] = obs[0,:]
P_k1=numpy.zeros((ss,ss)) # initial state error covariance guess
for i in range(0,3):
P_k1[i,i]=model['initial_position_covariance_estimate']
for i in range(3,6):
P_k1[i,i]=model.get('initial_velocity_covariance_estimate',0.0)
if ss > 6:
for i in range(6,9):
P_k1[i,i]=model.get('initial_acceleration_covariance_estimate',0.0)
if not 'C' in model:
raise ValueError('model does not have a linear observation matrix "C".')
xsmooth, Psmooth = adskalman.kalman_smoother(obs,
model['A'],
model['C'],
model['Q'],
model['R'],
init_x,
P_k1,
)
X = xsmooth[:,:3] # kalman estimates of position
if 0:
print 'X'
print X
print dynamic_model_name
sys.exit()
recarray = numpy.rec.fromarrays([frames_all,
X[:,0],
X[:,1],
X[:,2],
orig_data_present,
],
names='frame,x,y,z,orig_data_present')
return recarray
