Пример #1
0
Sigmas = np.zeros((Q.shape[0], Q.shape[1], T))
Sigmas[:, :, 0] = np.mat(np.diag([0.001] * num_states))  #arg

# Generate trajectory and obtain observations along trajectory

X_bar = np.mat(np.zeros((car.NX, T)))  #arg
X_bar[:, 0] = np.mat(x0).T
U_bar = np.ones((car.NU, T - 1)) * 1.7  #arg
for t in xrange(1, T):
    U_bar[1, t - 1] = 0

for t in xrange(1, T):
    X_bar[:,t] = np.mat(car.dynamics(X_bar[:,t-1], U_bar[:, t-1])) +\
                     np.mat(dynamics_noise[:,t-1]).T
    z = car.observe(s, x=X_bar[:, t]) + np.mat(measurement_noise[:, t - 1]).T
    mus[:,
        t], Sigmas[:, :,
                   t] = ekf_update(car.dynamics, lambda x: car.observe(s, x=x),
                                   Q, R, mus[:, t - 1], Sigmas[:, :, t - 1],
                                   U_bar[:, t - 1], z)

# Plot nominal trajectory with covariance ellipses

ax = plt.gca()
plt.title('Nominal')
s.draw(ax=ax)
#print Sigmas
car.draw_trajectory(mat2tuple(X_bar.T), mus=mus, Sigmas=Sigmas[0:2, 0:2, :])

plt.show()
X_bar = np.mat(np.zeros((car.NX, T))) #arg
X_bar[:,0] = np.mat(x0).T
U_bar = np.load('data/nominal_trajectory.npy')
#U_bar = np.random.random((car.NU, T-1))
#for t in xrange(1,T):
    #U_bar[1,t-1] = (U_bar[1,t-1] - 0.5)
