def calc_acceleration(velocities): ## kalman data = velocities ss = 6 # state size os = 3 # observation size F = np.array([ [1,0,0,1,0,0], # process update [0,1,0,0,1,0], [0,0,1,0,0,1], [0,0,0,1,0,0], [0,0,0,0,1,0], [0,0,0,0,0,1]], dtype=np.float) H = np.array([ [1,0,0,0,0,0], [0,1,0,0,0,0], [0,0,1,0,0,0]], # observation matrix dtype=np.float) Q = 0.01*np.eye(ss) # process noise R = 1*np.eye(os) # observation noise initx = np.array([velocities[0,0], velocities[0,1], velocities[0,2], 0, 0, 0], dtype=np.float) initv = 0*np.eye(ss) xsmooth,Vsmooth = kalman_math.kalman_smoother(data, F, H, Q, R, initx, initv, plot=False) accel_smooth = xsmooth[:,3:]*100. return accel_smooth
def calc_heading(velocities): heading_norollover = floris_math.remove_angular_rollover(np.arctan2(velocities[:,1], velocities[:,0]), 3) ## kalman data = heading_norollover.reshape([len(heading_norollover),1]) ss = 3 # state size os = 1 # observation size F = np.array([ [1,1,0], # process update [0,1,1], [0,0,1]], dtype=np.float) H = np.array([ [1,0,0]], # observation matrix dtype=np.float) Q = np.eye(ss) # process noise Q[0,0] = .01 Q[1,1] = .01 Q[2,2] = .01 R = 1*np.eye(os) # observation noise initx = np.array([data[0,0], data[1,0]-data[0,0], 0], dtype=np.float) initv = 0*np.eye(ss) xsmooth,Vsmooth = kalman_math.kalman_smoother(data, F, H, Q, R, initx, initv, plot=False) heading_norollover_smooth = xsmooth[:,0] heading_smooth_diff = xsmooth[:,1]*trajec.fps heading = floris_math.fix_angular_rollover(heading_norollover) heading_smooth = floris_math.fix_angular_rollover(heading_norollover_smooth) return heading_smooth