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