def main():
dt = 0.05 # time step - 20 Hz updates
n_steps = 2000
extents = (-100, 100) # map boundaries (square map)
n_landmarks = 5
speed = 10.0 # commanded speed in m/s (1 m/s = 2.237 mph)
# Start in the middle, pointed in a random direction.
starting_pose = np.array([0, 0, np.random.uniform(0, 2 * np.pi)])
vehicle = Vehicle(
wheelbase=0.7,
center_of_mass=0.35,
initial_state=starting_pose,
range_noise=0.1, # meters
bearing_noise=0.01, # rad
sensor_range=50,
)
vehicle.t = time()
# Add landmarks to the map at random.
landmarks = np.random.uniform(*extents, size=(n_landmarks, 2))
# ukf = vehicle_ukf(vehicle, landmarks, dt)
for i in range(n_steps):
start = vehicle.t
steer_angle = np.radians(1.0 * np.sin(i / 100))
u = np.array([speed, steer_angle])
u_noise = np.array([0.1, 0.01])
vehicle.move(u=u, u_noise=u_noise, dt=dt, extents=extents)
# ukf.predict(fx_args=(u, extents))
z, in_range = vehicle.observe(landmarks)
# ukf.update(z, hx_args=(landmarks[in_range],))
vehicle.t = time()
if start + dt > vehicle.t:
sleep(start + dt - vehicle.t)
draw(
vehicle.x,
landmarks=landmarks,
observations=z,
x_extents=extents,
y_extents=extents,
)
def main():
dt = 0.5 # time step
n_steps = 2000
extents = (-100, 100) # map boundaries (square map)
speed = 20. # commanded speed in m/s (1 m/s = 2.237 mph)
N = 500
particles = []
# I tried these combinations of variances for odometry sampling:
# v = [0.1, 0.0, 0.0, 0.0] # arc
# v = [0.0, 0.0001, 0.0, 0.0] # arc
# v = [0.1, 0.0001, 0.0, 0.0] # arc
# v = [0.0, 0.0, 0.01, 0.0] # straight-ahead line
# v = [0.1, 0.0, 0.01, 0.0] # teardrop, ring, line (angle-dependent)
# v = [0.001, 0.0001, 0.001, 0.0] # banana
# v = [0.001, 0.0001, 0.01, 0.0] # textbook
v = [1e-3, 1e-5, 1e-3, 1e-3] # compact blob
# Start in the middle, pointed in a random direction.
starting_pose = np.array([0, 0, np.random.uniform(0, 2*np.pi)])
vehicle = Vehicle(wheelbase=0.7,
center_of_mass=0.35,
initial_state=starting_pose,
range_noise=0.1, # meters
bearing_noise=0.01, # rad
sensor_range=50)
vehicle.t = time()
# Use control input vector to store odometry info. u_odom is a list of N
# queue structures. There will be two states in each entry: x_{t-1} and
# x_t. Each is a state in the local frame of the vehicle (x_bar, y_bar,
# theta_bar). The starting point is taken to be known; from there, we can
# only measure changes.
u_odom = [deque([starting_pose.copy()]) for _ in range(N)]
# Advance the vehicle one step so we can get an odometry measurement.
vehicle.move(u=np.array([speed, 0]), dt=dt, extents=extents)
for j in range(N):
xbar = vehicle.x - starting_pose + u_odom[j][-1]
xbar[2] = in2pi(xbar[2])
u_odom[j].append(xbar)
particles.append(vehicle.x)
for i in range(n_steps):
steer_angle = np.radians(1.0*np.sin(0.5*i/np.pi))
x_prev = vehicle.x.copy() # Previous global state
vehicle.move(u=np.array([speed, steer_angle]), dt=dt, extents=extents)
for j in range(N):
u_odom[j].popleft()
# Simulate a state \bar{x}_t measured by odometry.
xbar = vehicle.x - x_prev + u_odom[j][-1]
xbar[2] = in2pi(xbar[2])
u_odom[j].append(xbar)
# Update particle position with a new prediction.
particles[j] = sample_x_using_odometry(particles[j], u_odom[j],
variances=v, extents=extents)
sleep(dt)
pdf = 'motion_pred' if i < 10 else None
draw(x_prev,
x_extents=extents,
y_extents=extents,
particles=particles,
fig=pdf,
)
def main():
dt = 0.05 # time step - 20 Hz updates
n_steps = 2000
extents = (-100, 100) # map boundaries (square map)
n_landmarks = 5
speed = 20.0 # commanded speed in m/s (1 m/s = 2.237 mph)
ukf_step_size = 1
# Start in the middle, pointed in a random direction.
starting_pose = np.array([0, 0, np.random.uniform(0, 2 * np.pi)])
vehicle = Vehicle(
wheelbase=0.7,
center_of_mass=0.35,
initial_state=starting_pose,
range_noise=0.01, # meters
bearing_noise=0.01, # rad
sensor_range=50,
)
vehicle.t = time()
# Add landmarks to the map at random.
landmarks = np.random.uniform(*extents, size=(n_landmarks, 2))
P0 = np.diag([50, 50, 0.01])
Q = np.diag([1e-2, 1e-2, 1e-6])
ukf = create_ukf(deepcopy(vehicle), landmarks, P0, Q, dt * ukf_step_size,
extents)
for i in range(n_steps):
start = vehicle.t
steer_angle = np.radians(1.0 * np.sin(i / 100))
u = np.array([speed, steer_angle])
zs, in_range = vehicle.observe(landmarks)
if i % ukf_step_size == 0:
# Sigma point computation sometimes fails due to ill-conditioned
# covariance matrix. Wrap in try/except block until I figure out
# the root cause.
try:
ukf.predict(u=u, extents=extents)
except np.linalg.linalg.LinAlgError:
print("prediction failed on step {} - retrying".format(i))
ukf.x = vehicle.x.copy()
ukf.P = P0
ukf.predict(u=u, extents=extents)
for (z, lm) in zip(zs, landmarks[in_range]):
ukf.update(z, landmark=lm)
vehicle.move(u=u, dt=dt, extents=extents)
vehicle.t = time()
if start + dt > vehicle.t:
sleep(start + dt - vehicle.t)
ellipses = [
covariance_ellipse(ukf.x, ukf.P, nsigma=n, nsegs=32)
for n in [1, 2, 3]
]
# pdf = 'ukf-sim' if i < 500 and i % 5 == 0 else None
pdf = None
draw(
vehicle.x,
landmarks=landmarks,
observations=zs,
x_extents=extents,
y_extents=extents,
particles=ukf.sigmas_f,
ellipses=ellipses,
fig=pdf,
)
def main():
dt = 0.05 # time step - 20 Hz updates
n_steps = 1000
extents = (-100, 100) # map boundaries (square map)
L = extents[1] - extents[0]
n_landmarks = 5
speed = 20.0 # commanded speed in m/s (1 m/s = 2.237 mph)
N = 500 # number of particles
u_noise = np.array([0.1 * speed, 0.001]) # speed noise, steering noise
# Start in the middle, pointed in a random direction.
starting_pose = np.array([0, 0, np.random.uniform(0, 2 * np.pi)])
vehicle = Vehicle(
wheelbase=0.7,
center_of_mass=0.35,
initial_state=starting_pose,
range_noise=0.1, # meters
bearing_noise=0.01, # rad
sensor_range=50,
)
vehicle.t = time()
particles = []
weights = np.full(shape=(N, ), fill_value=1 / N)
R = np.diag([vehicle.range_sigma, vehicle.bearing_sigma])
# Use control input vector to store odometry info. u_odom is a list of N
# queue structures. There will be two states in each entry: x_{t-1} and
# x_t. Each is a state in the local frame of the vehicle (x_bar, y_bar,
# theta_bar). The starting point is taken to be known; from there, we can
# only measure changes.
u_odom = [deque([starting_pose.copy()]) for _ in range(N)]
# Advance the vehicle one step so we can get an odometry measurement.
vehicle.move(u=np.array([speed, 0]), dt=dt, extents=extents)
for j in range(N):
xbar = vehicle.x - starting_pose + u_odom[j][-1]
xbar[2] = in2pi(xbar[2])
u_odom[j].append(xbar)
# Initialize particle distribution
x0 = np.array([0.1 * L, 0.1 * L, 0.01
]) * np.random.randn(3) + starting_pose
particles.append(x0)
# Add landmarks to the map at random.
landmarks = np.random.uniform(*extents, size=(n_landmarks, 2))
for i in range(n_steps):
start = vehicle.t
steer_angle = np.radians(1.0 * np.sin(i / 100))
u = np.array([speed, steer_angle])
# Advance the ground-truth pose
x_prev = vehicle.x.copy() # Previous global state
vehicle.move(u=u, u_noise=u_noise, dt=dt, extents=extents)
z, in_range = vehicle.observe(landmarks)
# predict
for j in range(N):
u_odom[j].popleft()
# Simulate a state \bar{x}_t measured by odometry.
xbar = vehicle.x - x_prev + u_odom[j][-1]
xbar[2] = in2pi(xbar[2])
u_odom[j].append(xbar)
# Update particle position with a new prediction.
particles[j] = sample_x_using_odometry(
particles[j],
u_odom[j],
variances=np.array([1e-6, 1e-6, 1e-4, 1e-4]),
extents=extents,
)
if len(z) > 0 or i % 10 == 0:
# update particle weights
update(np.array(particles), weights, z, landmarks[in_range], R)
# resample
if n_effective(weights) < N / 2:
particles = resample(particles, weights)
vehicle.t = time()
if start + dt > vehicle.t:
sleep(start + dt - vehicle.t)
# pdf = 'pf-sim' if i < 10 else None
pdf = None
draw(
vehicle.x,
landmarks=landmarks,
observations=z,
x_extents=extents,
y_extents=extents,
particles=particles,
weights=weights,
fig=pdf,
)