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