def sample_x_using_odometry(x,
u,
variances=[0.01, 0.01, 0.01, 0.01],
extents=None):
"""Sample from p(x_t | u_t, x_{t-1}) where u is not a control vector, but
a measurement of the current and previous states in local coordinates:
u = [\bar{x}_{t-1}, \bar{x}_t]^T
"""
xt = np.empty(3) # output vector: sampled global pose
x_prev, y_prev, theta_prev = x # global pose at time t-1
du = u[1] - u[0]
if extents is not None:
du = wrap(du, extents)
dx, dy, d_theta = du
a1, a2, a3, a4 = variances
drot1 = in2pi(np.arctan2(dy, dx) - u[0][2])
# drot1 = in2pi(np.arctan2(dy, dx) - theta_prev)
dtran = np.sqrt(dx**2 + dy**2)
drot2 = in2pi(d_theta - drot1)
dr1 = drot1 - np.sqrt(a1 * drot1**2 + a2 * dtran**2) * randn()
dtr = dtran - np.sqrt(a3 * dtran**2 + a4 * (drot1**2 + drot2**2)) * randn()
dr2 = drot2 - np.sqrt(a1 * drot2**2 + a2 * dtran**2) * randn()
xt[0] = x_prev + dtr * np.cos(theta_prev + dr1)
xt[1] = y_prev + dtr * np.sin(theta_prev + dr1)
xt[2] = in2pi(theta_prev + dr1 + dr2)
if extents is not None:
xt = wrap(xt, extents)
return xt
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 = 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,
)