As, Bs, Cs = car.linearize_belief_dynamics_trajectory(Bel_bar, U_bar, s, Q, R)
for t in xrange(T - 1):
# Overwrites Bel_bar
#NOTE Keep making mistake in line below
# Small mismatch occurs due to dynamics noise
Bel_bar[:, t +
1] = Cs[:,
t] #As[:,:,t]*Bel_bar[:,t] + Bs[:,:,t]*np.mat(U_bar[:,t]).T + Cs[:,t]
# Apply value iteration
rho_bel = 0.1
rho_u = 0.05
N_iter = 1
goal_bel = np.copy(Bel_bar[:, -1])
goal_bel[car.NX:] = 0
opt_bels, opt_ctrls, opt_vals = value_iteration_solver(s, Bel_bar.copy(),
U_bar, Q, R, rho_bel,
rho_u, goal_bel, N_iter,
car.NX)
stop
Bel_opt = np.mat(np.copy(Bel_bar))
for t in xrange(T - 1):
Bel_opt[:, t + 1] = car.belief_dynamics(Bel_opt[:, t], opt_ctrls[:, t], s,
Q, R)
car.draw_belief_trajectory(Bel_opt, color='yellow')
plt.show()
"""
fig = plt.gcf()
s.draw_goal_state(Bel_bar[:,-1])
car.draw_trail(mat2tuple(Bel_bar[0:4,:].T))
ims = car.get_animation_artists(Bel_bar)
im_ani = animation.ArtistAnimation(fig, ims, interval=100,
blit=True)
plt.show()
stop
"""
# Apply SCP
rho_bel = 0.1
rho_u = 0.05
N_iter = 2
goal_bel = np.copy(Bel_bar[:, -1])
goal_bel[car.NX :] = 0
opt_bels, opt_ctrls, opt_vals = scp_solver_beliefs(
s, Bel_bar.copy(), U_bar, Q, R, rho_bel, rho_u, goal_bel, N_iter, method="shooting"
)
Bel_opt = np.mat(np.copy(Bel_bar))
for t in xrange(T - 1):
Bel_opt[:, t + 1] = car.belief_dynamics(Bel_opt[:, t], opt_ctrls[:, t], s, Q, R)
car.draw_belief_trajectory(Bel_opt, color="yellow")
plt.show()
