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