cls_enable = True
ML_update = True
cls_prior = [0.5, 0.5]
sem_exp = 0.3
sem_cov = 0.5
# Models
np.set_printoptions(precision=3)
plt.rcParams.update({'font.size': 16})
da_model = damodel.DAModel(camera_fov_angle_horizontal=opening_angle_rad, range_limit=measurement_radius_limit,
range_minimum=measurement_radius_minimum)
geo_model_noise_diag = np.array([0.00001, 0.00001, 0.001, 0.01, 0.01, 0.00001])
geo_model_noise = gtsam.noiseModel.Diagonal.Variances(geo_model_noise_diag)
geo_model = geomodel.GeoModel(geo_model_noise)
cls_model = clsUmodel.ClsModel()
# Robot priors
prior = gtsam.Pose3(gtsam.Pose2(-2.0, 0.0, 0.0))
prior_noise_diag = np.array([00.000001, 0.000001, 0.0000038, 00.00002, 00.0000202, 0.000001])
prior_noise = gtsam.noiseModel.Diagonal.Variances(prior_noise_diag)
# Action list
actions = list()
action_noise_diag = np.array([0.0003, 0.0003, 0.01, 0.03, 0.030, 0.001])
action_noise = gtsam.noiseModel.Diagonal.Variances(action_noise_diag)
actions.append(gtsam.Pose3(gtsam.Rot3.Ypr(0.0, 0.0, 0.0), np.array([0.25, 0.0, 0.0])))
actions.append(gtsam.Pose3(gtsam.Rot3.Ypr(0.0, 0.0, 0.0), np.array([0.25, 0.0, 0.0])))
actions.append(gtsam.Pose3(gtsam.Rot3.Ypr(0.04, 0.0, 0.0), np.array([0.25, 0.0, 0.0])))
actions.append(gtsam.Pose3(gtsam.Rot3.Ypr(-0.04, 0.0, 0.0), np.array([0.25, 0.0, 0.0])))
[special.digamma(prior_lambda[0]) - special.digamma(prior_lambda[1])]) #
lambda_prior_noise_diag = np.matrix(
special.polygamma(1, prior_lambda[0]) +
special.polygamma(1, prior_lambda[1]))
lambda_prior_noise = gtsam.noiseModel.Diagonal.Variances(
lambda_prior_noise_diag)
da_model = damodel.DAModel(camera_fov_angle_horizontal=opening_angle_rad,
range_limit=measurement_radius_limit,
range_minimum=measurement_radius_minimum)
geo_model_noise_diag = np.array([0.00001, 0.00001, 0.001, 0.01, 0.01, 0.001])
geo_model_noise = gtsam.noiseModel.Diagonal.Variances(geo_model_noise_diag)
geo_model = geomodel_proj.GeoModel(geo_model_noise)
if Lambda_BSP_mode == 1:
import clsmodel_lg1
cls_model = clsmodel_lg1.ClsModel()
if Lambda_BSP_mode == 2:
import clsUmodel_fake_1d
cls_model = clsUmodel_fake_1d.JLPModel()
# DEFINE GROUND TRUTH -------------------------------------------------
# Objects
GT_objects = list()
GT_objects.append(
gtsam.Pose3(gtsam.Rot3.Ypr(0.0, 0.0, 0.0), np.array([0.0, -2.0, 0.0])))
# GT_objects.append(gtsam.Pose3(gtsam.Rot3.Ypr(-math.pi/2, 0.0, 0.0), np.array([1.0, 2.0, 0.0])))
# GT_objects.append(gtsam.Pose3(gtsam.Rot3.Ypr(math.pi/2, 0.0, 0.0), np.array([-2.5, 3.5, 0.0])))
# GT_objects.append(gtsam.Pose3(gtsam.Rot3.Ypr(-3*math.pi/4, 0.0, 0.0), np.array([1.0, 4.0, 0.0])))
GT_objects.append(
gtsam.Pose3(gtsam.Rot3.Ypr(0.0, 0.0, 0.0), np.array([-6.0, -3.0, 0.0])))
# GT_objects.append(gtsam.Pose3(gtsam.Rot3.Ypr(math.pi/4, 0.0, 0.0), np.array([3.0, -2.0, 0.0])))
GT_cls_realization = ((1, 1), (2, 2), (3, 1), (4, 1), (5, 1), (6, 2), (7, 1),
(8, 1))
np.random.seed(3)
measurements_enabler = True
# Set models ---------------------------------------------------------
np.set_printoptions(precision=3)
plt.rcParams.update({'font.size': 16})
cls_prior = np.array([0.5, 0.5])
da_model = damodel.DAModel()
geo_model_noise_diag = np.array([0.00001, 0.00001, 0.01, 0.1, 0.1, 0.00001])
geo_model_noise = gtsam.noiseModel.Diagonal.Variances(geo_model_noise_diag)
geo_model = geomodel_proj.GeoModel(geo_model_noise)
cls_model = clsmodel.ClsModel()
# DEFINE GROUND TRUTH -------------------------------------------------
# Objects
GT_objects = list()
GT_objects.append(
gtsam.Pose3(gtsam.Rot3.Ypr(3 * math.pi / 8, 0.0, 0.0),
np.array([0.0, -2.0, 0.0])))
GT_objects.append(
gtsam.Pose3(gtsam.Rot3.Ypr(math.pi / 2, 0.0, 0.0),
np.array([1.0, 2.0, 0.0])))
GT_objects.append(
gtsam.Pose3(gtsam.Rot3.Ypr(-math.pi / 2, 0.0, 0.0),
np.array([-2.5, 3.5, 0.0])))
GT_objects.append(
def main():
number_of_seeds = 10
number_of_beliefs = 5
seed_offset = 270
cls_model_1 = clsmodel_lg1.ClsModel()
cls_model_2 = clsUmodel_fake_1d.JLPModel()
random_object_flag = True
if number_of_seeds == 1:
fixed_stat = 'Fixed'
else:
fixed_stat = 'Stat'
JLP_entropy_R1 = list()
JLP_MSDE_R1 = list()
JLP_sigma_R1 = list()
JLP_entropy_R2 = list()
JLP_MSDE_R2 = list()
JLP_sigma_R2 = list()
MH_entropy_R1 = list()
MH_MSDE_R1 = list()
MH_sigma_R1 = list()
MH_entropy_R2 = list()
MH_MSDE_R2 = list()
MH_sigma_R2 = list()
WEU_entropy = list()
WEU_MSDE = list()
time_dict = {'JLP_R1': list(), 'MH_R1': list(), 'JLP_R2': list(), 'MH_R2': list(), 'WEU': list()}
for seed_idx in range(number_of_seeds):
# JLP planning lambda entropy
Lambda_BSP_mode = 2
reward_mode = 1
entropy, MSDE, sigma, time, _ = motion_primitive_planning(seed_idx + seed_offset, Lambda_BSP_mode, reward_mode,
cls_model_2,
number_of_beliefs=number_of_beliefs,
random_objects=random_object_flag)
JLP_entropy_R1.append(entropy)
JLP_MSDE_R1.append(MSDE)
JLP_sigma_R1.append(sigma)
time_dict['JLP_R1'].append(time)
if seed_idx == 0:
JLP_entropy_exp_R1 = np.array(entropy) / number_of_seeds
JLP_MSDE_exp_R1 = np.array(MSDE) / number_of_seeds
JLP_sigma_exp_R1 = np.array(sigma) / number_of_seeds
else:
JLP_entropy_exp_R1 += np.array(entropy) / number_of_seeds
JLP_MSDE_exp_R1 += np.array(MSDE) / number_of_seeds
JLP_sigma_exp_R1 += np.array(sigma) / number_of_seeds
# JLP planning lambda expectation entropy
Lambda_BSP_mode = 2
reward_mode = 2
entropy, MSDE, sigma, time, _ = motion_primitive_planning(seed_idx + seed_offset, Lambda_BSP_mode, reward_mode,
cls_model_2,
number_of_beliefs=number_of_beliefs,
random_objects=random_object_flag)
JLP_entropy_R2.append(entropy)
JLP_MSDE_R2.append(MSDE)
JLP_sigma_R2.append(sigma)
time_dict['JLP_R2'].append(time)
if seed_idx == 0:
JLP_entropy_exp_R2 = np.array(entropy) / number_of_seeds
JLP_MSDE_exp_R2 = np.array(MSDE) / number_of_seeds
JLP_sigma_exp_R2 = np.array(sigma) / number_of_seeds
else:
JLP_entropy_exp_R2 += np.array(entropy) / number_of_seeds
JLP_MSDE_exp_R2 += np.array(MSDE) / number_of_seeds
JLP_sigma_exp_R2 += np.array(sigma) / number_of_seeds
# MH planning lambda entropy
Lambda_BSP_mode = 1
reward_mode = 1
entropy, MSDE, sigma, time, _ = motion_primitive_planning(seed_idx + seed_offset, Lambda_BSP_mode, reward_mode,
cls_model_1,
number_of_beliefs=number_of_beliefs,
random_objects=random_object_flag)
MH_entropy_R1.append(entropy)
MH_MSDE_R1.append(MSDE)
MH_sigma_R1.append(sigma)
time_dict['MH_R1'].append(time)
if seed_idx == 0:
MH_entropy_exp_R1 = np.array(entropy) / number_of_seeds
MH_MSDE_exp_R1 = np.array(MSDE) / number_of_seeds
MH_sigma_exp_R1 = np.array(sigma) / number_of_seeds
else:
MH_entropy_exp_R1 += np.array(entropy) / number_of_seeds
MH_MSDE_exp_R1 += np.array(MSDE) / number_of_seeds
MH_sigma_exp_R1 += np.array(sigma) / number_of_seeds
# MH planning lambda expectation entropy
Lambda_BSP_mode = 1
reward_mode = 2
entropy, MSDE, sigma, time, _ = motion_primitive_planning(seed_idx + seed_offset, Lambda_BSP_mode, reward_mode,
cls_model_1,
number_of_beliefs=number_of_beliefs,
random_objects=random_object_flag)
MH_entropy_R2.append(entropy)
MH_MSDE_R2.append(MSDE)
MH_sigma_R2.append(sigma)
time_dict['MH_R2'].append(time)
if seed_idx == 0:
MH_entropy_exp_R2 = np.array(entropy) / number_of_seeds
MH_MSDE_exp_R2 = np.array(MSDE) / number_of_seeds
MH_sigma_exp_R2 = np.array(sigma) / number_of_seeds
else:
MH_entropy_exp_R2 += np.array(entropy) / number_of_seeds
MH_MSDE_exp_R2 += np.array(MSDE) / number_of_seeds
MH_sigma_exp_R2 += np.array(sigma) / number_of_seeds
# Patten18arj like planning
entropy, MSDE, _, time, _ = motion_primitive_planning(seed_idx + seed_offset, Lambda_BSP_mode, reward_mode,
cls_model_1,
number_of_beliefs=1,
random_objects=random_object_flag)
WEU_entropy.append(entropy)
WEU_MSDE.append(MSDE)
time_dict['WEU'].append(time)
if seed_idx == 0:
WEU_MSDE_exp = np.array(MSDE) / number_of_seeds
else:
WEU_MSDE_exp += np.array(MSDE) / number_of_seeds
time_dict_track = {'5bel_MH':None, '10bel_MH':None, '15bel_MH':None, '20bel_MH':None, '25bel_MH':None, 'JLP':None,
'WEU':None}
for time_idx in range(5):
number_of_beliefs_time = (time_idx + 1) * 5
name = str(number_of_beliefs_time) + 'bel_MH'
_, _, _, _, time_dict_track[name] = motion_primitive_planning(seed_idx, 1, 1,
cls_model_1,
number_of_beliefs=number_of_beliefs_time,
random_objects=random_object_flag)
_, _, _, _, time_dict_track['JLP'] = motion_primitive_planning(seed_idx, 2, 1,
cls_model_2,
number_of_beliefs=number_of_beliefs_time,
random_objects=random_object_flag)
_, _, _, _, time_dict_track['WEU'] = motion_primitive_planning(seed_idx, 1, 1,
cls_model_1,
number_of_beliefs=1,
random_objects=random_object_flag)
fig = plt.figure(000)
for idx in range(5):
name = str((idx + 1) * 5) + 'bel_MH'
plt.plot(np.array(range(len(JLP_entropy_exp_R1))), time_dict_track[name], color='red', label='JLP')
plt.text(len(time_dict_track[name]) - 2, time_dict_track[name][-1], r'$N_b$=' + str((idx + 1) * 5))
plt.plot(np.array(range(len(JLP_entropy_exp_R1))), time_dict_track['JLP'], color='black', label='MH')
plt.text(len(time_dict_track['JLP']) - 2, time_dict_track['JLP'][-1] -1, 'JLP')
plt.plot(np.array(range(len(JLP_entropy_exp_R1))), time_dict_track['WEU'], color='green', label='JLP')
plt.text(len(time_dict_track['WEU']) - 2, time_dict_track['WEU'][-1], r'$N_b$=1')
#plt.title('Computation time of different methods with different number of beliefs')
plt.xlabel('Time step')
plt.ylabel('Time [s]')
#plt.yscale('log')
plt.grid(True)
plt.tight_layout()
plt.xlim(1, 20)
plt.savefig('../figures/Inf_' + fixed_stat + '_Time.eps')
plt.show()
fig = plt.figure(100)
for idx in range(number_of_seeds):
plt.plot(JLP_entropy_R1[idx], color='black', alpha=1 / number_of_seeds, label='JLP')
plt.plot(MH_entropy_R1[idx], color='purple', alpha=1 / number_of_seeds, label='MH')
plt.plot(JLP_entropy_R2[idx], color='blue', alpha=1 / number_of_seeds, label='JLP')
plt.plot(MH_entropy_R2[idx], color='red', alpha=1 / number_of_seeds, label='MH')
plt.title(r'Entropy reward of $b[\lambda]$ over time')
plt.xlabel('Time step')
plt.ylabel('Reward')
plt.grid(True)
plt.savefig('../figures/Inf_Fixed_R1.eps')
plt.show()
JLP_entropy_sigma_R1 = compute_sigma_vec(JLP_entropy_R1, JLP_entropy_exp_R1)
MH_entropy_sigma_R1 = compute_sigma_vec(MH_entropy_R1, MH_entropy_exp_R1)
JLP_entropy_sigma_R2 = compute_sigma_vec(JLP_entropy_R2, JLP_entropy_exp_R2)
MH_entropy_sigma_R2 = compute_sigma_vec(MH_entropy_R2, MH_entropy_exp_R2)
fig = plt.figure(101)
plt.plot(JLP_entropy_exp_R1, color='black', label='JLP')
plt.fill_between(np.array(range(len(JLP_entropy_exp_R1))), JLP_entropy_exp_R1 - JLP_entropy_sigma_R1, JLP_entropy_exp_R1 +
JLP_entropy_sigma_R1, color='black', alpha=0.2)
plt.plot(MH_entropy_exp_R1, color='purple', label='MH')
plt.fill_between(np.array(range(len(JLP_entropy_exp_R1))), MH_entropy_exp_R1 - MH_entropy_sigma_R1, MH_entropy_exp_R1 +
MH_entropy_sigma_R1, color='purple', alpha=0.2)
plt.plot(JLP_entropy_exp_R2, color='blue', label='JLP')
plt.fill_between(np.array(range(len(JLP_entropy_exp_R1))), JLP_entropy_exp_R2 - JLP_entropy_sigma_R2, JLP_entropy_exp_R2 +
JLP_entropy_sigma_R2, color='blue', alpha=0.2)
plt.plot(MH_entropy_exp_R2, color='red', label='MH')
plt.fill_between(np.array(range(len(JLP_entropy_exp_R1))), MH_entropy_exp_R2 - MH_entropy_sigma_R2, MH_entropy_exp_R2 +
MH_entropy_sigma_R2, color='red', alpha=0.2)
#plt.title(r'Average entropy of $b[\lambda]$ over time')
plt.xlabel('Time step')
plt.ylabel('Reward')
plt.yscale('log')
plt.tight_layout()
plt.grid(True)
plt.xlim(0, 20)
plt.savefig('../figures/Inf_Stat_R1.eps')
plt.show()
fig = plt.figure(200)
for idx in range(number_of_seeds):
#plt.plot(JLP_MSDE_R1[idx], color='black', alpha=1 / number_of_seeds, label='JLP')
#plt.plot(MH_MSDE_R1[idx], color='purple', alpha=1 / number_of_seeds, label='MH')
plt.plot(JLP_MSDE_R2[idx], color='blue', alpha=1 / number_of_seeds, label='JLP')
plt.plot(MH_MSDE_R2[idx], color='red', alpha=1 / number_of_seeds, label='MH')
plt.plot(WEU_MSDE[idx], color='green', alpha=1 / number_of_seeds, label='WEU')
plt.title('Average MSDE over time')
plt.xlabel('Time step')
plt.ylabel('MSDE')
plt.legend(loc='lower left')
plt.ylim(0, 1)
plt.xlim(0, 20)
plt.grid(True)
plt.savefig('../figures/Inf_Fixed_MSDE.eps')
plt.show()
JLP_MSDE_sigma_R1 = compute_sigma_vec(JLP_MSDE_R1, JLP_MSDE_exp_R1)
MH_MSDE_sigma_R1 = compute_sigma_vec(MH_MSDE_R1, MH_MSDE_exp_R1)
JLP_MSDE_sigma_R2 = compute_sigma_vec(JLP_MSDE_R2, JLP_MSDE_exp_R2)
MH_MSDE_sigma_R2 = compute_sigma_vec(MH_MSDE_R2, MH_MSDE_exp_R2)
WEU_MSDE_sigma = compute_sigma_vec(WEU_MSDE, WEU_MSDE_exp)
fig = plt.figure(201)
# plt.plot(np.array(range(len(JLP_entropy_exp_R1))), JLP_MSDE_exp_R1, color='black', label='JLP')
# plt.fill_between(np.array(range(len(JLP_entropy_exp_R1))), JLP_MSDE_exp_R1 - JLP_MSDE_sigma_R1, JLP_MSDE_exp_R1 +
# JLP_MSDE_sigma_R1, color='black', alpha=0.2)
# plt.plot(np.array(range(len(JLP_entropy_exp_R1))), MH_MSDE_exp_R1, color='purple', label='MH')
# plt.fill_between(np.array(range(len(JLP_entropy_exp_R1))), MH_MSDE_exp_R1 - MH_MSDE_sigma_R1, MH_MSDE_exp_R1 +
# MH_MSDE_sigma_R1, color='purple', alpha=0.2)
plt.plot(np.array(range(len(JLP_entropy_exp_R1))), JLP_MSDE_exp_R2, color='blue', label='JLP')
plt.fill_between(np.array(range(len(JLP_entropy_exp_R1))), JLP_MSDE_exp_R2 - JLP_MSDE_sigma_R2, JLP_MSDE_exp_R2 +
JLP_MSDE_sigma_R2, color='blue', alpha=0.2)
plt.plot(np.array(range(len(JLP_entropy_exp_R1))), MH_MSDE_exp_R2, color='red', label='MH')
plt.fill_between(np.array(range(len(JLP_entropy_exp_R1))), MH_MSDE_exp_R2 - MH_MSDE_sigma_R2, MH_MSDE_exp_R2 +
MH_MSDE_sigma_R2, color='red', alpha=0.2)
plt.plot(np.array(range(len(JLP_entropy_exp_R1))), WEU_MSDE_exp, color='green', label='WEU')
plt.fill_between(np.array(range(len(JLP_entropy_exp_R1))), WEU_MSDE_exp - WEU_MSDE_sigma, WEU_MSDE_exp +
WEU_MSDE_sigma, color='green', alpha=0.2)
#plt.title('Average MSDE over time')
plt.xlabel('Time step')
plt.ylabel('MSDE')
#plt.yscale('log')
plt.legend(loc='lower left')
plt.ylim(0, 0.4)
plt.xlim(0, 20)
plt.grid(True)
plt.savefig('../figures/Inf_Stat_MSDE.eps')
plt.show()
fig = plt.figure(400)
idx_time = 0
key_color = {'JLP_R1': 'black', 'MH_R1': 'purple', 'JLP_R2': 'blue','MH_R2': 'red', 'WEU': 'green'}
for key in time_dict:
idx_time += 1
plt.plot(np.repeat(idx_time, number_of_seeds), time_dict[key], '*', color=key_color[key])
#plt.title('Scenario time')
plt.xticks(np.arange(5) + 1, time_dict.keys())
plt.ylabel('Time [sec]')
plt.grid(True)
plt.show()