def test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together sp0 = JulierSigmaPoints(n=2, kappa=kappa) sp1 = JulierSigmaPoints(n=2, kappa=kappa * 1000) w0, _ = sp0.weights() w1, _ = sp1.weights() Xi0 = sp0.sigma_points(x, P) Xi1 = sp1.sigma_points(x, P) assert max(Xi1[:, 0]) > max(Xi0[:, 0]) assert max(Xi1[:, 1]) > max(Xi0[:, 1]) if DO_PLOT: plt.figure() for i in range(Xi0.shape[0]): plt.scatter((Xi0[i, 0] - x[0, 0]) * w0[i] + x[0, 0], (Xi0[i, 1] - x[0, 1]) * w0[i] + x[0, 1], color='blue') for i in range(Xi1.shape[0]): plt.scatter((Xi1[i, 0] - x[0, 0]) * w1[i] + x[0, 0], (Xi1[i, 1] - x[0, 1]) * w1[i] + x[0, 1], color='green') stats.plot_covariance_ellipse([1, 2], P)
def show_sigma_transform(with_text=False): fig = plt.gcf() ax = fig.gca() x = np.array([0, 5]) P = np.array([[4, -2.2], [-2.2, 3]]) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=9) sigmas = MerweScaledSigmaPoints(2, alpha=.5, beta=2., kappa=0.) S = sigmas.sigma_points(x=x, P=P) plt.scatter(S[:, 0], S[:, 1], c='k', s=80) x = np.array([15, 5]) P = np.array([[3, 1.2], [1.2, 6]]) plot_covariance_ellipse(x, P, facecolor='g', variance=9, alpha=0.3) ax.add_artist(arrow(S[0, 0], S[0, 1], 11, 4.1, 0.6)) ax.add_artist(arrow(S[1, 0], S[1, 1], 13, 7.7, 0.6)) ax.add_artist(arrow(S[2, 0], S[2, 1], 16.3, 0.93, 0.6)) ax.add_artist(arrow(S[3, 0], S[3, 1], 16.7, 10.8, 0.6)) ax.add_artist(arrow(S[4, 0], S[4, 1], 17.7, 5.6, 0.6)) ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) if with_text: plt.text(2.5, 1.5, r"$\chi$", fontsize=32) plt.text(13, -1, r"$\mathcal{Y}$", fontsize=32) #plt.axis('equal') plt.show()
def show_sigma_transform(with_text=False): fig = plt.figure() ax=fig.gca() x = np.array([0, 5]) P = np.array([[4, -2.2], [-2.2, 3]]) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=9) sigmas = MerweScaledSigmaPoints(2, alpha=.5, beta=2., kappa=0.) S = sigmas.sigma_points(x=x, P=P) plt.scatter(S[:,0], S[:,1], c='k', s=80) x = np.array([15, 5]) P = np.array([[3, 1.2],[1.2, 6]]) plot_covariance_ellipse(x, P, facecolor='g', variance=9, alpha=0.3) ax.add_artist(arrow(S[0,0], S[0,1], 11, 4.1, 0.6)) ax.add_artist(arrow(S[1,0], S[1,1], 13, 7.7, 0.6)) ax.add_artist(arrow(S[2,0], S[2,1], 16.3, 0.93, 0.6)) ax.add_artist(arrow(S[3,0], S[3,1], 16.7, 10.8, 0.6)) ax.add_artist(arrow(S[4,0], S[4,1], 17.7, 5.6, 0.6)) ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) if with_text: plt.text(2.5, 1.5, r"$\chi$", fontsize=32) plt.text(13, -1, r"$\mathcal{Y}$", fontsize=32) #plt.axis('equal') plt.show()
def plot_sigma_points(): x = np.array([0, 0]) P = np.array([[4, 2], [2, 4]]) sigmas = MerweScaledSigmaPoints(n=2, alpha=.3, beta=2., kappa=1.) S0 = sigmas.sigma_points(x, P) Wm0, Wc0 = sigmas.weights() sigmas = MerweScaledSigmaPoints(n=2, alpha=1., beta=2., kappa=1.) S1 = sigmas.sigma_points(x, P) Wm1, Wc1 = sigmas.weights() def plot_sigmas(s, w, **kwargs): min_w = min(abs(w)) scale_factor = 100 / min_w return plt.scatter(s[:, 0], s[:, 1], s=abs(w)*scale_factor, alpha=.5, **kwargs) plt.subplot(121) plot_sigmas(S0, Wc0, c='b') plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4]) plt.title('alpha=0.3') plt.subplot(122) plot_sigmas(S1, Wc1, c='b', label='Kappa=2') plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4]) plt.title('alpha=1') plt.show() print(sum(Wc0))
def plot_sigma_points(): x = np.array([0, 0]) P = np.array([[4, 2], [2, 4]]) sigmas = MerweScaledSigmaPoints(n=2, alpha=.3, beta=2., kappa=1.) S0 = sigmas.sigma_points(x, P) Wm0, Wc0 = sigmas.Wm, sigmas.Wc sigmas = MerweScaledSigmaPoints(n=2, alpha=1., beta=2., kappa=1.) S1 = sigmas.sigma_points(x, P) Wm1, Wc1 = sigmas.Wm, sigmas.Wc def plot_sigmas(s, w, **kwargs): min_w = min(abs(w)) scale_factor = 100 / min_w return plt.scatter(s[:, 0], s[:, 1], s=abs(w) * scale_factor, alpha=.5, **kwargs) plt.subplot(121) plot_sigmas(S0, Wc0, c='b') plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4]) plt.title('alpha=0.3') plt.subplot(122) plot_sigmas(S1, Wc1, c='b', label='Kappa=2') plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4]) plt.title('alpha=1') plt.show()
def run_localization( cmds, landmarks, sigma_vel, sigma_steer, sigma_range, sigma_bearing, ellipse_step=1, step=10): plt.figure() points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0, subtract=residual_x) ukf = UKF(dim_x=3, dim_z=2*len(landmarks), fx=fx, hx=Hx, dt=dt, points=points, x_mean_fn=state_mean, z_mean_fn=z_mean, residual_x=residual_x, residual_z=residual_h) ukf.x = np.array([2, 6, .3]) ukf.P = np.diag([.1, .1, .05]) ukf.R = np.diag([sigma_range**2, sigma_bearing**2]*len(landmarks)) ukf.Q = np.eye(3)*0.0001 sim_pos = ukf.x.copy() # plot landmarks if len(landmarks) > 0: plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60) track = [] for i, u in enumerate(cmds): sim_pos = move(sim_pos, u, dt/step, wheelbase) track.append(sim_pos) if i % step == 0: ukf.predict(fx_args=u) if i % ellipse_step == 0: plot_covariance_ellipse( (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6, facecolor='k', alpha=0.3) x, y = sim_pos[0], sim_pos[1] z = [] for lmark in landmarks: dx, dy = lmark[0] - x, lmark[1] - y d = sqrt(dx**2 + dy**2) + randn()*sigma_range bearing = atan2(lmark[1] - y, lmark[0] - x) a = (normalize_angle(bearing - sim_pos[2] + randn()*sigma_bearing)) z.extend([d, a]) ukf.update(z, hx_args=(landmarks,)) if i % ellipse_step == 0: plot_covariance_ellipse( (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6, facecolor='g', alpha=0.8) track = np.array(track) plt.plot(track[:, 0], track[:,1], color='k', lw=2) plt.axis('equal') plt.title("UKF Robot localization") plt.show() return ukf
def plotEllipseNew(mu, sigma, s): sigma = sigma[:2, :2] mu = mu[:2] muT = (mu[0, 0], mu[1, 0]) print('muT' + str(muT)) plot_covariance_ellipse(muT, cov=sigma, variance=1.0, std=s) return 0
def test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together sp0 = JulierSigmaPoints(n=2, kappa=kappa) sp1 = JulierSigmaPoints(n=2, kappa=kappa * 1000) sp2 = MerweScaledSigmaPoints(n=2, kappa=0, beta=2, alpha=1e-3) sp3 = SimplexSigmaPoints(n=2) # test __repr__ doesn't crash str(sp0) str(sp1) str(sp2) str(sp3) w0, _ = sp0.weights() w1, _ = sp1.weights() w2, _ = sp2.weights() w3, _ = sp3.weights() Xi0 = sp0.sigma_points(x, P) Xi1 = sp1.sigma_points(x, P) Xi2 = sp2.sigma_points(x, P) Xi3 = sp3.sigma_points(x, P) assert max(Xi1[:, 0]) > max(Xi0[:, 0]) assert max(Xi1[:, 1]) > max(Xi0[:, 1]) if DO_PLOT: plt.figure() for i in range(Xi0.shape[0]): plt.scatter((Xi0[i, 0] - x[0, 0]) * w0[i] + x[0, 0], (Xi0[i, 1] - x[0, 1]) * w0[i] + x[0, 1], color='blue', label='Julier low $\kappa$') for i in range(Xi1.shape[0]): plt.scatter((Xi1[i, 0] - x[0, 0]) * w1[i] + x[0, 0], (Xi1[i, 1] - x[0, 1]) * w1[i] + x[0, 1], color='green', label='Julier high $\kappa$') # for i in range(Xi2.shape[0]): # plt.scatter((Xi2[i, 0] - x[0, 0]) * w2[i] + x[0, 0], # (Xi2[i, 1] - x[0, 1]) * w2[i] + x[0, 1], # color='red') for i in range(Xi3.shape[0]): plt.scatter((Xi3[i, 0] - x[0, 0]) * w3[i] + x[0, 0], (Xi3[i, 1] - x[0, 1]) * w3[i] + x[0, 1], color='black', label='Simplex') stats.plot_covariance_ellipse([1, 2], P)
def plot3(): P = np.array([[6, 2.5], [2.5, .6]]) circle1=plt.Circle((10,0),3,color='#004080',fill=False,linewidth=4, alpha=.7) ax = plt.gca() ax.add_artist(circle1) plt.xlim(0,10) plt.ylim(0,3) plt.axhline(3, ls='--') stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2)
def run_localization(landmarks, sigma_vel=0.1, sigma_steer=np.radians(1), sigma_range=0.3, sigma_bearing=0.1, dt=1.0, x_init=[2, 6, .3]): ekf = RobotEKF(dt, wheelbase=0.5, sigma_vel=sigma_vel, sigma_steer=sigma_steer) ekf.x = array([x_init]).T ekf.P = np.diag([.1, .1, .1]) ekf.R = np.diag([sigma_range**2, sigma_bearing**2]) sim_pos = ekf.x.copy() # simulated position u = array([1.1, .01]) # steering command (vel, steering angle radians) plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60) for i in range(200): sim_pos = ekf.move(sim_pos, u, dt / 10.) # simulate robot plt.plot(sim_pos[0], sim_pos[1], ',', color='g') if i % 10 == 0: ekf.predict(u=u) plot_covariance_ellipse((ekf.x[0, 0], ekf.x[1, 0]), ekf.P[0:2, 0:2], std=6, facecolor='b', alpha=0.08) x, y = sim_pos[0, 0], sim_pos[1, 0] for lmark in landmarks: d = np.sqrt((lmark[0] - x)**2 + (lmark[1] - y)**2) a = atan2(lmark[1] - y, lmark[0] - x) - sim_pos[2, 0] z = np.array([[d + np.random.randn() * sigma_range], [a + np.random.randn() * sigma_bearing]]) ekf.update(z, HJacobian=H_of, Hx=Hx, residual=residual, args=(lmark), hx_args=(lmark)) plot_covariance_ellipse((ekf.x[0, 0], ekf.x[1, 0]), ekf.P[0:2, 0:2], std=6, facecolor='g', alpha=0.4) plt.axis('equal') plt.show() return ekf
def run_localization(landmarks, std_vel, std_steer, std_range, std_bearing, step=10, ellipse_step=20, ylim=None): ekf = RobotEKF(dt, wheelbase=0.5, std_vel=std_vel, std_steer=std_steer) ekf.x = array([[2, 6, .3]]).T # x, y, steer angle ekf.P = np.diag([.1, .1, .1]) ekf.R = np.diag([std_range**2, std_bearing**2]) sim_pos = ekf.x.copy() # simulated position # steering command (vel, steering angle radians) u = array([1.1, .07]) plt.figure() plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60) track = [] for i in range(200): sim_pos = ekf.move(sim_pos, u, dt / 10.) # simulate robot track.append(sim_pos) if i % step == 0: ekf.predict(u=u) if i % ellipse_step == 0: plot_covariance_ellipse((ekf.x[0, 0], ekf.x[1, 0]), ekf.P[0:2, 0:2], std=6, facecolor='k', alpha=0.3) x, y = sim_pos[0, 0], sim_pos[1, 0] for lmark in landmarks: z = z_landmark(lmark, sim_pos, std_range, std_bearing) ekf_update(ekf, z, lmark) if i % ellipse_step == 0: plot_covariance_ellipse((ekf.x[0, 0], ekf.x[1, 0]), ekf.P[0:2, 0:2], std=6, facecolor='g', alpha=0.8) track = np.array(track) plt.plot(track[:, 0], track[:, 1], color='k', lw=2) plt.axis('equal') plt.title("EKF Robot localization") if ylim is not None: plt.ylim(*ylim) plt.show() return ekf
def plot_correlation_covariance(): P = [[4, 3.9], [3.9, 4]] plot_covariance_ellipse((5, 10), P, edgecolor='k', variance=[1, 2**2, 3**2]) plt.xlabel('X') plt.ylabel('Y') plt.gca().autoscale(tight=True) plt.axvline(7.5, ls='--', lw=1) plt.axhline(12.5, ls='--', lw=1) plt.scatter(7.5, 12.5, s=1500, alpha=0.5) plt.title('|4.0 3.9|\n|3.9 4.0|') plt.show()
def show_x_with_unobserved(): """ shows x=1,2,3 with velocity superimposed on top """ # plot velocity sigma=[0.5,1.,1.5,2] cov = np.array([[1,1],[1,1.1]]) stats.plot_covariance_ellipse ((2,2), cov=cov, variance=sigma, axis_equal=False) # plot positions cov = np.array([[0.003,0], [0,12]]) sigma=[0.5,1.,1.5,2] e = stats.covariance_ellipse (cov) stats.plot_covariance_ellipse ((1,1), ellipse=e, variance=sigma, axis_equal=False) stats.plot_covariance_ellipse ((2,1), ellipse=e, variance=sigma, axis_equal=False) stats.plot_covariance_ellipse ((3,1), ellipse=e, variance=sigma, axis_equal=False) # plot intersection cirle isct = Ellipse(xy=(2,2), width=.2, height=1.2, edgecolor='r', fc='None', lw=4) plt.gca().add_artist(isct) plt.ylim([0,11]) plt.xlim([0,4]) plt.xticks(np.arange(1,4,1)) plt.xlabel("Position") plt.ylabel("Time") plt.show()
def plot3(): P = np.array([[6, 2.5], [2.5, .6]]) circle1 = plt.Circle((10, 0), 3, color='#004080', fill=False, linewidth=4, alpha=.7) ax = plt.gca() ax.add_artist(circle1) plt.xlim(0, 10) plt.ylim(0, 3) plt.axhline(3, ls='--') stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2)
def test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together sp0 = JulierSigmaPoints(n=2, kappa=kappa) sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000) sp2 = MerweScaledSigmaPoints(n=2, kappa=0, beta=2, alpha=1e-3) sp3 = SimplexSigmaPoints(n=2) w0, _ = sp0.weights() w1, _ = sp1.weights() w2, _ = sp2.weights() w3, _ = sp3.weights() Xi0 = sp0.sigma_points(x, P) Xi1 = sp1.sigma_points(x, P) Xi2 = sp2.sigma_points(x, P) Xi3 = sp3.sigma_points(x, P) assert max(Xi1[:,0]) > max(Xi0[:,0]) assert max(Xi1[:,1]) > max(Xi0[:,1]) if DO_PLOT: plt.figure() for i in range(Xi0.shape[0]): plt.scatter((Xi0[i,0]-x[0, 0])*w0[i] + x[0, 0], (Xi0[i,1]-x[0, 1])*w0[i] + x[0, 1], color='blue', label='Julier low $\kappa$') for i in range(Xi1.shape[0]): plt.scatter((Xi1[i, 0]-x[0, 0]) * w1[i] + x[0,0], (Xi1[i, 1]-x[0, 1]) * w1[i] + x[0,1], color='green', label='Julier high $\kappa$') # for i in range(Xi2.shape[0]): # plt.scatter((Xi2[i, 0] - x[0, 0]) * w2[i] + x[0, 0], # (Xi2[i, 1] - x[0, 1]) * w2[i] + x[0, 1], # color='red') for i in range(Xi3.shape[0]): plt.scatter((Xi3[i, 0] - x[0, 0]) * w3[i] + x[0, 0], (Xi3[i, 1] - x[0, 1]) * w3[i] + x[0, 1], color='black', label='Simplex') stats.plot_covariance_ellipse([1, 2], P)
def run_localization(cmds, x, x_guess, P, std_an, std_r, std_b,\ dt=1.0, ellipse_step=1, step=10, show=True): if show: plt.figure(figsize=(9, 6)) ukf = build_ukf(x_guess, P, std_r, std_b, dt=1.0) sim_pos = x # ideal_pos = ukf.x.copy() np.random.seed(1) # ideal = [] traj = [] xs = [] for i, u in enumerate(cmds): # ideal_pos = move(ideal_pos, dt/step, u) sim_pos = noisy_move(sim_pos, dt/step, u, std_an) traj.append(sim_pos) # ideal.append(ideal_pos) if i % step == 0: ukf.predict(u=u) if i % ellipse_step == 0 and show: cov = np.array([[ukf.P[0, 0], ukf.P[2, 0]], [ukf.P[0, 2], ukf.P[2, 2]]]) plot_covariance_ellipse((ukf.x[0], ukf.x[2]), cov, std=6, facecolor='k', alpha=0.3) z = noisy_sensor(sim_pos, std_r, std_b) ukf.update(z) if i % ellipse_step == 0 and show: cov = np.array([[ukf.P[0, 0], ukf.P[2, 0]], [ukf.P[0, 2], ukf.P[2, 2]]]) plot_covariance_ellipse((ukf.x[0], ukf.x[2]), cov, std=6, facecolor='g', alpha=0.8) xs.append(ukf.x.copy()) xs = np.array(xs) traj = np.array(traj) # ideal = np.array(ideal) if show: plt.plot(traj[:, 0], traj[:, 2], color='b', linewidth=2, label='Actual trajectory') plt.plot(xs[:, 0], xs[:, 2], 'r--', linewidth=2, label='Estimated trajectory') # plt.plot(ideal[:, 0], ideal[:, 1], 'y', linewidth=2, label='Ideal trajectory') plt.legend() plt.title("UKF Robot Localization") plt.axis([0, 750, 0, 500]) plt.xlabel('x (mm)') plt.ylabel('y (mm)') plt.grid() plt.show() return xs, traj
def plot_track_ellipses(N, zs, ps, cov, title): #bp.plot_measurements(range(1,N + 1), zs) #plt.plot(range(1, N + 1), ps, c='b', lw=2, label='filter') plt.title(title) for i,p in enumerate(cov): plot_covariance_ellipse( (i+1, ps[i]), cov=p, variance=4, axis_equal=False, ec='g', alpha=0.5) if i == len(cov)-1: s = ('$\sigma^2_{pos} = %.2f$' % p[0,0]) plt.text (20, 5, s, fontsize=18) s = ('$\sigma^2_{vel} = %.2f$' % p[1, 1]) plt.text (20, 0, s, fontsize=18) plt.ylim(-5, 20) plt.gca().set_aspect('equal')
def plot_3_covariances(): P = [[2, 0], [0, 2]] plt.subplot(131) plot_covariance_ellipse((2, 7), cov=P, facecolor='g', alpha=0.2, title='|2 0|\n|0 2|', axis_equal=False) plt.ylim((4, 10)) plt.gca().set_aspect('equal', adjustable='box') plt.subplot(132) P = [[2, 0], [0, 9]] plt.ylim((4, 10)) plt.gca().set_aspect('equal', adjustable='box') plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2, axis_equal=False, title='|2 0|\n|0 9|') plt.subplot(133) P = [[2, 1.2], [1.2, 2]] plt.ylim((4, 10)) plt.gca().set_aspect('equal', adjustable='box') plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2, axis_equal=False, title='|2 1.2|\n|1.2 2|') plt.tight_layout() plt.show()
def show_sigma_selections(): ax=plt.gca() ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) x = np.array([2, 5]) P = np.array([[3, 1.1], [1.1, 4]]) points = MerweScaledSigmaPoints(2, .09, 2., 1.) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') x = np.array([5, 5]) points = MerweScaledSigmaPoints(2, .15, 1., .15) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') x = np.array([8, 5]) points = MerweScaledSigmaPoints(2, .2, 3., 10) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') plt.axis('equal') plt.xlim(0,10); plt.ylim(0,10) plt.show()
def plot_3_covariances(): P = [[2, 0], [0, 2]] plt.subplot(131) plt.gca().grid(b=False) plt.gca().set_xticks([0,1,2,3,4]) plot_covariance_ellipse((2, 7), cov=P, facecolor='g', alpha=0.2, title='|2 0|\n|0 2|', std=[1,2,3], axis_equal=False) plt.ylim((0, 15)) plt.gca().set_aspect('equal', adjustable='box') plt.subplot(132) plt.gca().grid(b=False) plt.gca().set_xticks([0,1,2,3,4]) P = [[2, 0], [0, 6]] plt.ylim((0, 15)) plt.gca().set_aspect('equal', adjustable='box') plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2, std=[1,2,3],axis_equal=False, title='|2 0|\n|0 6|') plt.subplot(133) plt.gca().grid(b=False) plt.gca().set_xticks([0,1,2,3,4]) P = [[2, 1.2], [1.2, 2]] plt.ylim((0, 15)) plt.gca().set_aspect('equal', adjustable='box') plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2, axis_equal=False,std=[1,2,3], title='|2 1.2|\n|1.2 2|') plt.tight_layout() plt.show()
def plot_3_covariances(): P = [[2, 0], [0, 2]] plt.subplot(131) plt.gca().grid(b=False) plt.gca().set_xticks([0,1,2,3,4]) plot_covariance_ellipse((2, 7), cov=P, facecolor='g', alpha=0.2, title='|2 0|\n|0 2|', std=[3], axis_equal=False) plt.ylim((0, 15)) plt.gca().set_aspect('equal', adjustable='box') plt.subplot(132) plt.gca().grid(b=False) plt.gca().set_xticks([0,1,2,3,4]) P = [[2, 0], [0, 6]] plt.ylim((0, 15)) plt.gca().set_aspect('equal', adjustable='box') plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2, std=[3],axis_equal=False, title='|2 0|\n|0 6|') plt.subplot(133) plt.gca().grid(b=False) plt.gca().set_xticks([0,1,2,3,4]) P = [[2, 1.2], [1.2, 2]] plt.ylim((0, 15)) plt.gca().set_aspect('equal', adjustable='box') plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2, axis_equal=False,std=[3], title='|2.0 1.2|\n|1.2 2.0|') plt.tight_layout() plt.show()
def show_sigma_selections(): ax=plt.gca() ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) x = np.array([2, 5]) P = np.array([[3, 1.1], [1.1, 4]]) points = MerweScaledSigmaPoints(2, .05, 2., 1.) sigmas = points.sigma_points(x, P) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5]) plt.scatter(sigmas[:,0], sigmas[:, 1], c='k', s=50) x = np.array([5, 5]) points = MerweScaledSigmaPoints(2, .15, 2., 1.) sigmas = points.sigma_points(x, P) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5]) plt.scatter(sigmas[:,0], sigmas[:, 1], c='k', s=50) x = np.array([8, 5]) points = MerweScaledSigmaPoints(2, .4, 2., 1.) sigmas = points.sigma_points(x, P) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5]) plt.scatter(sigmas[:,0], sigmas[:, 1], c='k', s=50) plt.axis('equal') plt.xlim(0,10); plt.ylim(0,10) plt.show()
def show_sigma_selections(): ax = plt.gca() ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) x = np.array([2, 5]) P = np.array([[3, 1.1], [1.1, 4]]) points = MerweScaledSigmaPoints(2, .09, 2., 1.) sigmas = points.sigma_points(x, P) Wm, Wc = points.Wm, points.Wc plot_covariance_ellipse(x, P, facecolor='b', alpha=.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') x = np.array([5, 5]) points = MerweScaledSigmaPoints(2, .15, 1., .15) sigmas = points.sigma_points(x, P) Wm, Wc = points.Wm, points.Wc plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') x = np.array([8, 5]) points = MerweScaledSigmaPoints(2, .2, 3., 10) sigmas = points.sigma_points(x, P) Wm, Wc = points.Wm, points.Wc plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') plt.axis('equal') plt.xlim(0, 10) plt.ylim(0, 10) plt.show()
def plot_interactive_covariance_multiply(x1, x2, x12, z1, z2, z12): plt.figure(figsize=(9, 6)) P1 = [[x1, x12], [x12, x2]] P2 = [[z1, z12], [z12, z2]] P3 = multivariate_multiply((0, 0), P1, (0, 0), P2)[1] plot_covariance_ellipse((0, 0), P1, ec='k', fc='y', alpha=0.6) plot_covariance_ellipse((0, 0), P2, ec='k', fc='g', alpha=0.6) plot_covariance_ellipse((0, 0), P3, ec='k', fc='b')
def plotResults(measurements, predictions, covariances): for pos_vel, cov_matrix in zip(predictions, covariances): cov = np.array([[cov_matrix[0, 0], cov_matrix[2, 0]], [cov_matrix[0, 2], cov_matrix[2, 2]]]) # cov = np.array([[cov_matrix[1, 1], cov_matrix[3, 1]],[cov_matrix[1, 3], cov_matrix[3, 3]]]) [x_pos, x_vel, y_pos, y_vel] = pos_vel[:, 0] mean = (x_pos, y_pos) # Plotting elipses plot_covariance_ellipse(mean, cov=cov, fc='r', alpha=0.20, show_semiaxis=True) # plotting velocity plt.quiver(x_pos, y_pos, x_vel, y_vel, color='b', scale_units='xy', scale=1, alpha=0.50, width=0.005) # Plotting meditions x = [] y = [] for measure in measurements: if not measure: continue x.append(measure[0]) y.append(measure[1]) plt.plot(x, y, '.g', lw=1, ls='--') plt.axis('equal') plt.show()
def example(): dt = 0.1 x = np.array([10.0, 4.5]) P = np.diag([500, 49]) F = np.array([[1, dt], [0, 1]]) # Q is the process noise fig = plt.figure() for _ in range(40): # if the history should be deleted uncomment #fig.clear() plot_covariance_ellipse(x, P, edgecolor='r') x, P = predict(x=x, P=P, F=F, Q=0) print('x =', x) plot_covariance_ellipse(x, P, edgecolor='k', ls='dashed') fig.canvas.draw() plt.show(block=False) time.sleep(0.1) plt.show()
def run_localisation(ukf, landmarks): global epoch plt.figure() sim_pos = ukf.x.copy() # plot landmarks if len(landmarks) > 0: plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60) track = [] while not stop: print("epoch:", epoch) #print("cov", ukf.P) print("pos:", ukf.x) epoch += 1 ukf.predict() track.append((time.time(), ukf.x)) redis_send_pose(ukf.x) if epoch % 5 == 0: plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6, facecolor='g', alpha=0.8) time.sleep(0.5) track = np.array(track) #plt.plot(track[:, 0], track[:, 1], color='k', lw=2) #plt.axis('equal') #plt.title("UKF Robot localization") #plt.show() with open("track.txt", "w") as f: f.write("timestamp, x, y, theta\n") for t, p in track: f.write("{}, {}, {}, {}\n".format(t, *p)) print(track) return ukf
def test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together sp0 = JulierSigmaPoints(n=2, kappa=kappa) sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000) w0, _ = sp0.weights() w1, _ = sp1.weights() Xi0 = sp0.sigma_points (x, P) Xi1 = sp1.sigma_points (x, P) assert max(Xi1[:,0]) > max(Xi0[:,0]) assert max(Xi1[:,1]) > max(Xi0[:,1]) if DO_PLOT: plt.figure() for i in range(Xi0.shape[0]): plt.scatter((Xi0[i,0]-x[0, 0])*w0[i] + x[0, 0], (Xi0[i,1]-x[0, 1])*w0[i] + x[0, 1], color='blue') for i in range(Xi1.shape[0]): plt.scatter((Xi1[i, 0]-x[0, 0]) * w1[i] + x[0,0], (Xi1[i, 1]-x[0, 1]) * w1[i] + x[0,1], color='green') stats.plot_covariance_ellipse([1, 2], P)
def run(r): landmarks = np.array([[5, 5], [5, 0], [10, 5]]) landmark_cov = np.diag([0.1, 0.1]) control_ideal = np.array([1, 1.1]) #plot landmark covariances for l in landmarks: plot_covariance_ellipse((l[0], l[1]), landmark_cov, std=6, facecolor='k', alpha=0.3) track = [] for i in range(200): print i control = control_ideal # * [np.random.normal(1, 0.1),np.random.normal(1, 0.1)] r.predict(control, control_ideal) # print "yo" # print track track.append(r.x_act[:, 0]) if (i % 10 == 0): plot_covariance_ellipse((r.x[0, 0], r.x[1, 0]), r.P[0:2, 0:2], std=6, facecolor='k', alpha=0.3) for l in landmarks: dist = np.sqrt((l[0] - r.x_act[0][0])**2 + (l[1] - r.x_act[1][0])**2) + np.random.normal( 0, 0.1) theta = np.arctan2( l[1] - r.x_act[1][0], l[0] - r.x_act[0][0]) - r.x_act[2][0] + np.random.normal(0, 0.02) measurement = np.array([[dist], [theta]]) r.update(measurement, l, landmark_cov) plot_covariance_ellipse((r.x[0][0], r.x[1][0]), r.P[0:2, 0:2], std=6, facecolor='g', alpha=0.8) #change direction #control_ideal = np.array([np.random.normal(1, 0.5),np.random.normal(1, 0.5)]) #output the final covariance print r.P track = np.array(track) plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60) plt.plot(track[:, 0], track[:, 1], color='k', lw=2) plt.ylim((-10, 15)) plt.show()
if __name__=='__main__': if False: # fixme: vary R and Q N = 30 sensor = PosSensor1 ((0, 0), (2, 1), 1.) zs = np.array([np.array([sensor.read()]).T for _ in range(N)]) # run filter robot_tracker = tracker1() mu, cov, _, _ = robot_tracker.batch_filter(zs) for x, P in zip(mu, cov): # covariance of x and y cov = np.array([[P[0, 0], P[2, 0]], [P[0, 2], P[2, 2]]]) mean = (x[0, 0], x[2, 0]) plot_covariance_ellipse(mean, cov=cov, fc='g', alpha=0.15) print robot_tracker.P # plot results zs *= .3048 # convert to meters bp.plot_filter(mu[:, 0], mu[:, 2]) bp.plot_measurements(zs[:, 0], zs[:, 1]) plt.legend(loc=2) plt.gca().set_aspect('equal') plt.xlim((0, 20)); if True: a()
kalman_filter.predict(u, wmega, dt) kalman_filter.update(u, wmega, dt, tempZ) output = kalman_filter.x results.append(output) output_listx.append(np.asscalar(output[0])) output_listy.append(np.asscalar(output[1])) line1.set_ydata(output_listy) line1.set_xdata(output_listx) lambda_, v = np.linalg.eig(kalman_filter.P[0:2, 0:2]) lambda_ = np.sqrt(lambda_) plot_covariance_ellipse(np.array([(output[5]), (output[6])]), kalman_filter.P[5:7, 5:7], facecolor='lightgreen', alpha=0.1) plot_covariance_ellipse(np.array([(output[3]), (output[4])]), kalman_filter.P[3:5, 3:5], facecolor='lightcoral', alpha=0.1) plot_covariance_ellipse(np.array([(output[0]), (output[1])]), kalman_filter.P[0:2, 0:2], facecolor='lightskyblue', alpha=0.1) ax.scatter(np.asscalar(output[3]), np.asscalar(output[4]), color='r', zorder=t) ax.scatter(np.asscalar(output[5]),
# lists for the motion model Xk,Y ListXk=[] ListY=[] X_pures=[] Xk=X Xpepe=X ListXk.append(X) X_pures.append(X) fig = plt.figure() ax10 = fig.add_subplot(111) plot_covariance_ellipse([0,0],P, facecolor='lightskyblue', alpha=0.2) # applying kalman filter for t in range (0,N_iter): w1=np.random.normal(0, np.sqrt(0), 1) w2=np.random.normal(0, np.sqrt(0), 1) # process noise W =np.array([[w1[0]],[w2[0]]]) e=np.random.normal(0, np.sqrt(R), 1) # motion model Xk=np.matmul(F,Xk)+np.matmul(B,U)+np.matmul(G,W)
y0 = data["data_set"][2]["world"][2] tracker.x = np.array([[x0, y0, 0, 0]]).T tracker.P = np.eye(4) * 50. return tracker ############################################### zs = np.array([[d["world"][0], d["world"][2]] for d in data["data_set"]]) # run filter robot_tracker = Tracker() mu, cov, _, _ = robot_tracker.batch_filter(zs) for x, P in zip(mu, cov): # covariance of x and y cov = np.array([[P[0, 0], P[2, 0]], [P[0, 2], P[2, 2]]]) mean = (x[0, 0], x[2, 0]) #print(x[0, 0], x[1, 0], x[2, 0], x[3, 0]) #print(cov) plot_covariance_ellipse(mean, cov=cov, fc='g', std=3, alpha=0.5) #plt.pause(0.1) #plot results plot_filter(mu[:, 0], mu[:, 2]) plot_measurements(zs[:, 0], zs[:, 1]) plt.legend(loc=2) plt.xlim(0, 20) plt.show()
# sigma[s_l:(s_l + 2), s_l:(s_l + 2)], # std=6, facecolor='none', ec='#e74c3c', alpha=0.8) # if (i == 2) and (len(sigma) >= 9): # plot_covariance_ellipse( # (u[s_l], u[(s_l + 1)]), # sigma[s_l:(s_l + 2), s_l:(s_l + 2)], # std=6, facecolor='none', ec='#f1c40f', alpha=0.8) # if (i == 3) and (len(sigma) >= 11): # plot_covariance_ellipse( # (u[s_l], u[(s_l + 1)]), # sigma[s_l:(s_l + 2), s_l:(s_l + 2)], # std=6, facecolor='none', ec='#2c3e50', alpha=0.8) # if (i == 4) and (len(sigma) >= 13): # plot_covariance_ellipse( # (u[s_l], u[(s_l + 1)]), # sigma[s_l:(s_l + 2), s_l:(s_l + 2)], # std=6, facecolor='none', ec='#2ecc71', alpha=0.8) #Plot the robot covariance plot_covariance_ellipse((float(u[0]), float(u[1])), (sigma[0:2, 0:2]), std=6, facecolor='none', ec='#004080', alpha=0.3) print(u) print(sigma) plt.axis([-1.0, 2.0, -1.0, 2.0]) plt.pause(0.01)
def run_localization(std_vel, std_steer, std_range, std_bearing, ylim=None): ekf = RobotEKF(dt, wheelbase=0.5, std_vel=std_vel, std_steer=std_steer) robot_x = 0.0 robot_y = 0.0 robot_theta = 0.0 ekf.x = np.array([[robot_x, robot_y, robot_theta]]).T # x, y, theta ekf.P = np.diag([.1, .1, .1]) #NOISE ekf.R = np.diag([1, 1]) M = np.diag([0.001, 0.1]) data_file = open("data.txt", 'r') sensor_file = open("sensor.txt", 'r') xr_file = open("xr.txt", 'r') with data_file as f: data = [tuple(map(float, i.split(','))) for i in f] with sensor_file as f: sensor = [tuple(map(float, i.split(','))) for i in f] with xr_file as f: xr = [tuple(map(float, i.split(','))) for i in f] plt.figure() initialize_map = bm.base_map() initialize_map.draw_base_map() steps = 0 for i in range(49): delta_d, delta_theta = data[steps] robot_vel = delta_d / dt XJacobian = np.matrix([[1, 0, (-delta_d * math.sin(delta_theta))], [0, 1, (delta_d * math.cos(delta_theta))], [0, 0, 1]]) UJacobian = np.matrix([[(math.cos(delta_theta)), 0], [(math.sin(delta_theta)), 0], [0, 1]]) # # steering command (velocity, heading) u = np.array([robot_vel, delta_theta]) ekf.predict(XJacobian, UJacobian, M, u=u) for j in range(5): sensor_read = sense(steps, sensor) rng = sensor_read[j][0] bearing = sensor_read[j][1] landmark_pos = initialize_map.get_landmark_pos(j) lmark = [landmark_pos[1], landmark_pos[2]] z_new = np.array([[rng], [bearing]]) # sim_pos = ekf.x.copy() # simulated position # z = z_landmark(lmark, sim_pos) #print z_new ekf_update(ekf, z_new, lmark, rng) plot_covariance_ellipse((ekf.x[0, 0], ekf.x[1, 0]), ekf.P[0:2, 0:2], std=6, facecolor='none', alpha=0.8) steps += 1 #x_true is our real robot data x_true = ask_the_oracle(steps, xr) axis_marker = draw_axis_marker(math.degrees(x_true[2])) axis_marker_predict = draw_axis_marker(math.degrees(ekf.x[2, 0])) #plot the data plt.scatter(x_true[0], x_true[1], marker=(3, 0, math.degrees(x_true[2]) + 26), color='b', s=200) plt.scatter(x_true[0], x_true[1], marker=axis_marker, color='r', s=300) plt.scatter(ekf.x[0, 0], ekf.x[1, 0], marker=(3, 0, (math.degrees(ekf.x[2, 0] + 26))), color='y', s=200) plt.scatter(ekf.x[0, 0], ekf.x[1, 0], marker=axis_marker_predict, color='r', s=300) plt.pause(dt) plt.show() return ekf
xp = ekf.x.copy() plt.figure() plt.scatter(m[:, 0], m[:, 1]) for i in range(500): xp = ekf.move(xp, u, dt/10.) plt.plot(xp[0], xp[1], ',', color='g') if i % 10 == 0: ekf.predict(u=u) print('x',ekf.x[0,0]) print('y',ekf.x[1,0]) plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=3, facecolor='b', alpha=0.08) for lmark in m: d = sqrt((lmark[0] - xp[0, 0])**2 + (lmark[1] - xp[1, 0])**2) + randn()*sigma_r a = atan2(lmark[1] - xp[1, 0], lmark[0] - xp[0, 0]) - xp[2, 0] + randn()*sigma_h z = np.array([[d], [a]]) ekf.update(z, HJacobian=H_of, Hx=Hx, residual=residual, args=(lmark), hx_args=(lmark)) plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=3, facecolor='g', alpha=0.4) plt.axis('equal') plt.title("EKF Robot localization")
def plot1(): P = np.array([[6, 2.5], [2.5, .6]]) stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2)
def show_x_error_chart(count): """ displays x=123 with covariances showing error""" plt.cla() plt.gca().autoscale(tight=True) cov = np.array([[0.03, 0], [0, 8]]) e = stats.covariance_ellipse(cov) cov2 = np.array([[0.03, 0], [0, 4]]) e2 = stats.covariance_ellipse(cov2) cov3 = np.array([[12, 11.95], [11.95, 12]]) e3 = stats.covariance_ellipse(cov3) sigma = [1, 4, 9] if count >= 1: stats.plot_covariance_ellipse((0, 0), ellipse=e, variance=sigma) if count == 2 or count == 3: stats.plot_covariance_ellipse((5, 5), ellipse=e, variance=sigma) if count == 3: stats.plot_covariance_ellipse((5, 5), ellipse=e3, variance=sigma, edgecolor='r') if count == 4: M1 = np.array([[5, 5]]).T m4, cov4 = stats.multivariate_multiply(M1, cov2, M1, cov3) e4 = stats.covariance_ellipse(cov4) stats.plot_covariance_ellipse((5, 5), ellipse=e, variance=sigma, alpha=0.25) stats.plot_covariance_ellipse((5, 5), ellipse=e3, variance=sigma, edgecolor='r', alpha=0.25) stats.plot_covariance_ellipse(m4[:, 0], ellipse=e4, variance=sigma) plt.ylim((-9, 16)) #plt.ylim([0,11]) #plt.xticks(np.arange(1,4,1)) plt.xlabel("Position") plt.ylabel("Velocity") plt.show()
def show_x_error_chart(count): """ displays x=123 with covariances showing error""" plt.cla() plt.gca().autoscale(tight=True) cov = np.array([[0.03,0], [0,8]]) e = stats.covariance_ellipse (cov) cov2 = np.array([[0.03,0], [0,4]]) e2 = stats.covariance_ellipse (cov2) cov3 = np.array([[12,11.95], [11.95,12]]) e3 = stats.covariance_ellipse (cov3) sigma=[1, 4, 9] if count >= 1: stats.plot_covariance_ellipse ((0,0), ellipse=e, variance=sigma) if count == 2 or count == 3: stats.plot_covariance_ellipse ((5,5), ellipse=e, variance=sigma) if count == 3: stats.plot_covariance_ellipse ((5,5), ellipse=e3, variance=sigma, edgecolor='r') if count == 4: M1 = np.array([[5, 5]]).T m4, cov4 = stats.multivariate_multiply(M1, cov2, M1, cov3) e4 = stats.covariance_ellipse (cov4) stats.plot_covariance_ellipse ((5,5), ellipse=e, variance=sigma, alpha=0.25) stats.plot_covariance_ellipse ((5,5), ellipse=e3, variance=sigma, edgecolor='r', alpha=0.25) stats.plot_covariance_ellipse (m4[:,0], ellipse=e4, variance=sigma) plt.ylim((-9, 16)) #plt.ylim([0,11]) #plt.xticks(np.arange(1,4,1)) plt.xlabel("Position") plt.ylabel("Velocity") plt.show()
track = [] for i, u in enumerate(cmds): sim_pos = move(sim_pos, dt/step, u, wheelbase) track.append(sim_pos) if i % step == 0: try: ukf.predict(u=u, wheelbase=wheelbase) except: # force positive definite matrix ukf.P = np.matmul(ukf.P, ukf.P.T) ukf.predict(u=u, wheelbase=wheelbase) if i % ellipse_step == 0: plot_covariance_ellipse( (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6, facecolor='k', alpha=0.3) x, y = sim_pos[0], sim_pos[1] z = [] for lmark in landmarks: dx, dy = lmark[0] - x, lmark[1] - y d = np.sqrt(dx**2 + dy**2) + np.random.randn()*sigma_range bearing = np.arctan2(lmark[1] - y, lmark[0] - x) a = (normalize_angle(bearing - sim_pos[2] + np.random.randn()*sigma_bearing)) z.extend([d, a]) ukf.update(z, landmarks=landmarks) if i % ellipse_step == 0: plot_covariance_ellipse(
def plot1(): stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2)
def show_x_error_chart(count): """ displays x=123 with covariances showing error""" plt.cla() plt.gca().autoscale(tight=True) cov = np.array([[0.1, 0], [0, 8]]) pos_ellipse = stats.covariance_ellipse(cov) cov2 = np.array([[0.1, 0], [0, 4]]) cov3 = np.array([[12, 11.95], [11.95, 12]]) vel_ellipse = stats.covariance_ellipse(cov3) sigma = [1, 4, 9] if count < 4: stats.plot_covariance_ellipse((0, 0), ellipse=pos_ellipse, variance=sigma) if count == 2: stats.plot_covariance_ellipse((0, 0), ellipse=vel_ellipse, variance=sigma, edgecolor='r') if count == 3: stats.plot_covariance_ellipse((5, 5), ellipse=pos_ellipse, variance=sigma) stats.plot_covariance_ellipse((0, 0), ellipse=vel_ellipse, variance=sigma, edgecolor='r') if count == 4: M0 = np.array([[0, 0]]).T M1 = np.array([[5, 5]]).T _, cov4 = stats.multivariate_multiply(M0, cov2, M1, cov3) e4 = stats.covariance_ellipse(cov4) stats.plot_covariance_ellipse((0, 0), ellipse=pos_ellipse, variance=sigma, alpha=0.25) stats.plot_covariance_ellipse((5, 5), ellipse=pos_ellipse, variance=sigma, alpha=0.25) stats.plot_covariance_ellipse((0, 0), ellipse=vel_ellipse, variance=sigma, edgecolor='r', alpha=0.25) stats.plot_covariance_ellipse((5, 5), ellipse=e4, variance=sigma) plt.ylim((-9, 16)) plt.xlabel("Position") plt.ylabel("Velocity") plt.axis('equal') plt.show()