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 state_mean(sigmas, w_m):
x = np.empty(3)
# This is a hack (and not a very good one) to handle periodic boundary
# crossings. Seems to help a little.
std = np.std(sigmas[:, :2])
if std > (extents[1] - extents[0]) / 4:
x[:2] = sigmas[0, :2]
else:
x[0] = np.dot(sigmas[:, 0], w_m)
x[1] = np.dot(sigmas[:, 1], w_m)
sum_sin = np.dot(np.sin(sigmas[:, 2]), w_m)
sum_cos = np.dot(np.cos(sigmas[:, 2]), w_m)
x[2] = np.arctan2(sum_sin, sum_cos)
wrap(x, extents)
return x
def move(self, u=np.zeros(2), u_noise=np.zeros(2), dt=0.1, extents=None):
"""Simulate vehicle motion from control inputs and kinematic
equations.
Parameters
----------
u : ndarray - shape (2,)
control input vector [speed (m/s), steering angle (rad)]
u_noise : ndarray - shape (2,)
Sigma parameters characterizing uncertainy on u
dt : float (optional, default 0.1)
Time step size (sec)
extents : tuple(float, float) (optional, default None)
Boundaries of the square, periodic robot world.
"""
self.u = u + np.random.randn() * u_noise
self.x, self.t = rk4(self.t, dt, self.x, self.eqs_of_motion)
if extents is not None:
self.x = wrap(self.x, extents)
def residual_x(xa, xb):
"""Subtract [x, y, phi] vectors, accounting for angle"""
dx = xa - xb
dx[2] = mpi_to_pi(dx[2])
wrap(dx, extents)
return dx
def residual_h(z, h):
"""Subtract [range, bearing] vectors, accounting for angle"""
y = z - h
y[1] = mpi_to_pi(y[1])
wrap(y, extents)
return y