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, :])
