def inner_expectation(true_dict, est_dict_ls, Sigma, prior_marg):
if not isinstance(est_dict_ls, list):
raise TypeError(
"Input est_dict_ls must be a list of dictionary object. Your input object is a {}"
.format(type(est_dict_ls)))
if len(true_dict.values()) != 1:
raise ValueError(
"For each timestamp, the true trajectory dictionary is fixed. Check the dimension first!"
)
s_obj = true_dict.values()[0]
gaussian_ls = []
posterior = []
for i, est_dict in enumerate(est_dict_ls):
gaussian_obj = 1
gaussian_sum = []
for obj in est_dict:
r = est_dict[obj]['r']
theta = est_dict[obj]['rotation']
d = (r, theta)
r_s = s_obj[obj]['r']
theta_s = s_obj[obj]['rotation']
s = (r_s, theta_s)
error = gaussian(s, d, Sigma)
gaussian_sum.append(error)
gaussian_ls.append(np.mean(gaussian_sum))
gaussian_obj *= np.prod(softmax(gaussian_sum))
posterior.append(gaussian_obj * prior_marg[i])
return np.average(gaussian_ls, weights=prior_marg), posterior
def get_likelihood(true_dict, est_dict, Sigma):
s_obj = true_dict.values()[0]
error = []
for obj in est_dict:
r_list = est_dict[obj]['r']
theta_list = est_dict[obj]['rotation']
d_list = [(r_list[i], theta_list[i]) for i in range(len(r_list))]
r_s_list = s_obj[obj]['r']
theta_s_list = s_obj[obj]['rotation']
s_list = [(r_s_list[i], theta_s_list[i]) for i in range(len(r_s_list))]
errors = [gaussian(s_list[i], d_list[i], Sigma) for i in range(len(s_list))]
error.append(np.prod(errors)) # likelihood over T
return np.mean(error) # average over four objects
def __init__(self, plant, mu0, S0, nc):
self.maxU = np.array([10])
self.param = OrderedDict()
self.fcn = 0
self.nigp = 0
(mm, ss, cc) = gTrig(mu0, S0, plant.angi, 3)
mm = np.vstack([mu0, mm])
cc = S0.dot(cc)
ss = np.vstack([np.hstack([S0, cc]), np.hstack([cc.T, ss])])
self.param['hyp'] = np.log(np.array([[1, 1, 1, 0.7, 0.7, 1, 0.01]])).T
self.param['inputs'] = gaussian(mm[poli], ss[poli, :][:, poli], nc).T
# self.param['inputs'] = 1 * np.sin(np.arange(1, 51).reshape([10, 5], order='F'))
# self.param['targets'] = 0.1*np.ones([nc,np.size(self.maxU)])#for debug
# self.param['targets'] = 1 * np.sin(np.arange(1, 1 + nc * len(self.maxU)).reshape([nc, len(self.maxU)], order='F')) # for debug
self.param['targets'] = 0.1 * np.random.randn(nc, np.size(self.maxU))
def createTargets(locations, gaussian=False): spaces = [] for i in range(locations): space_i = np.zeros(locations) space_i[i] = 1 if locations >= 4: if gaussian: for j in range(1,3): gauss = util.gaussian(j,0,1) space_i[(i+j) % locations] = gauss space_i[(i-j) % locations] = gauss spaces.append(space_i) targets = [] #now we have to generate the targets for i in range(locations): rads = (float(i) / float(locations)) * 2 * math.pi dx = math.cos(rads) dy = math.sin(rads) # store the deviation from the center in our two x coor variables if dx >= 0: dxPlus = dx dxMinus = 0 else: dxPlus = 0 dxMinus = abs(dx) if dy >= 0: dyPlus = dy dyMinus = 0 else: dyPlus = 0 dyMinus = abs(dy) targets.append([dxPlus, dxMinus, dyPlus, dyMinus]) # print targets[-1] # space.printSpace(i) return targets, spaces
def applyController(HH, policy, plant, cost, mu0, S0):
[xx, yy, realCost, latent] = rollout(gaussian(mu0, S0), policy, HH, plant,
cost)
return xx, yy, realCost, latent
[xx, yy, realCost[j], latent[j]] = applyController(H, policy, plant, cost,
mu0, S0)
# シミュレーション結果を、スタックに積んでいく
x = np.vstack([x, xx])
y = np.vstack([y, yy])
# ロボットの動作結果設定
drawer.setLatent(latent[j])
# 初回実行(乱数で実行)
x = 0
y = 0
for jj in range(J):
[xx, yy, realCost[jj], latent[jj]] = rollout(gaussian(mu0, S0), policy, H,
plant, cost)
if x == 0:
x = np.empty((0, np.size(xx, 1)))
y = np.empty((0, np.size(yy, 1)))
x = np.vstack([x, xx])
y = np.vstack([y, yy])
# 制御入力を計算する関数を設定
policy.fcn = conCat
# 描画開始
drawer.setIdleFunc(learn) # 描画していないときに実行する関数
drawer.setLatent(latent[0]) # ロボットの動作結果設定
drawer.main() # 描画開始