utils = reload(import_module("utils"))
##########################
animate = Animator()
mapping = OcpGridMap()
params = utils.read_param('map_params.yaml')
# fig1, ax = plt.subplots()
#load given data on robot position
given = sio.loadmat('state_meas_data.mat')
X = given['X'] #x,y,th of the robot at each time step
z = np.nan_to_num(given['z'], np.inf) #range and bearing at each time step
thk = given['thk'] #11 angles for the laser range finders
#set up log ratios l = log(p/(1-p))
p0 = params['map0'] * np.ones((params['xlim'], params['ylim']))
l0 = np.log(p0 / (1 - p0))
lprev_i = np.log(p0 / (1 - p0))
steps = len(X[1, :])
for i in range(steps):
xt = X[:, i]
zt = z[:, :, i]
lt_i = mapping.ocp_grid_map(lprev_i, xt, zt)
pt_i = 1 - 1 / (1 + np.exp(lt_i))
lprev_i = lt_i
print('i = ', i)
animate.animator(pt_i, xt)
#
def main():
markers = 3
given = 1
rob = Rob2Wh()
animate = Animator()
t = []
vc = []
wc = []
x = []
y = []
th = []
v = []
w = []
z = []
mu = []
Sig = []
K = []
xe = []
ye = []
the = []
ve = []
we = []
x_new = rob.x0
y_new = rob.y0
th_new = rob.th0
mu_prev = np.array([[x_new], [y_new], [th_new]])
Sig_prev = np.array([[0.1, 0.0, 0.0], [0.0, 0.1, 0.0], [0.0, 0.0, 0.1]])
elements = int(rob.tf / rob.dt)
t_given = rob.ttr
x_given = rob.xtr
y_given = rob.ytr
th_given = rob.thtr
v_given = rob.vtr
w_given = rob.wtr
# z_given = np.squeeze(np.array([[rob.z_rtr],[rob.z_btr]]))
for i in range(0, elements + 1):
if given == 0:
t.append(i * rob.dt)
vc_new, wc_new = generate_command(t[i])
(x_new, y_new, th_new, v_new,
w_new) = rob.vel_motion_model(vc_new, wc_new, x_new, y_new,
th_new)
u_new = np.array([vc_new, wc_new])
z_new = rob.simulate_sensor(x_new, y_new, th_new)
else:
t.append(t_given[0][i])
vc_new, wc_new = generate_command(t[i])
x_new = x_given[0][i]
y_new = y_given[0][i]
th_new = th_given[0][i]
u_new = np.array([v_given[0][i], w_given[0][i]])
z_new = rob.simulate_sensor(x_new, y_new, th_new)
# if markers == 1:
# z_new = np.array([[z_given[0,i], 0, 0, z_given[1,i], 0, 0]]).T
# else:
# z_new = rob.simulate_sensor(x_new, y_new, th_new)
for j in range(markers):
marker = j
mu_new, Sig_new, K_new = rob.UKF(mu_prev, Sig_prev, u_new, z_new,
marker)
mu_prev = mu_new
Sig_prev = Sig_new
mu.append(mu_new)
Sig.append(Sig_new)
K.append(K_new)
x.append(x_new)
y.append(y_new)
th.append(th_new)
v.append(u_new[0])
w.append(u_new[0])
vc.append(vc_new)
wc.append(wc_new)
xe.append(mu_new[0] - x[i])
ye.append(mu_new[1] - y[i])
the.append(mu_new[2] - th[i])
ve.append(vc_new - v[i])
we.append(wc_new - w[i])
mu_prev = np.array(mu_new)
Sig_prev = np.array(Sig_new)
size = len(mu)
x_hat = []
y_hat = []
th_hat = []
sig_x = []
sig_y = []
sig_th = []
for i in range(size):
x_hat.append(mu[i][0])
y_hat.append(mu[i][1])
th_hat.append(mu[i][2])
sig_x.append(Sig[i][0][0])
sig_y.append(Sig[i][1][1])
sig_th.append(Sig[i][2][2])
animate.animator(x, y, th, x_hat, y_hat, th_hat, elements)
rob.plotting(x_hat, x, y_hat, y, th_hat, th, vc, v, wc, w,\
t, xe, ye, the, ve, we, K, sig_x, sig_y, sig_th)
return (x, y, th, z, mu, Sig)
def main():
#user inputs
markers = 3
given = 0 #set to one if data is given
#instatiate objects
rob = Rob2Wh()
animate = Animator()
mc = Monte_Carlo()
plot = Plotter()
#create lists
t = []
vc = []
wc = []
xt = []
yt = []
tht = []
zt = []
mu = []
Sig = []
xe = []
ye = []
the = []
Xk = []
#collect initial parameters
states_new = np.array([[rob.x0], [rob.y0], [rob.th0]])
elements = int(rob.tf / rob.dt)
M = rob.particles
#collect given info
if given != 0:
t_given = rob.ttr
x_given = rob.xtr
y_given = rob.ytr
th_given = rob.thtr
v_given = rob.vtr
w_given = rob.wtr
z_given = np.squeeze(np.array([[rob.z_rtr], [rob.z_btr]]))
#initialize particles
Xkt = mc.uniform_point_cloud(rob.xgrid, rob.ygrid, M)
# Xkt = np.array([[rob.x0]*M, [rob.y0]*M, [rob.th0]*M])
##loop through each time step
for i in range(0, elements + 1):
##extract truth for time step
if given == 0: #generate truth
t.append(i * rob.dt)
vc_new, wc_new = rob.generate_command(t[i])
ut = np.array([[vc_new], [wc_new]])
#propogate truth
# if i != 0:
states_new = rob.vel_motion_model(ut, states_new)
z_new = rob.simulate_sensor(states_new)
else: #get truth from given data
print('truth given')
t.append(t_given[0][i])
vc_new, wc_new = rob.generate_command(t[i])
ut = np.array([[vc_new], [wc_new]])
states_new = np.array(
[x_given[0][i], y_given[0][i], th_given[0][i]])
if markers == 1:
z_new = np.array([[z_given[0, i], 0, 0, z_given[1, i], 0,
0]]).T
else:
z_new = rob.simulate_sensor(
states_new
) #may need to change depending on the data given
Xkt = mc.monte_carlo(Xkt, ut, z_new, M, rob)
x_new = np.mean(Xkt[0, :])
y_new = np.mean(Xkt[1, :])
th_new = np.mean(Xkt[2, :])
mu_new = np.array([x_new, y_new, th_new])
Sig_new = np.cov(Xkt)
#append values to lists
mu.append(mu_new)
Sig.append(Sig_new)
xt.append(states_new[0])
yt.append(states_new[1])
tht.append(states_new[2])
xe.append(mu_new[0] - xt[i])
ye.append(mu_new[1] - yt[i])
the.append(mu_new[2] - tht[i])
Xk.append(Xkt)
#prep varialbes for plotting and animation
size = len(mu)
x_hat = []
y_hat = []
th_hat = []
sig_x = []
sig_y = []
sig_th = []
for i in range(size):
x_hat.append(mu[i][0]) #hats are the estimates
y_hat.append(mu[i][1])
th_hat.append(mu[i][2])
sig_x.append(Sig[i][0][0])
sig_y.append(Sig[i][1][1])
sig_th.append(Sig[i][2][2])
animate.animator(xt, yt, tht, x_hat, y_hat, th_hat, elements, rob, Xk)
plot.plotting(x_hat, xt, y_hat, yt, th_hat, tht,\
t, xe, ye, the, sig_x, sig_y, sig_th)
return (xt, yt, tht, zt, mu, Sig)
