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