def transition_model(self, u, dt):
"""
Combined Turtlebot + map dynamics.
Adapt this method from EkfLocalization.transition_model().
"""
g = np.copy(self.x)
Gx = np.eye(self.x.size)
Gu = np.zeros((self.x.size, 2))
########## Code starts here ##########
# TODO: Compute g, Gx, Gu.
# HINT: This should be very similar to EkfLocalization.transition_model() and take 1-5 lines of code.
# HINT: Call tb.compute_dynamics() with the correct elements of self.x
N = self.x.size
g_l, Gx_l, Gu_l = tb.compute_dynamics(self.x[0:3], u, dt)
g[0:3] = g_l
Gx = scipy.linalg.block_diag(Gx_l, np.eye(N - 3))
Gu[0:3, 0:2] = Gu_l
########## Code ends here ##########
return g, Gx, Gu
def transition_model(self, u, dt):
"""
Turtlebot dynamics (unicycle model).
"""
########## Code starts here ##########
# TODO: Compute g, Gx, Gu using tb.compute_dynamics().
g, Gx, Gu = tb.compute_dynamics(self.x, u, dt)
########## Code ends here ##########
return g, Gx, Gu
def transition_model(self, u, dt):
"""Specifies how control input |u| updates the augmented state vector."""
g = np.copy(self.x)
Gx = np.eye(self.x.size)
Gu = np.zeros((self.x.size, 2))
########## Code starts here ##########
# TODO: Compute g, Gx, Gu.
robot_pose = self.x[:3]
g_robot, Gx_robot, Gu_robot = tb.compute_dynamics(robot_pose, u, dt)
g[:3] = g_robot
Gx[:3, :3] = Gx_robot
Gu[:3, :3] = Gu_robot
########## Code ends here ##########
return g, Gx, Gu
def transition_model(self, u, dt):
"""
Combined Turtlebot + map dynamics.
Adapt this method from EkfLocalization.transition_model().
"""
g = np.copy(self.x)
Gx = np.eye(self.x.size)
Gu = np.zeros((self.x.size, 2))
########## Code starts here ##########
# TODO: Compute g, Gx, Gu.
# HINT: This should be very similar to EkfLocalization.transition_model() and take 1-5 lines of code.
# HINT: Call tb.compute_dynamics() with the correct elements of self.x
g[:3], Gx[:3, :3], Gu[:3, :] = tb.compute_dynamics(self.x[:3], u, dt)
########## Code ends here ##########
return g, Gx, Gu
def transition_model(self, u, dt):
"""
Combined Turtlebot + map dynamics.
Adapt this method from EkfLocalization.transition_model().
"""
g = np.copy(self.x)
Gx = np.eye(self.x.size)
Gu = np.zeros((self.x.size, 2))
########## Code starts here ##########
g_tb, Gx_tb, Gu_tb = tb.compute_dynamics(self.x[:3], u, dt)
g[0:3] = g_tb
Gx[0:3, 0:3] = Gx_tb
Gu[0:3, 0:2] = Gu_tb
########## Code ends here ##########
return g, Gx, Gu
def transition_model(self, u, dt):
"""
Combined Turtlebot + map dynamics.
Adapt this method from EkfLocalization.transition_model().
"""
g = np.copy(self.x)
Gx = np.eye(self.x.size)
Gu = np.zeros((self.x.size, 2))
########## Code starts here ##########
# TODO: Compute g, Gx, Gu.
g_p, Gx_p, Gu_p = tb.compute_dynamics(self.x[:3], u, dt)
g[:3] = g_p
Gx[:3, :3] = Gx_p
Gu[:3, :] = Gu_p
########## Code ends here ##########
return g, Gx, Gu
def transition_model(self, u, dt):
"""
Turtlebot dynamics (unicycle model).
Propagates exact (nonlinear) state dynamics.
Inputs:
u: np.array[2,] - zero-order hold control input.
dt: float - duration of discrete time step.
Outputs:
g: np.array[n,] - result of belief mean propagated according to the
system dynamics with control u for dt seconds.
Gx: np.array[n,n] - Jacobian of g with respect to belief mean self.x.
Gu: np.array[n,2] - Jacobian of g with respect to control u.
"""
########## Code starts here ##########
# TODO: Compute g, Gx, Gu using tb.compute_dynamics().
g, Gx, Gu = tb.compute_dynamics(self.x, u, dt)
########## Code ends here ##########
return g, Gx, Gu
def transition_model(self, us, dt):
"""
Unicycle model dynamics.
Inputs:
us: np.array[M,2] - zero-order hold control input for each particle.
dt: float - duration of discrete time step.
Output:
g: np.array[M,3] - result of belief mean for each particle
propagated according to the system dynamics with
control u for dt seconds.
"""
########## Code starts here ##########
# TODO: Compute g.
# Hint: We don't need Jacobians for particle filtering.
# Hint: A simple solution can be using a for loop for each partical
# and a call to tb.compute_dynamics
# Hint: To maximize speed, try to compute the dynamics without looping
# over the particles. If you do this, you should implement
# vectorized versions of the dynamics computations directly here
# (instead of modifying turtlebot_model). This results in a
# ~10x speedup.
# Hint: This faster/better solution does not use loop and does
# not call tb.compute_dynamics. You need to compute the idxs
# where abs(om) > EPSILON_OMEGA and the other idxs, then do separate
# updates for them
########## Code ends here ##########
g = np.empty([self.M,3])
for i in range(self.M):
xvec = self.xs[i]
u = us[i]
g_temp, Gx, Gu = tb.compute_dynamics(xvec, u, dt)
g[i,:] = g_temp
########## Code ends here ##########
return g
