data = sio.loadmat(f)
imu = data['imu']
ts = data['ts'][0]
gt = data['gt']
trigger_ind = data['gt_idx'][0]
ins = INS(imu, sigma_a=0.00098, sigma_w=8.7266463e-5,
T=1.0 / 200) #microstrain
for i in range(0, len(det_list)): #Iterate through detector list
for j in range(0,
len(thresh_list[i])): #iterate through threshold list
if load_traj != True:
zv = ins.Localizer.compute_zv_lrt(W=W_list[i],
G=thresh_list[i][j],
detector=det_list[i])
x = ins.baseline(zv=zv)
saved_trajectories["{}_{}_{}_det_{}_G_{}".format(
trial_type, person, folder, det_list[i],
thresh_list[i][j])] = x
else:
x = stored_trajectories["{}_{}_{}_det_{}_G_{}".format(
trial_type, person, folder, det_list[i],
thresh_list[i][j])]
x, gt = align_plots(x,
gt,
dist=0.8,
use_totstat=True,
align_idx=trigger_ind[1]) #rotate data
###Calculate ARMSE between estimate and Vicon
armse_3d = compute_error(x[trigger_ind], gt, '3d')
error_logger.update(armse_3d, trial_type, person, det_list[i], j)
imu = data['imu']
ts = data['ts'][0]
gt = data['gt']
ins = INS(imu, sigma_a=0.00098, sigma_w=8.7266463e-5, T=1.0 / 200)
###Optimize zero-velocity threshold for given trial
#G_opt_shoe, _, zv_opt_shoe = optimize_gamma(ins, gt, thresh=[0.5e7, 10e7], W=5, detector='shoe')
#G_opt_ared, _, zv_opt_shoe = optimize_gamma(ins, gt, thresh=[0.1, 2], W=5, detector='ared')
###load the pre-computed optimal thresholds
G_opt_shoe = float(data['G_shoe_opt'])
G_opt_ared = float(data['G_ared_opt'])
###Estimate trajectory
x_shoe = ins.baseline(W=5, G=G_opt_shoe, detector='shoe')
x_ared = ins.baseline(W=5, G=G_opt_ared, detector='ared')
x_shoe, _ = align_plots(x_shoe, gt) #rotate data
x_ared, _ = align_plots(x_ared, gt)
visualize.plot_topdown([x_shoe, x_ared, gt])
plt.figure()
plt.plot(data['zv_shoe_opt'][0])
###Calculate ARMSE between estimate and Vicon
shoe_error = compute_error(x_shoe, gt, '2d')
ared_error = compute_error(x_ared, gt, '2d')
print("ARMSE for ARED: {}".format(ared_error))
print("ARMSE for SHOE: {}".format(shoe_error))
best_detector = data['best_detector'][0]
best_detector_count[best_detector] += 1
###add custom detector and its zv output to lists:
det_list = ['shoe'] #['shoe', 'ared', 'amvd', 'mbgtd', 'vicon']
zv_list = [
zv_shoe_opt, zv_ared_opt, zv_amvd_opt, zv_mbgtd_opt, zv_vicon_opt
]
ins = INS(imu, sigma_a=0.00098, sigma_w=8.7266463e-5,
T=1.0 / 200) #microstrain
###Estimate trajectory
for i in range(0, len(det_list)):
if load_traj != False:
x = ins.baseline(zv=zv_list[i])
print(x.shape)
x, gt = align_plots(x, gt) #rotate data
saved_trajectories['{}_{}'.format(trial_name, det_list[i])] = x
else:
x = stored_trajectories['{}_{}'.format(trial_name, det_list[i])]
###Calculate ARMSE between estimate and Vicon
armse_2d = compute_error(x, gt, '2d')
armse_3d = compute_error(x, gt, '3d')
# print("ARMSE for {}: {}".format(det_list[i], armse_2d))
trial_stats.append(armse_2d)
trial_stats.append(armse_3d)
stats.append(trial_stats)
print("ROS Bag Demo: " + fileName)
imu = ros_data[:, 1:]
timeStep = 1.0 / 40
print("Input shape: ", imu.shape)
ins = INS(imu, sigma_a=0.00098, sigma_w=8.7266463e-5, T=timeStep)
detector = "shoe"
if (detector == "shoe"):
# load the pre-computed optimal thresholds
G_opt_shoe = 2.5e8
#Estimate trajectory
x_out = ins.baseline(W=5, G=G_opt_shoe, detector='shoe')
elif (detector == "ared"):
# load the pre-computed optimal thresholds
G_opt_ared = 1.5000000000000004
#Estimate trajectory
x_out = ins.baseline(W=5, G=G_opt_ared, detector='ared')
elif (detector == "lstm"):
zv_lstm = ins.Localizer.LSTM()
x_out = ins.baseline(zv=zv_lstm)
else:
print("Invalid detector")
exit