def run_kalman(T_obs, x_init, covar_init, f=30.):
"""
Runs the kalman filter using just the observations from hydra.
"""
dt = 1/f
N = len(T_obs)
## load the noise covariance matrices:
covar_mats = cPickle.load(open(hd_path + '/hd_track/data/old/pr2-hydra-kinect-covars-xyz-rpy.cpickle'))
motion_covar = 1e4*covar_mats['process']
hydra_covar = 1e5*covar_mats['hydra']
## initialize the filter:
KF = kalman()
KF.init_filter(0, x_init, covar_init, motion_covar, hydra_covar, None)
ts = dt * (np.arange(N)+1)
estimates = [] ## the kalman filter estimates
covars = []
## run the filter:
for i in xrange(len(ts)-1):
KF.observe_hydra(T_obs[i+1], ts[i])
estimates.append(KF.x_filt)
covars.append(KF.S_filt)
return (estimates, covars)
def run_smoother():
dt = 1/30.
N = 3000
Ts_bh, Ts_bg, T_gh, Ts_bg_gh, X_bh, X_bg_gh = load_data()
Ts_bh = Ts_bh[10:N]
X_bh = X_bh[:,10:N]
X_bg_gh = X_bg_gh[:,10:N]
## initialize the kalman belief:
x_init = X_bg_gh[:,0]
I3 = np.eye(3)
S_init = scl.block_diag(1e-6*I3, 1e-3*I3, 1e-4*I3, 1e-1*I3)
## run the kalman filter:
X_kf, S_kf = run_kalman(Ts_bh, x_init, S_init, 1./dt)
## load the noise covariance matrices:
covar_mats = cPickle.load(open(hd_path + '/hd_track/data/old/pr2-hydra-kinect-covars-xyz-rpy.cpickle'))
R = covar_mats['process']
A, _ = kalman().get_motion_mats(dt)
X_s, _ = smoother(A, 4e2*np.eye(12), X_kf, S_kf)
X_s = np.array(X_s).T[0,:,:]
X_kf = np.array(X_kf)
X_kf = np.reshape(X_kf, (X_kf.shape[0], X_kf.shape[1])).T
## plot the results:
print X_kf.shape, X_bh.shape, X_bg_gh.shape
plot_smoother(X_s, X_kf[:,1:], X_bh, X_bg_gh)
plt.show()
def fit_process_noise(fname=None, f=30.):
"""
Gets the motion-model noise covariance.
fname : file name of pickle file containing joint-angles of the pr2.
f : the frequency at which the joints were logged.
covariance is calculated as:
covar = 1/n sum_i=1^n (x(t+1) - A*x(t)).T * (x(t+1) - A*x(t))
"""
dt = 1.0/f
if fname==None:
fname = hd_path + '/hd_track/data/pr2-knot-tie-joint-states.cpickle'
joints = cPickle.load(open(fname, 'rb'))['mat']
rstates = state_from_tfms(tfms_from_joints(joints[:,0:7])).T
lstates = state_from_tfms(tfms_from_joints(joints[:,8:15])).T
A = kalman().get_motion_mat(dt)
r_err = A.dot(rstates[:,0:-1]) - rstates[:,1:]
r_err[6:9,:] = put_in_range(r_err[6:9,:])
r_err = r_err[:,np.nonzero(np.sum(r_err, axis=0))[0]] # take out all zero columns : not sure if correct thing to do.
r_covar = r_err.dot(r_err.T) / r_err.shape[1]
l_err = A.dot(lstates[:,0:-1]) - lstates[:,1:]
l_err[6:9,:] = put_in_range(l_err[6:9,:])
l_err = l_err[:, np.nonzero(np.sum(l_err, axis=0))[0]]
l_covar = l_err.dot(l_err.T) / l_err.shape[1]
return (l_err, l_covar, r_err, r_covar)
def setup_kalman(fname, lr, freq, single_camera):
c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs = load_data(fname, lr, single_camera)
motion_var, ar_var, hydra_var, hydra_vvar = load_covariances()
dt = 1./freq
if c2_ts.any():
tmin = min(np.min(c1_ts), np.min(c2_ts), np.min(hy_ts))
tmax = max(np.max(c1_ts), np.max(c2_ts), np.max(hy_ts))
else:
tmin = min(np.min(c1_ts), np.min(hy_ts))
tmax = max(np.max(c1_ts), np.max(hy_ts))
## calculate the number of time-steps for the kalman filter.
nsteps = int(math.ceil((tmax-tmin)/dt))
## get rid of absolute time, put the three data-streams on the same time-scale
c1_ts -= tmin
c2_ts -= tmin
hy_ts -= tmin
# initialize KF:
x0, S0 = get_first_state(dt, c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs)
KF = kalman()
## ===> assumes that the variance for ar1 and ar2 are the same!!!
KF.init_filter(-dt, x0, S0, motion_var, hydra_var, hydra_vvar, ar_var, ar_var)
ar1_strm = streamize(c1_tfs, c1_ts, freq, avg_transform)
ar2_strm = streamize(c2_tfs, c2_ts, freq, avg_transform)
hy_strm = streamize(hy_tfs, hy_ts, freq, avg_transform)
return (KF, nsteps, tmin, ar1_strm, ar2_strm, hy_strm)
def fit_ar_noise(Ts_bg, Ts_ba_raw, T_ga, f):
"""
Similar to the function above for fitting hydra-noise covariance.
"""
dt = 1./f
assert len(Ts_bg) == len(Ts_ba_raw), "Number of ar and pr2 transforms not equal."
Ts_ba = []
Ts_bg_ga = []
for i in xrange(len(Ts_ba_raw)):
T_ba = Ts_ba_raw[i]
T_bg = Ts_bg[i]
if T_ba == None:
continue
else:
#print len(t)
Ts_bg_ga.append(T_bg.dot(T_ga))
Ts_ba.append(T_ba)
assert len(Ts_bg_ga) == len(Ts_ba), "Number of valid ar and pr2 transforms not equal."
## extract the full state vector:
X_ba = state_from_tfms(Ts_ba, dt).T
X_bg_ga = state_from_tfms(Ts_bg_ga, dt).T
X_bg_ga[6:9,:] = closer_angle(X_bg_ga[6:9,:], X_ba[6:9,:])
C = kalman().get_ar_mats()[0]
err = C.dot(X_ba) - C.dot(X_bg_ga)
covar = (err.dot(err.T))/err.shape[1]
return (err, covar)
def fit_hydra_noise(Ts_bg, Ts_bh_raw, T_gh, f):
"""
Get the hydra measurement covariance.
Ts_bg : list of transforms from pr2's base to its gripper holding the hydra.
Ts_bh : list of transforms from pr2's base to hydra sensor.
T_gh : One offset transform from pr2's gripper holding the hydra and the hydra sensor.
f : the frequency at which the data is logged.
"""
dt = 1./f
assert len(Ts_bg) == len(Ts_bh_raw), "Number of hydra and pr2 transforms not equal."
Ts_bh = []
Ts_bg_gh = []
for i in xrange(len(Ts_bh_raw)):
T_bh = Ts_bh_raw[i]
T_bg = Ts_bg[i]
if T_bh == None:
continue
else:
#print len(t)
Ts_bg_gh.append(T_bg.dot(T_gh))
Ts_bh.append(T_bh)
assert len(Ts_bg_gh) == len(Ts_bh), "Number of valid ar and pr2 transforms not equal."
## extract the full state vector:
X_bh = state_from_tfms(Ts_bh, dt).T
X_bg_gh = state_from_tfms(Ts_bg_gh, dt).T
X_bg_gh[6:9,:] = closer_angle(X_bg_gh[6:9,:], X_bh[6:9,:])
C = kalman().get_hydra_mats()[0]
err = C.dot(X_bh) - C.dot(X_bg_gh)
covar = (err.dot(err.T))/err.shape[1]
return (err, covar)
def setup_kalman(fname, freq=30.):
c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs = load_data(fname)
motion_var, ar_var, hydra_var = load_covariances()
dt = 1/freq
tmin = min(np.min(c1_ts), np.min(c2_ts), np.min(hy_ts))
tmax = max(np.max(c1_ts), np.max(c2_ts), np.max(hy_ts))
## calculate the number of time-steps for the kalman filter.
nsteps = int(math.ceil((tmax-tmin)/dt))
## get rid of absolute time, put the three data-streams on the same time-scale
c1_ts -= tmin
c2_ts -= tmin
hy_ts -= tmin
# initialize KF:
x0, S0 = get_first_state(dt, c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs)
KF = kalman()
## ===> assumes that the variance for ar1 and ar2 are the same!!!
hydra_var_vel = np.zeros((6, 6))
hydra_var_vel[3:6, 3:6] = hydra_var[3:6, 3:6]
hydra_var_vel[0:3, 0:3] = 25e-8 * np.eye(3)
KF.init_filter(0,x0, S0, motion_var, hydra_var_vel, ar_var, ar_var)
ar1_strm = streamize(c1_tfs, c1_ts, freq, avg_transform)
ar2_strm = streamize(c2_tfs, c2_ts, freq, avg_transform)
hy_strm = streamize(hy_tfs, hy_ts, freq, avg_transform)
return (KF, nsteps, tmin, ar1_strm, ar2_strm, hy_strm)
def initialize_KFs(kf_data, freq):
"""
Initializes the Kalman filters (one-each for left/right x segment).
KF_DATA : Data as returned by prepare_kf_data function above.
"""
KFs = {'l':None, 'r':None}
motion_covar = initialize_motion_covariance(freq)
n_segs = len(kf_data['r'])
for lr in 'lr':
KF_lr = []
for i in xrange(n_segs):
hydra_strm = kf_data[lr][i]['hydra_strm']
cam_dat = kf_data[lr][i]['cam_dat']
cam_strms= []
for cinfo in cam_dat.values():
cam_strms.append(cinfo['stream'])
x0, S0 = get_first_state(cam_strms + [hydra_strm], freq=1./hydra_strm.dt, start_time=kf_data[lr][i]['shifted_seg_start_times'])
KF = kalman()
kf_tstart = hydra_strm.get_start_time()
KF.init_filter(kf_tstart, x0, S0, motion_covar)
KF_lr.append(KF)
KFs[lr] = KF_lr
return KFs
def run_kalman_filter(T_hydra, T_ar, x_init, covar_init, ar_cov_scale, hydra_cov_scale, f=30.):
"""
Runs the kalman filter
"""
dt = 1/f
N = len(T_hydra)
## load the noise covariance matrices:
covar_mats = cPickle.load(open(hd_path + '/hd_track/data/nodup-covars-1.cpickle'))
motion_covar = covar_mats['process']
hydra_covar = hydra_cov_scale*covar_mats['hydra']
ar_covar = ar_cov_scale*covar_mats['kinect']
## initialize the filter:
KF = kalman()
KF.init_filter(0, x_init, covar_init, motion_covar, hydra_covar, ar_covar)
ts = dt * (np.arange(N)+1)
filter_estimates = [] ## the kalman filter estimates
filter_covariances = [] ## the kalman filter covariances
## run the filter:
for i in xrange(len(ts)-1):
KF.register_observationx(ts[i], T_ar[i+1], T_hydra[i+1])
filter_estimates.append(KF.x_filt)
filter_covariances.append(KF.S_filt)
A, R = KF.get_motion_mats(dt)
return filter_estimates, filter_covariances, A, R
