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)