## Plot results
pyplot.figure(1)
pyplot.clf()
pyplot.hold(True)
# True state
pyplot.plot(x[0, 1:t], x[2, 1:t], 'ro--')
# EKF
pyplot.plot(mu_S[0, 1:t], mu_S[2, 1:t], 'bx--')
# Particle%
pyplot.plot(muP_S[0, 1:t], muP_S[2, 1:t], 'mx--')
# EKF Ellipses
mu_pos = [mu[0], mu[2]];
S_pos = [[S[0, 0], S[0, 2]], [S[2, 0], S[2, 2]]]
# error_ellipse(S_pos,mu_pos,0.75);
draw.error_ellipse(S_pos, mu_pos, 0.95);
# Particle set
pyplot.plot(X[0, 0::10], X[2, 0::10], 'm.')
# Ground
pyplot.plot(0, 0, 'bx', markersize=6, linewidth=2)
pyplot.plot([20, -1], [0, 0], 'b--')
pyplot.title('True state, EKF and Particle Filter')
pyplot.axis([-5, 20, -1, 10])
pyplot.legend(('True state', 'EKF', 'Particles'))
pyplot.waitforbuttonpress(1e-9)
pyplot.figure(2)
pyplot.clf()
pyplot.hold(True)
e = np.sqrt(np.power(x[0, 1:] - mu_S[0, 1:], 2) + np.power(x[2, 1:] - mu_S[2, 1:], 2))
pyplot.plot(T[1:], np.array(e)[0], 'b', linewidth=1.5)
ep = np.sqrt(np.power(x[0, 1:] - muP_S[0, 1:], 2) + np.power(x[2, 1:] - muP_S[2, 1:], 2))
np.array([u[:, i]]).T,
R,
G,
g,
meas_model['H'],
meas_model['h'],
T[i],
T[i - 1])
mu[:, i] = ekf_data['mu'].flatten()
S[:, :, i] = ekf_data['S']
plt.figure(1)
# Render cones
patches = []
for i in xrange(len(cones)):
patches.append(Circle((cones[i, 0], cones[i, 1]), d_cone / 2))
p = PatchCollection(patches, alpha=0.4)
plt.gca().add_collection(p)
# Render start position
plt.plot(x[0, 0], x[1, 0], 'bx', markersize=10, linewidth=2)
# Render car
for i, pnt in enumerate(mu.T):
plt.plot(pnt[0], pnt[1], 'rx', markersize=10, linewidth=2)
draw.drawcar(pnt[0], pnt[1], pnt[2], 0.25, fig=1, style='k')
draw.error_ellipse(S[0:2, 0:2, i], (pnt[0], pnt[1]), 0.95, style='g')
for pnt in x.T:
draw.drawcar(pnt[0], pnt[1], pnt[2], 0.25, fig=1)
plt.axis('equal')
plt.axis([0, 20, -2, 12])
plt.show()
## Plot results
pyplot.figure(1)
pyplot.clf()
pyplot.hold(True)
pyplot.plot(map[:, 0], map[:, 1], 'go', markersize=10, linewidth=2)
pyplot.plot(mf[0, t], mf[1, t], 'mx', markersize=10, linewidth=2)
pyplot.plot(x[0, :t], x[1, :t], 'ro--')
# if meas == 1:
# pyplot.circle(1, x(1:2, t), y(1, t))
if meas == 2:
pyplot.plot([x[0, t], x[0, t] + 10 * np.cos(y[0, t] + x[2, t])],
[ x[1, t], x[1, t] + 10 * np.sin(y[0, t] + x[2, t])],
'c')
elif meas == 3:
pyplot.plot([x[0, t], x[0, t] + y[0, t] * np.cos(y[1, t] + x[2, t])],
[x[1, t], x[1, t] + y[0, t] * np.sin(y[1, t] + x[2, t])],
'c')
pyplot.plot(mu_S[0, :t], mu_S[1, :t], 'bx--')
pyplot.plot(mup_S[0, :t - 1], mup_S[1, :t - 1], 'go--')
pyplot.axis('equal')
pyplot.axis([-4, 6, -1, 7])
mu_pos = np.array([mu[0], mu[1]])
S_pos = np.array([[S[0, 0], S[0, 1]], [S[1, 0], S[1, 1]]])
draw.error_ellipse(S_pos, mu_pos, 0.95)
draw.error_ellipse(S_pos, mu_pos, 0.999)
pyplot.title('Range & Bearing Measurements for Localization')
pyplot.waitforbuttonpress(1e-9)
Example of error ellipse and Gaussian noise generation
Created on Dec 15, 2010
@author: Michael Kwan
'''
import numpy as np
from matplotlib import pyplot
from amr import draw
if __name__ == '__main__':
# Define distribution
mu = np.array([[1], [2]])
S = np.array([[4, -1], [-1, 1]])
# Find eigenstuff
Se, SE = np.linalg.eig(S)
# Generate samples
samples = np.dot(np.dot(SE, np.sqrt(Se*np.eye(2))), np.random.randn(2, 10000))
# Create ellipse plots
pyplot.figure(1)
pyplot.clf()
pyplot.hold(True)
pyplot.axis('equal')
pyplot.plot(mu[0] + samples[0, :], mu[1] + samples[1, :], 'r.')
draw.error_ellipse(S, mu, 0.5)
draw.error_ellipse(S, mu, 0.99)
pyplot.show()