x_hist.append(state) mu_hist.append(ekf.mu[:3]) err = state - ekf.mu[:3] err[2] = unwrap(err[2]) err_hist.append(err) x_covar_hist.append(ekf.Sigma[0, 0]) y_covar_hist.append(ekf.Sigma[1, 1]) psi_covar_hist.append(ekf.Sigma[2, 2]) Car.animateCar(state, ekf.mu[:2], dead_reckon, ekf.mu[3:], ekf.Sigma[3:, 3:], ekf.lms_found) plt.pause(0.02) state = ekf.propagateState(state, v[i], w[i]) zt, lm_ind = getMeasurements(state) ekf.update(zt, lm_ind, vc[i], wc[i]) dead_reckon = ekf.propagateState(dead_reckon, vc[i], wc[i]) fig1, ax1 = plt.subplots(nrows=3, ncols=1, sharex=True) x_hist = np.array(x_hist).T mu_hist = np.array(mu_hist).T ax1[0].plot(t, x_hist[0, :], label="Truth") ax1[0].plot(t, mu_hist[0, :], label="Est") ax1[0].set_ylabel("x (m)") ax1[0].legend() ax1[1].plot(t, x_hist[1, :], label="Truth") ax1[1].plot(t, mu_hist[1, :], label="Est") ax1[1].set_ylabel("y (m)") ax1[1].legend() ax1[2].plot(t, x_hist[2, :], label="Truth") ax1[2].plot(t, mu_hist[2, :], label="Est")
#stuff for plotting x_hist.append(state) mu_hist.append(mu) err = state - mu err[2] = unwrap(err[2]) err_hist.append(err) x_covar_hist.append(Sigma[0, 0]) y_covar_hist.append(Sigma[1, 1]) psi_covar_hist.append(Sigma[2, 2]) Car.animateCar(state, mu, dead_reckon) plt.pause(0.02) state = ekf.propagateState(state, v[i + 1], w[i + 1]) zt = getMeasurements(state) mu, Sigma, K = ekf.update(mu, zt, vc[i + 1], wc[i + 1]) dead_reckon = ekf.propagateState(dead_reckon, vc[i + 1], wc[i + 1]) K_hist.append(K) t = t[1:] fig1, ax1 = plt.subplots(nrows=3, ncols=1, sharex=True) x_hist = np.array(x_hist).T mu_hist = np.array(mu_hist).T ax1[0].plot(t, x_hist[0, :], label="Truth") ax1[0].plot(t, mu_hist[0, :], label="Est") ax1[0].set_ylabel("x (m)") ax1[0].legend() ax1[1].plot(t, x_hist[1, :], label="Truth") ax1[1].plot(t, mu_hist[1, :], label="Est") ax1[1].set_ylabel("y (m)")
ekf_mses = [] lstm_mses = [] lstm_stacked_mses = [] bar = ProgressBar() for s in bar(range(num_sims)): #x_0 = np.random.normal(0, x_var, 1) x_0 = np.array([0]) sim = UNGM(x_0, R, Q, x_var) ukf = UKF(sim.f, sim.F, sim.h, sim.H, sim.Q, sim.R, 5., first_x_0, 1) ekf = EKF(sim.f, sim.F, sim.h, sim.H, sim.Q, sim.R, first_x_0, 1) for t in range(T): x, y = sim.process_next() ukf.predict() ukf.update(y) ekf.predict() ekf.update(y) ukf_mses.append(MSE(ukf.get_all_predictions(), sim.get_all_x())) ekf_mses.append(MSE(ekf.get_all_predictions(), sim.get_all_x())) all_x.append(np.array(sim.get_all_x())) all_y.append(np.array(sim.get_all_y())) X = np.array(all_y)[:, :-1, :] y = np.array(all_x)[:, 1:, :] lstm10_pred = lstm10.predict(X) lstm100_pred = lstm100.predict(X) rnn10_pred = rnn10.predict(X) rnn100_pred = rnn100.predict(X) lstm10_mses = []
measurement_range_variance = 0.3 * 0.3 measurement_angle_variance = 5.0 * math.pi / 180.0 * 5.0 * math.pi / 180.0 ekf = EKF(start_x, start_y, start_yaw) ekf.add_landmark(2.0, 2.0) ekf.add_landmark(4.0, -4.0) ekf.add_landmark(-2.0, -2.0) ekf.add_landmark(-4.0, 4.0) ekf.set_odom_noises(5.0, 2.0, 2.0, 8.0) ekf.set_measurement_variances(measurement_range_variance, measurement_angle_variance) ekf.set_min_trace(0.001) ekf.set_plot_sizes(5.0, 5.0) while True: # simulate robot behaviors robot_sim.update_pose(0.2, -0.2) delta_dist = robot_sim.sim_v * robot_sim.sim_time_step delta_yaw = robot_sim.sim_w * robot_sim.sim_time_step measurements = robot_sim.get_sensor_measurements() # ekf ekf.predict(delta_dist, delta_yaw) ekf.update(measurements) ekf.print_estimated_pose() ekf.print_estimated_covariance() ekf.plot_ekf_world(measurements) # sleep time.sleep(robot_sim.sim_time_step)
class TestEKF(unittest.TestCase): def setUp(self): self.empty_ekf = EKF(landmark_threshold=0) def tearDown(self): del self.empty_ekf def test_init(self): self.assertIsInstance(self.empty_ekf, EKF) self.assertEqual(self.empty_ekf.landmark_types, []) self.assertEqual(self.empty_ekf.landmark_counts, []) np.testing.assert_array_equal(self.empty_ekf.system_state, np.array([[0], [0], [0]])) np.testing.assert_array_equal( self.empty_ekf.covariance, np.array([[0.95, 0, 0], [0, 0.95, 0], [0, 0, 0.95]])) def test_noop_move(self): # Run the move function, but without actually moving. self.empty_ekf.update(radians(0), 0, []) np.testing.assert_array_equal(self.empty_ekf.pos(), np.array([[0], [0], [0]])) np.testing.assert_array_equal( self.empty_ekf.covariance, np.array([[0.95, 0, 0], [0, 0.95, 0], [0, 0, 0.95]])) def test_simple_move(self): # Turn left 30 degrees and advance 10 units of distance. self.empty_ekf.update(radians(30), 10, []) np.testing.assert_almost_equal( self.empty_ekf.pos(), np.array([[8.66025404], [5], [0.52359878]])) np.testing.assert_almost_equal( self.empty_ekf.covariance, np.array([[75.95, -38.9711432, -8.0005164], [-38.9711432, 25.95, 4.8808997], [-8.0005164, 4.8808997, 0.9637078]])) def test_linear_move_and_observe(self): # Add a landmark at the start. self.empty_ekf.update(radians(0), 0, [(20, 0, 'statue')]) np.testing.assert_array_equal(self.empty_ekf.pos(), np.array([[0], [0], [0]])) np.testing.assert_array_equal(self.empty_ekf.system_state, np.array([[0], [0], [0], [20], [0]])) np.testing.assert_array_equal( self.empty_ekf.covariance, np.array([[0.95, 0, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95], [0, 0, 0.95, 0, 0], [0.95, 0, 0, 1.15, 0], [0, 0.95, 0, 0, 0.95]])) # Move 5 units, but imply some error by landmark drift. self.empty_ekf.update(radians(0), 5, [(19, 0, 'statue')]) np.testing.assert_almost_equal( self.empty_ekf.pos(), np.array([[5.8274554], [0], [-0.042244909]])) np.testing.assert_almost_equal( self.empty_ekf.system_state, np.array([[5.8274554], [0], [-0.042244909], [19.9014031], [0]])) np.testing.assert_almost_equal( self.empty_ekf.covariance, np.array([[25.95, 0, -4.75, 0.95, 0], [0, 0.95, 0, 0, 0.95], [-4.75, 0, 0.95, 0, 0], [0.95, 0, 0, 1.15, 0], [0, 0.95, 0, 0, 0.95]])) def test_rotate_and_observe(self): # Add a landmark at the start. self.empty_ekf.update(radians(0), 0, [(20, 0, 'statue')]) np.testing.assert_array_equal(self.empty_ekf.pos(), np.array([[0], [0], [0]])) np.testing.assert_array_equal(self.empty_ekf.system_state, np.array([[0], [0], [0], [20], [0]])) np.testing.assert_array_equal( self.empty_ekf.covariance, np.array([[0.95, 0, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95], [0, 0, 0.95, 0, 0], [0.95, 0, 0, 1.15, 0], [0, 0.95, 0, 0, 0.95]])) # Turn 30 degrees, but imply some error by landmark drift. self.empty_ekf.update( radians(30), 0, [(20 * cos(radians(-2)), 20 * sin(radians(-2)), 'statue')]) np.testing.assert_almost_equal(self.empty_ekf.pos(), np.array([[0], [0], [0.55788443]])) np.testing.assert_almost_equal( self.empty_ekf.system_state, np.array([[0], [0], [0.55788443], [20], [0]])) np.testing.assert_almost_equal( self.empty_ekf.covariance, np.array([[0.95, 0, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95], [0, 0, 0.9637078, 0, 0], [0.95, 0, 0, 1.15, 0], [0, 0.95, 0, 0, 0.95]])) def test_add_two_landmarks(self): # Add first landmark. self.empty_ekf.update(radians(0), 0, [(20, 0, 'statue')]) np.testing.assert_array_equal(self.empty_ekf.pos(), np.array([[0], [0], [0]])) np.testing.assert_array_equal(self.empty_ekf.system_state, np.array([[0], [0], [0], [20], [0]])) np.testing.assert_array_equal( self.empty_ekf.covariance, np.array([[0.95, 0, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95], [0, 0, 0.95, 0, 0], [0.95, 0, 0, 1.15, 0], [0, 0.95, 0, 0, 0.95]])) # Add second landmark. self.empty_ekf.update(radians(0), 0, [(0, 20, 'fountain')]) np.testing.assert_array_equal(self.empty_ekf.pos(), np.array([[0], [0], [0]])) np.testing.assert_array_equal( self.empty_ekf.system_state, np.array([[0], [0], [0], [20], [0], [0], [20]])) np.testing.assert_array_equal( self.empty_ekf.covariance, np.array([[0.95, 0, 0, 0.95, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95, 0, 0.95], [0, 0, 0.95, 0, 0, 0, 0], [0.95, 0, 0, 1.15, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95, 0, 0.95], [0.95, 0, 0, 0.95, 0, 1.15, 0], [0, 0.95, 0, 0, 0.95, 0, 0.95]]))
import matplotlib.pyplot as plt import time lin_acc, ang_vel, vicon_pose, t = get_data() u_prev = np.concatenate([vicon_pose[0], np.zeros(9)], axis=0) u_prev = np.expand_dims(u_prev, axis=1) covar_prev = np.eye(15) saved_states = np.zeros([15, len(t)]) ekf = EKF() curr_time = time.time() for i in range(len(t)): if (i == 0): dt = t[i] covar_est, u_est = ekf.predict(u_prev, covar_prev, ang_vel[i], lin_acc[i], dt) z_t = np.expand_dims(vicon_pose[i], axis=1) u_curr, covar_curr = ekf.update(z_t, covar_est, u_est) saved_states[:, i] = np.squeeze(u_curr, axis=1) out = u_curr u_prev, covar_prev = u_curr, covar_curr else: print(time.time() - curr_time) dt = t[i] - t[i - 1] covar_est, u_est = ekf.predict(u_prev, covar_prev, ang_vel[i], lin_acc[i], dt) z_t = np.expand_dims(vicon_pose[i], axis=1) u_curr, covar_curr = ekf.update(z_t, covar_est, u_est) saved_states[:, i] = np.squeeze(u_curr, axis=1) out = np.concatenate([out, u_curr], axis=1) u_prev, covar_prev = u_curr, covar_curr plt.plot(t, saved_states[0, :])