#np.save('data/nominal_trajectory', U_bar)
'''
X_bar = np.mat(d['dat_states'])
U_bar = np.mat(d['dat_ctrls'])

for t in xrange(1,T):
    X_bar[:,t] = np.mat(car.dynamics(X_bar[:,t-1], U_bar[:, t-1])) +\
                     np.mat(dynamics_noise[:,t-1]).T 
    mus[:,t], Sigmas[:,:,t] = ekf_update(car.dynamics,
                                         lambda x: car.observe(s, x=x),
                                         Q, R, mus[:,t-1], Sigmas[:,:,t-1],
                                         U_bar[:,t-1], None) #NOTE No obs
                                         
# Plot nominal trajectory with covariance ellipses

ax = plt.gca()
s.draw(ax=ax)
#print Sigmas
#car.draw_trajectory(mat2tuple(X_bar.T), mus=X_bar, Sigmas=Sigmas[0:2,0:2,:])

Bel_bar = np.mat(np.zeros((car.NB, T)))
for t in xrange(T):
    Bel_bar[:,t] = np.vstack((X_bar[:,t], cov2vec(Sigmas[:,:,t])))
As, Bs, Cs = car.linearize_belief_dynamics_trajectory(Bel_bar, U_bar, s, Q, R)
for t in xrange(T-1):
    # Overwrites Bel_bar
# Sigmas[2,2,0] = 0.0000001

X = np.mat(np.zeros((arm.NX, T)))
X[:, 0] = np.mat(x0).T
U = np.mat(np.zeros((arm.NU, T - 1)))

for t in range(T - 1):
    U[:, t] = du
# U[:,T/2:] = -2 * U[:,T/2:]
# U = np.mat(np.random.random_sample((arm.NU, T-1))/5)
# U[:,10:] = -2 * U[:,10:]

for t in xrange(1, T):
    X[:, t] = arm.dynamics(X[:, t - 1], U[:, t - 1])
    mus[:, t], Sigmas[:, :, t] = ekf_update(
        arm.dynamics, lambda x: arm.observe(s, x=x), Q, R, mus[:, t - 1], Sigmas[:, :, t - 1], U[:, t - 1], None
    )  # NOTE No obs

arm.draw_trajectory(X, mus, Sigmas)

Bel_bar = np.mat(np.zeros((arm.NB, T)))
for t in xrange(T):
    Bel_bar[:, t] = np.vstack((X[:, t], cov2vec(Sigmas[:, :, t])))

rho_bel = 0.2
rho_u = 0.3
N_iter = 1
goal_bel = np.copy(Bel_bar[:, -1])
goal_bel[arm.NX :] = 0

opt_bels, opt_ctrls, opt_vals = scp_solver_beliefs(
Пример #4
0
Sigmas.append(diag([.05, .05]))  # In m
Q = diag([.0, .0])  # In m
R = 100  # In pixels
u = zeros2((2, 1))  # Static dynamics/no controls

f = lambda x, u: x  # Dynamics
hs = list()  # Measurement function for each camera
zs = list()  # Simulated measurements from each camera

for cam in cams:
    hs.append(cam.project_point)
    #TODO Better measurement simulation
    zs.append(hs[-1](mus[0]) + (rand() - 0.5) * 10)

# Indexing doesn't follow convention, z_k measurement of mu_k
mu1, Sigma1 = ekf_update(f, hs[0], Q, R, mus[0], Sigmas[0], u, zs[0])
mus.append(mu1)
Sigmas.append(Sigma1)
for k in range(1, len(cams)):
    print zs[k]
    mu1, Sigma1 = ekf_update(f, hs[k], Q, R, mus[1], Sigmas[1], u, zs[k])
    mus.append(mu1)
    Sigmas.append(Sigma1)

# Display

world.display()
axes().set_xlim(-1, 1)
axes().set_ylim(-1, 1)

conf = 0.95
X = np.mat(np.zeros((localizer.NX, T)))
X[:, 0] = x0
U = np.mat(np.zeros((localizer.NU, T - 1)))

for t in range(T - 1):
    U[:, t] = du
#U[:,T/2:] = -2 * U[:,T/2:]
#U = np.mat(np.random.random_sample((arm.NU, T-1))/5)
#U[:,10:] = -2 * U[:,10:]

for t in xrange(1, T):
    X[:, t] = localizer.dynamics(X[:, t - 1], U[:, t - 1])
    mus[:, t], Sigmas[:, :,
                      t] = ekf_update(localizer.dynamics,
                                      lambda x: localizer.observe(s, x=x), Q,
                                      R, mus[:, t - 1], Sigmas[:, :, t - 1],
                                      U[:, t - 1], None)  #NOTE No obs

localizer.draw_trajectory(X, mus, Sigmas)

Bel_bar = np.mat(np.zeros((localizer.NB, T)))
for t in xrange(T):
    Bel_bar[:, t] = np.vstack((X[:, t], cov2vec(Sigmas[:, :, t])))

rho_bel = 0.1
rho_u = 0.1
N_iter = 5
goal_bel = np.copy(Bel_bar[:, -1])
goal_bel[localizer.NX:] = 0

opt_bels, opt_ctrls, opt_vals = scp_solver_beliefs(s, Bel_bar.copy(), U,\
Пример #6
0
X = np.mat(np.zeros((arm.NX, T)))
X[:, 0] = np.mat(x0).T
U = np.mat(np.zeros((arm.NU, T - 1)))

for t in range(T - 1):
    U[:, t] = du
#U[:,T/2:] = -2 * U[:,T/2:]
#U = np.mat(np.random.random_sample((arm.NU, T-1))/5)
#U[:,10:] = -2 * U[:,10:]

for t in xrange(1, T):
    X[:, t] = arm.dynamics(X[:, t - 1], U[:, t - 1])
    mus[:,
        t], Sigmas[:, :,
                   t] = ekf_update(arm.dynamics, lambda x: arm.observe(s, x=x),
                                   Q, R, mus[:, t - 1], Sigmas[:, :, t - 1],
                                   U[:, t - 1], None)  #NOTE No obs

arm.draw_trajectory(X, mus, Sigmas)

drawRobot = X
getch = _Getch._Getch()
t = 0
while True:
    c = getch()
    print c
    if c == 'x':
        break
    elif ord(c) == 3 or ord(c) == 4:
        exit(0)
    elif c == 'n':
Пример #7
0
Sigmas.append(diag([.05, .05])) # In m
Q = diag([.0, .0])              # In m
R = 100                         # In pixels
u = zeros2((2,1))               # Static dynamics/no controls

f = lambda x, u: x # Dynamics
hs = list()        # Measurement function for each camera
zs = list()        # Simulated measurements from each camera

for cam in cams:
    hs.append(cam.project_point)
    #TODO Better measurement simulation
    zs.append(hs[-1](mus[0]) + (rand()-0.5)*10)

# Indexing doesn't follow convention, z_k measurement of mu_k
mu1, Sigma1 = ekf_update(f, hs[0], Q, R, mus[0], Sigmas[0], u, zs[0])
mus.append(mu1); Sigmas.append(Sigma1)
for k in range(1, len(cams)):
   print zs[k]
   mu1, Sigma1 = ekf_update(f, hs[k], Q, R, mus[1], Sigmas[1], u, zs[k])
   mus.append(mu1); Sigmas.append(Sigma1)

# Display

world.display()
axes().set_xlim(-1, 1);
axes().set_ylim(-1, 1);

conf = 0.95

for k in xrange(len(mus)):
Sigmas[2,2,0] = 0.0000001
# Generate nominal belief trajectory

X_bar = np.mat(np.zeros((localizer.NX, T))) #arg
X_bar[:,0] = np.mat(x0).T
U_bar = np.ones((localizer.NU, T-1))*0.35
for t in xrange(1,T):
    U_bar[1,t-1] = -0.005
    
#print U_bar

for t in xrange(1,T):
    X_bar[:,t] = np.mat(localizer.dynamics(X_bar[:,t-1], U_bar[:, t-1])) +\
                     np.mat(dynamics_noise[:,t-1]).T 
    mus[:,t], Sigmas[:,:,t] = ekf_update(localizer.dynamics,
                                         lambda x: localizer.observe(s, x=x),
                                         Q, R, mus[:,t-1], Sigmas[:,:,t-1],
                                         U_bar[:,t-1], None) #NOTE No obs
                                         
# Plot nominal trajectory with covariance ellipses

ax = plt.gca()
s.draw(ax=ax)
localizer.draw_trajectory(mat2tuple(X_bar.T), mus=X_bar, Sigmas=Sigmas[0:2,0:2,:], color='yellow')
localizer.draw_trajectory([], mus=X_bar[4:6,0:1], Sigmas=Sigmas[4:6,4:6,0:1], color='yellow')
localizer.draw_trajectory([], mus=X_bar[4:6,T-2:T-1], Sigmas=Sigmas[4:6,4:6,T-2:T-1], color='yellow')

#for t in range(0,T): 
#  localizer.mark_fov(X_bar[:,t], s, [-1, 1, -1, 1], color=colors[t % len(colors)])
#plt.show()
#stop
Sigmas[:,:,0] = np.mat(np.diag([0.005]*num_states)) #arg
#Sigmas[2,2,0] = 0.0000001
#Sigmas[3,3,0] = 0.0000001

# Generate a nomimal trajectory
X_bar = np.mat(np.zeros((links.NX, T)))
X_bar[:,0] = x0;
U_bar = np.mat(np.zeros((links.NU, T-1)))
#U_bar = 10*np.mat(np.random.random_sample((links.NU, T-1))) - 5
for t in xrange(1,T):
    U_bar[0, t-1] = 1*float(t)/T 
    U_bar[1, t-1] = 1.2
    X_bar[:,t] = links.dynamics(X_bar[:,t-1], U_bar[:, t-1])
    
    mus[:,t], Sigmas[:,:,t] = ekf_update(links.dynamics,
                                         lambda x: links.observe(s, x=x),
                                         Q, R, mus[:,t-1], Sigmas[:,:,t-1],
                                         U_bar[:,t-1], None) 
    
# Plot nominal trajectory
#ax = plt.subplot(121)
ax = plt.gca()
s.draw(ax=ax)
links.draw_trajectory(mat2tuple(X_bar.T), mus=X_bar, Sigmas=Sigmas[0:2,0:2,:], color='red')
#plt.show()
#stop

Bel_bar = np.mat(np.zeros((links.NB, T)))
for t in xrange(T):
  Bel_bar[:,t] = np.vstack((X_bar[:,t], cov2vec(Sigmas[:,:,t])))

goal_bel = np.copy(Bel_bar[:,-1])
Пример #10
0
#Sigmas[2,2,0] = 0.0000001
#Sigmas[3,3,0] = 0.0000001

# Generate a nomimal trajectory
X_bar = np.mat(np.zeros((links.NX, T)))
X_bar[:, 0] = x0
U_bar = np.mat(np.zeros((links.NU, T - 1)))
#U_bar = 10*np.mat(np.random.random_sample((links.NU, T-1))) - 5
for t in xrange(1, T):
    U_bar[0, t - 1] = 1 * float(t) / T
    U_bar[1, t - 1] = 1.2
    X_bar[:, t] = links.dynamics(X_bar[:, t - 1], U_bar[:, t - 1])

    mus[:, t], Sigmas[:, :, t] = ekf_update(links.dynamics,
                                            lambda x: links.observe(s, x=x), Q,
                                            R, mus[:, t - 1], Sigmas[:, :,
                                                                     t - 1],
                                            U_bar[:, t - 1], None)

# Plot nominal trajectory
#ax = plt.subplot(121)
ax = plt.gca()
s.draw(ax=ax)
links.draw_trajectory(mat2tuple(X_bar.T),
                      mus=X_bar,
                      Sigmas=Sigmas[0:2, 0:2, :],
                      color='red')
#plt.show()
#stop

Bel_bar = np.mat(np.zeros((links.NB, T)))
R = 100                         # In pixels
u = zeros2((2,1))               # Static dynamics/no controls

f = lambda x, u: x # Dynamics
hs = list()        # Measurement function for each camera

for cam in cams:
    # Second camera will be moving aronud but h[1] should change with it
    hs.append(cam.project_point)

def sim_noise():
    #return (rand()-0.5)*10
    return 0

# Indexing doesn't follow convention, z_k measurement of mu_k
mu1, Sigma1 = ekf_update(f, hs[0], Q, R, mus[0], Sigmas[0], u, hs[0](mus[0]) +
    sim_noise())
mus.append(mu1); Sigmas.append(Sigma1)

# Discretization/search
dxy = 0.1
dtheta = .314
min_trace = inf
z1 = hs[1](mus[0]) + sim_noise()
for x in arange(-1, dxy, 1):
    for y in arange(-1, dxy, 1):
        if x == 0 and y == 0: # Object at focal pt. exception
            continue
        for theta in arange(-pi, pi, dtheta):
            if theta == 0: # Results in inf/error
                continue
            print x, y, theta
Пример #12
0
f = lambda x, u: x  # Dynamics
hs = list()  # Measurement function for each camera

for cam in cams:
    # Second camera will be moving aronud but h[1] should change with it
    hs.append(cam.project_point)


def sim_noise():
    #return (rand()-0.5)*10
    return 0


# Indexing doesn't follow convention, z_k measurement of mu_k
mu1, Sigma1 = ekf_update(f, hs[0], Q, R, mus[0], Sigmas[0], u,
                         hs[0](mus[0]) + sim_noise())
mus.append(mu1)
Sigmas.append(Sigma1)

# Discretization/search
dxy = 0.1
dtheta = .314
min_trace = inf
z1 = hs[1](mus[0]) + sim_noise()
for x in arange(-1, dxy, 1):
    for y in arange(-1, dxy, 1):
        if x == 0 and y == 0:  # Object at focal pt. exception
            continue
        for theta in arange(-pi, pi, dtheta):
            if theta == 0:  # Results in inf/error
                continue