## 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()