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 = UKF.weights(2, kappa) sp1 = UKF.weights(2, kappa*1000) Xi0 = UKF.sigma_points (x, P, kappa) Xi1 = UKF.sigma_points (x, P, kappa*1000) 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])*sp0[i] + x[0, 0], (Xi0[i,1]-x[0, 1])*sp0[i] + x[0, 1], color='blue') for i in range(Xi1.shape[0]): plt.scatter((Xi1[i, 0]-x[0, 0]) * sp1[i] + x[0,0], (Xi1[i, 1]-x[0, 1]) * sp1[i] + x[0,1], color='green') stats.plot_covariance_ellipse([1, 2], P)
def test_batch_missing_data(): """ batch filter should accept missing data with None in the measurements """ def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = MerweScaledSigmaPoints(4, .1, 2., -1) kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([-1., 1., -1., 1]) kf.P*=0.0001 zs = [] for i in range(20): z = np.array([i+randn()*0.1, i+randn()*0.1]) zs.append(z) zs[2] = None Rs = [1]*len(zs) Rs[2] = None Ms, Ps = kf.batch_filter(zs)
def test_sigma_points_1D(): """ tests passing 1D data into sigma_points""" kappa = 0. ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa) points = ukf.weights(1, 0.) assert len(points) == 3 mean = 5 cov = 9 Xi = ukf.sigma_points (mean, cov, kappa) xm, ucov = unscented_transform(Xi, ukf.W, ukf.W, 0) # sum of weights*sigma points should be the original mean m = 0.0 for x,w in zip(Xi, ukf.W): m += x*w assert abs(m-mean) < 1.e-12 assert abs(xm[0] - mean) < 1.e-12 assert abs(ucov[0,0]-cov) < 1.e-12 assert Xi.shape == (3,1) assert len(ukf.W) == 3
def test_linear_2d(): """ should work like a linear KF if problem is linear """ def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = MerweScaledSigmaPoints(4, .1, 2., -1) kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([-1., 1., -1., 1]) kf.P*=0.0001 #kf.R *=0 #kf.Q zs = [] for i in range(20): z = np.array([i+randn()*0.1, i+randn()*0.1]) zs.append(z) Ms, Ps = kf.batch_filter(zs) smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt)
def sensor_fusion_kf(): global hx, fx # create unscented Kalman filter with large initial uncertainty points = JulierSigmaPoints(2, kappa=2) kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, points=points) kf.x = np.array([100, 100.]) kf.P *= 40 return kf
def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)): n_state = len(Q) n_meas = len(R) sigmas = SigmaPoints(n_state, alpha=.1, beta=2., kappa=1.) ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal, hx=h_kal, dt=dt, points=sigmas) ukf.Q = Q ukf.R = R self.ukf = ukf self.isFirst = True
def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)): n_state = len(Q) n_meas = len(R) sigmas = SigmaPoints(n_state, alpha=.5, beta=2., kappa=0.) ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal_accel, hx=h_kal_accel, dt=dt, points=sigmas, x_mean_fn = state_mean, residual_x=res_x, residual_z=res_x) ukf.Q = Q ukf.R = R self.ukf = ukf self.isFirst = True
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 __init__(self, trueTrajectory, dt, route, Q=np.eye(2), R=np.eye(2)): #from filterpy.kalman import KalmanFilter as KF from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints n_state = len(Q) n_meas = len(R) sigmas = SigmaPoints(n_state, alpha=.1, beta=2., kappa=0.) ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal_v, hx=h_kal, dt=dt, points=sigmas) ukf.Q = Q ukf.R = R self.ukf = ukf self.isFirst = True self.route = route
def show_sigma_transform(): 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', variance=9, alpha=0.5) S = UKF.sigma_points(x=x, P=P, kappa=0) 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.5) 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) #plt.axis('equal') plt.show()
def test_saver_UKF(): def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = MerweScaledSigmaPoints(4, .1, 2., -1) kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) z_std = 0.1 kf.R = np.diag([z_std**2, z_std**2]) # 1 standard kf.x = np.array([-1., 1., -1., 1]) kf.P *= 1. zs = [[i, i] for i in range(40)] s = Saver(kf, skip_private=False, skip_callable=False, ignore=['z_mean']) for z in zs: kf.predict() kf.update(z) #print(kf.x, kf.log_likelihood, kf.P.diagonal()) s.save() s.to_array()
def __init__(self, trueTrajectory, dt=.1, noise = 0.): from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints self.dt = dt sigmas = SigmaPoints(3, alpha=.1, beta=2., kappa=0.) self.KF = UKF(dim_x=3, dim_z=2, fx=f_kal_a, hx=h_kal, dt=dt, points=sigmas) self.KF.Q = np.diag([1., 0.5, 0.2]) self.KF.R = np.diag([2., 1.12])*noise + np.diag([.05, .05]) self.first = True
def test_linear_2d(): """ should work like a linear KF if problem is linear """ def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, kappa=0) kf.x = np.array([-1., 1., -1., 1]) kf.P*=0.0001 #kf.R *=0 #kf.Q zs = [] for i in range(20): z = np.array([i+randn()*0.1, i+randn()*0.1]) zs.append(z) Ms, Ps = kf.batch_filter(zs) smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt) if DO_PLOT: zs = np.asarray(zs) #plt.plot(zs[:,0]) plt.plot(Ms[:,0]) plt.plot(smooth_x[:,0], smooth_x[:,2]) print(smooth_x)
def estimateUKF(camPoses): points = MerweScaledSigmaPoints(3,.1,2.,0.) filter = UKF(3,3,0,h, f, points, sqrt_fn=None, x_mean_fn=state_mean, z_mean_fn=state_mean, residual_x=residual, residual_z=residual) filter.P = np.diag([0.04,0.04,0.003]) filter.Q = np.diag([(0.2)**2,(0.2)**2,(1*3.14/180)**2]) filter.R = np.diag([100,100,0.25]) Uposes = [[],[]] for i in range(len(speed)): x = filter.x Uposes[0].append(x[0]) Uposes[1].append(x[1]) filter.predict(fx_args=[speed[i],angle[i]*0.01745]) filter.update(z = [camPoses[0][i],camPoses[1][i],camPoses[2][i]]) return Uposes
def test_1d(): def fx(x, dt): F = np.array([[1., dt], [0, 1]]) return np.dot(F, x) def hx(x): return np.array([[x[0]]]) ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx) ckf.x = np.array([[1.], [2.]]) ckf.P = np.array([[1, 1.1], [1.1, 3]]) ckf.R = np.eye(1) * .05 ckf.Q = np.array([[0., 0], [0., .001]]) dt = 0.1 points = MerweScaledSigmaPoints(2, .1, 2., -1) kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([1, 2]) kf.P = np.array([[1, 1.1], [1.1, 3]]) kf.R *= 0.05 kf.Q = np.array([[0., 0], [0., .001]]) for i in range(50): z = np.array([[i+randn()*0.1]]) #xx, pp, Sx = predict(f, x, P, Q) #x, P = update(h, z, xx, pp, R) ckf.predict() ckf.update(z) kf.predict() kf.update(z[0]) assert abs(ckf.x[0] -kf.x[0]) < 1e-10 assert abs(ckf.x[1] -kf.x[1]) < 1e-10 plt.show()
def test_1d(): def fx(x, dt): F = np.array([[1., dt], [0, 1]]) return np.dot(F, x) def hx(x): return x[0:1] ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx) ckf.x = np.array([[1.], [2.]]) ckf.P = np.array([[1, 1.1], [1.1, 3]]) ckf.R = np.eye(1) * .05 ckf.Q = np.array([[0., 0], [0., .001]]) dt = 0.1 points = MerweScaledSigmaPoints(2, .1, 2., -1) kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([1, 2]) kf.P = np.array([[1, 1.1], [1.1, 3]]) kf.R *= 0.05 kf.Q = np.array([[0., 0], [0., .001]]) s = Saver(kf) for i in range(50): z = np.array([[i+randn()*0.1]]) ckf.predict() ckf.update(z) kf.predict() kf.update(z[0]) assert abs(ckf.x[0] - kf.x[0]) < 1e-10 assert abs(ckf.x[1] - kf.x[1]) < 1e-10 s.save() # test mahalanobis a = np.zeros(kf.y.shape) maha = scipy_mahalanobis(a, kf.y, kf.SI) assert kf.mahalanobis == approx(maha) s.to_array()
def test_radar(): def fx(x, dt): A = np.eye(3) + dt * np.array ([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) return A.dot(x) def hx(x): return np.sqrt (x[0]**2 + x[2]**2) dt = 0.05 sp = JulierSigmaPoints(n=3, kappa=0.) kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp) kf.Q *= 0.01 kf.R = 10 kf.x = np.array([0., 90., 1100.]) kf.P *= 100. radar = RadarSim(dt) t = np.arange(0,20+dt, dt) n = len(t) xs = np.zeros((n,3)) random.seed(200) rs = [] #xs = [] for i in range(len(t)): r = radar.get_range() #r = GetRadar(dt) kf.predict() kf.update(z=[r]) xs[i,:] = kf.x rs.append(r) if DO_PLOT: print(xs[:,0].shape) plt.figure() plt.subplot(311) plt.plot(t, xs[:,0]) plt.subplot(312) plt.plot(t, xs[:,1]) plt.subplot(313) plt.plot(t, xs[:,2])
def __init__(self, deltaT, measurementNoiseStd): self.updatedPredictions = [] self.mus = [] """ First init constant linear model """ points1 = MerweScaledSigmaPoints(5, alpha=0.0025, beta=2., kappa=0) self.constantLinearModel = UnscentedKalmanFilter( dim_x=5, dim_z=2, dt=deltaT, fx=f_unscented_linearModel, hx=h_unscented_linearModel, points=points1) self.constantLinearModel.x = np.array([0.01, 0.01, 0.01, 0.01, 0]) self.constantLinearModel.P = np.eye(5) * (measurementNoiseStd**2) / 2.0 self.constantLinearModel.R = np.eye(2) * (measurementNoiseStd**2) self.constantLinearModel.Q = np.diag([0.003, 0.003, 6e-4, 0.004, 0]) """ Second init constant turn rate model """ points2 = MerweScaledSigmaPoints(5, alpha=0.0025, beta=2., kappa=0) self.constantTurnRateModel = UnscentedKalmanFilter( dim_x=5, dim_z=2, dt=deltaT, fx=f_unscented_turnRateModel, hx=h_unscented_turnRateModel, points=points2) self.constantTurnRateModel.x = np.array( [0.01, 0.01, 0.01, 0.001, 1e-5]) self.constantTurnRateModel.P = np.eye(5) * (measurementNoiseStd** 2) / 2.0 self.constantTurnRateModel.R = np.eye(2) * (measurementNoiseStd**2) self.constantTurnRateModel.Q = np.diag( [1e-24, 1e-24, 1e-3, 4e-3, 1e-10]) """ Third init random motion model """ points3 = MerweScaledSigmaPoints(5, alpha=0.0025, beta=2., kappa=0) self.randomModel = UnscentedKalmanFilter(dim_x=5, dim_z=2, dt=deltaT, fx=f_unscented_randomModel, hx=h_unscented_randomModel, points=points3) self.randomModel.x = np.array([0.01, 0.01, 0.01, 0.001, 1e-5]) self.randomModel.P = np.eye(5) * (measurementNoiseStd**2) / 2.0 self.randomModel.R = np.eye(2) * (measurementNoiseStd**2) self.randomModel.Q = np.diag([1, 1, 1e-24, 1e-24, 1e-24]) #############################33 if (1): filters = [self.constantLinearModel, self.constantTurnRateModel] mu = [0.5, 0.5] trans = np.array([[0.9, 0.1], [0.1, 0.9]]) self.imm = IMMEstimator(filters, mu, trans) else: filters = [ self.constantLinearModel, self.constantTurnRateModel, self.randomModel ] mu = [0.34, 0.33, 0.33] trans = np.array([[0.9, 0.05, 0.05], [0.05, 0.9, 0.05], [0.05, 0.05, 0.9]]) self.imm = IMMEstimator(filters, mu, trans)
[0., 0., 1.]]) return np.dot(F, x) def hx(x): """ returns slant range based on downrange distance and altitude""" return (x[0]**2 + x[2]**2)**.5 if __name__ == "__main__": dt = 0.05 radarUKF = UKF(dim_x=3, dim_z=1, dt=dt, kappa=0.) radarUKF.Q *= Q_discrete_white_noise(3, 1, .01) radarUKF.R *= 10 radarUKF.x = np.array([0., 90., 1100.]) radarUKF.P *= 100. t = np.arange(0, 20+dt, dt) n = len(t) xs = [] rs = [] for i in range(n): r = GetRadar(dt) rs.append(r) radarUKF.update(r, hx, fx)
#m = array([[5, 10], [10, 5], [15, 15], [20, 5],[5,5], [8, 8.4]])#, [0, 30], [50, 30], [40, 10]]) #m = array([[5, 10], [10, 5]])#, [0, 30], [50, 30], [40, 10]]) #m = array([[5., 10], [10, 5]]) #m = array([[5., 10], [10, 5]]) sigma_r = .3 sigma_h = .1#radians(.5)#np.radians(1) #sigma_steer = radians(10) dt = 0.1 wheelbase = 0.5 points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0, subtract=residual_x) #points = JulierSigmaPoints(n=3, kappa=3) ukf= UKF(dim_x=3, dim_z=2*len(m), 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 = array([2, 6, .3]) ukf.P = np.diag([.1, .1, .05]) ukf.R = np.diag([sigma_r**2, sigma_h**2]* len(m)) ukf.Q =np.eye(3)*.00001 u = array([1.1, 0.]) xp = ukf.x.copy() plt.cla() plt.scatter(m[:, 0], m[:, 1])
def test_julier_weights(): for n in range(1,15): for k in np.linspace(0,5,0.1): Wm = UKF.weights(n, k) assert abs(sum(Wm) - 1) < 1.e-12
def test_linear_1d(): """ should work like a linear KF if problem is linear """ def fx(x, dt): F = np.array([[1., dt], [0, 1]]) return np.dot(F, x) def hx(x): return np.array([x[0]]) dt = 0.1 points = MerweScaledSigmaPoints(2, .1, 2., -1) kf = UnscentedKalmanFilter(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([1, 2]) kf.P = np.array([[1, 1.1], [1.1, 3]]) kf.R *= 0.05 kf.Q = np.array([[0., 0], [0., .001]]) z = np.array([2.]) kf.predict() kf.update(z) zs = [] for i in range(50): z = np.array([i + randn()*0.1]) zs.append(z) kf.predict() kf.update(z) print('K', kf.K.T) print('x', kf.x)
def test_rts(): def fx(x, dt): A = np.eye(3) + dt * np.array ([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) f = np.dot(A, x) return f def hx(x): return np.sqrt (x[0]**2 + x[2]**2) dt = 0.05 kf = UKF(3, 1, dt, fx=fx, hx=hx, kappa=1.) kf.Q *= 0.01 kf.R = 10 kf.x = np.array([0., 90., 1100.]) kf.P *= 100. radar = RadarSim(dt) t = np.arange(0,20+dt, dt) n = len(t) xs = np.zeros((n,3)) random.seed(200) rs = [] #xs = [] for i in range(len(t)): r = radar.get_range() #r = GetRadar(dt) kf.predict() kf.update(z=[r]) xs[i,:] = kf.x rs.append(r) kf.x = np.array([0., 90., 1100.]) kf.P = np.eye(3)*100 M, P = kf.batch_filter(rs) assert np.array_equal(M, xs), "Batch filter generated different output" Qs = [kf.Q]*len(t) M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs) if DO_PLOT: print(xs[:,0].shape) plt.figure() plt.subplot(311) plt.plot(t, xs[:,0]) plt.plot(t, M2[:,0], c='g') plt.subplot(312) plt.plot(t, xs[:,1]) plt.plot(t, M2[:,1], c='g') plt.subplot(313) plt.plot(t, xs[:,2]) plt.plot(t, M2[:,2], c='g')
def test_fixed_lag(): def fx(x, dt): A = np.eye(3) + dt * np.array([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) f = np.dot(A, x) return f def hx(x): return [np.sqrt(x[0]**2 + x[2]**2)] dt = 0.05 sp = JulierSigmaPoints(n=3, kappa=0) kf = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp) kf.Q *= 0.01 kf.R = 10 kf.x = np.array([0., 90., 1100.]) kf.P *= 1. radar = RadarSim(dt) t = np.arange(0, 20 + dt, dt) n = len(t) xs = np.zeros((n, 3)) random.seed(200) rs = [] M = [] P = [] N = 10 flxs = [] for i in range(len(t)): r = radar.get_range() kf.predict() kf.update(z=[r]) xs[i, :] = kf.x flxs.append(kf.x) rs.append(r) M.append(kf.x) P.append(kf.P) print(i) if i == 20 and len(M) >= N: try: M2, P2, K = kf.rts_smoother(Xs=np.asarray(M)[-N:], Ps=np.asarray(P)[-N:]) flxs[-N:] = M2 except: print('except', i) kf.x = np.array([0., 90., 1100.]) kf.P = np.eye(3) * 100 M, P = kf.batch_filter(rs) Qs = [kf.Q]*len(t) M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs) flxs = np.asarray(flxs) print(xs[:, 0].shape) plt.figure() plt.subplot(311) plt.plot(t, xs[:, 0]) plt.plot(t, flxs[:, 0], c='r') plt.plot(t, M2[:, 0], c='g') plt.subplot(312) plt.plot(t, xs[:, 1]) plt.plot(t, flxs[:, 1], c='r') plt.plot(t, M2[:, 1], c='g') plt.subplot(313) plt.plot(t, xs[:, 2]) plt.plot(t, flxs[:, 2], c='r') plt.plot(t, M2[:, 2], c='g')
sigma_bearing=0.1 # x = [1.3905235974633778, 7.832049402391674, 9.020314462351989, 3.687485010010496, 4.933087914504979, 5.842278721831094, 7.374384722598193, 11.548435059429593, 10.08330407331331, 4.048332216167037, 11.753400231528877, 5.24814930818585, 0.3780861370621249, 3.819226471192638, 3.219009627591963, -0.660991670650813, -6.810902143188039, -2.648632039619782, -0.8651469995661083, -5.882826870804094, -6.771881598941817, -6.883212384180803, -10.970288387596014, -9.38416048883138, -2.0041408634734, -4.1999609273146294, -4.427392485133488, -4.094107923812431, -4.539507495972897, -6.958816977357863, -0.5837156626237164, 3.211904185873981, 2.6999090897786973, 4.946462347786874, 6.250544373196611, 1.4360007457367132, 6.376447944684558, 5.395872942944427, 3.6592834591198713, 3.6799054753035985, 6.372070482657179, 10.83971503990248, 6.471684767984395, -4.13122094642976, 1.3657905443071188, -2.1545837703277186, -3.6218498314375362, -5.990316163817381, -2.825131401522447, -8.47962049553197, -3.0710249464300077, -5.242374500395425, -11.356227320748491, -3.580215175474069, -8.949524398862625, -12.148475712671257, -3.541357682569754, -2.3218551317498113, -4.10750098248105, -1.154362522019348, 1.719708807469195, -2.9317706605909493, 5.581580285882415, 7.322885401113478, 8.035547065332059, 7.115626723235949, 6.8834109751060195, 9.605681429819468, 5.378480946301948, 4.55936497171603, 0.9696632799355314, 3.0281397205843197, -2.4315041509242974, -0.4213417059299991, 0.13371487118430309, -2.5616840637149783, -3.330922819747191, -3.238792470549532, -12.848697164298123, -11.650132469341331, -2.9250345334239203, -8.844969766009035, -7.54940178448948, -10.204223020208033, -7.4851192397126525, -9.156624882388012, -6.366425519643718, -8.553734125327592, -4.609302566545153, -4.264881667574036, 1.4533519725675805] # y = [5.1592964225372135, 8.216934337621904, 14.221677454416298, 9.606966741201592, 15.393598119630198, 15.463991661062682, 18.798461424361072, 17.83425651827887, 16.480645145332982, 22.97048479933387, 19.510051232214657, 24.642699320629887, 25.45829257417096, 25.497397786842278, 22.450305133766594, 21.947846746481055, 18.037770599424036, 23.858011123610375, 19.308656588152285, 16.69873918296076, 20.488408553230123, 16.367880867164754, 23.654044378766407, 20.73648776720881, 15.90450795825495, 16.135338583542634, 8.493714748632618, 8.913938224865586, 8.734285273935438, 8.050993570472942, 10.158164230122795, 8.08449276240243, 14.886302280598855, 14.237310205824675, 17.306486687897937, 13.8601024213346, 17.90588766410904, 15.869047349619871, 20.686353443244506, 18.42398071352659, 23.22554467544738, 21.198830063607687, 24.727001975565397, 23.188652263477202, 25.512192853204706, 25.07747201310473, 24.054712728944807, 22.68899006890544, 22.641944066718526, 18.33228864387304, 17.94999176153331, 22.10590333682616, 19.005801408713417, 13.582191596376182, 10.639031293990215, 15.577633926934181, 9.911438955202737, 8.666040822401431, 11.027340108357107, 12.56573800372876, 11.771468252890216, 12.46635569231553, 8.986038622183006, 12.215863938535502, 10.291879983793017, 16.505270167068254, 19.6009636347262, 19.10972604819267, 15.892910888479538, 26.63984263799601, 17.951774058138326, 21.107945349676598, 23.441244468726836, 21.853959925952115, 21.162499359087953, 21.808181391384416, 20.52011983367284, 19.153098462721978, 26.471740153772178, 25.208554508505728, 17.67356964440606, 18.51548339755294, 16.646498463883713, 15.599794378503121, 17.747778170685166, 13.998749493955843, 13.588001144109704, 16.94352162086974, 9.476077823803571, 5.4278110057897955, 12.342893333479008] # x_actual = [0.0, 1.4672214011007085, 2.83753958756461, 4.0510650791270315, 5.054760988665319, 5.80476098866532, 6.268286480227742, 6.4250791751292216, 6.268286480227741, 5.804760988665318, 5.054760988665317, 4.051065079127029, 2.837539587564608, 1.4672214011007072, -8.881784197001252e-16, -1.5000000000000009, -2.9672214011007103, -4.337539587564613, -5.551065079127038, -6.554760988665329, -7.304760988665335, -7.768286480227762, -7.925079175129248, -7.768286480227774, -7.3047609886653575, -6.55476098866536, -5.551065079127074, -4.3375395875646525, -2.9672214011007503, -1.500000000000041, -4.107825191113079e-14, 1.4672214011006668, 2.837539587564567, 4.051065079126986, 5.054760988665272, 5.8047609886652705, 6.268286480227689, 6.4250791751291665, 6.268286480227683, 5.804760988665258, 5.0547609886652545, 4.051065079126965, 2.8375395875645424, 1.4672214011006404, -6.816769371198461e-14, -1.5000000000000682, -2.967221401100777, -4.337539587564679, -5.551065079127102, -6.554760988665391, -7.304760988665395, -7.76828648022782, -7.925079175129303, -7.768286480227826, -7.304760988665407, -6.55476098866541, -5.551065079127124, -4.337539587564702, -2.9672214011008, -1.5000000000000908, -9.08162434143378e-14, 1.467221401100617, 2.837539587564517, 4.0510650791269365, 5.0547609886652225, 5.804760988665221, 6.26828648022764, 6.425079175129117, 6.2682864802276335, 5.804760988665208, 5.054760988665205, 4.051065079126915, 2.8375395875644926, 1.4672214011005906, -1.1790568521519162e-13, -1.500000000000118, -2.9672214011008267, -4.337539587564729, -5.551065079127151, -6.554760988665441, -7.3047609886654445, -7.76828648022787, -7.925079175129353, -7.768286480227876, -7.304760988665457, -6.55476098866546, -5.551065079127174, -4.337539587564752, -2.9672214011008498, -1.5000000000001406, -1.4055423491754482e-13] # y_actual = [10.0, 10.31186753622664, 10.92197250084034, 11.803650379279048, 12.91836761749514, 14.217405723171797, 15.643990497614528, 17.135773340666937, 18.627556183719346, 20.054140958162076, 21.353179063838734, 22.467896302054825, 23.349574180493534, 23.959679145107234, 24.271546681333874, 24.271546681333877, 23.95967914510724, 23.349574180493544, 22.46789630205484, 21.353179063838752, 20.054140958162098, 18.62755618371937, 17.13577334066696, 15.643990497614551, 14.217405723171819, 12.918367617495159, 11.803650379279066, 10.921972500840356, 10.311867536226657, 10.000000000000021, 10.000000000000025, 10.311867536226666, 10.921972500840369, 11.80365037927908, 12.918367617495173, 14.217405723171833, 15.643990497614563, 17.135773340666972, 18.62755618371938, 20.05414095816211, 21.353179063838766, 22.467896302054854, 23.349574180493562, 23.95967914510726, 24.2715466813339, 24.2715466813339, 23.95967914510726, 23.349574180493562, 22.467896302054854, 21.353179063838766, 20.05414095816211, 18.62755618371938, 17.135773340666972, 15.643990497614562, 14.217405723171831, 12.918367617495171, 11.803650379279079, 10.921972500840369, 10.31186753622667, 10.000000000000034, 10.000000000000037, 10.311867536226679, 10.921972500840381, 11.803650379279093, 12.918367617495186, 14.217405723171845, 15.643990497614576, 17.135773340666987, 18.627556183719395, 20.054140958162126, 21.35317906383878, 22.467896302054868, 23.349574180493576, 23.959679145107273, 24.271546681333913, 24.271546681333913, 23.959679145107273, 23.349574180493576, 22.467896302054868, 21.35317906383878, 20.054140958162126, 18.627556183719395, 17.135773340666987, 15.643990497614576, 14.217405723171845, 12.918367617495186, 11.803650379279093, 10.921972500840383, 10.311867536226684, 10.000000000000048, 10.000000000000052] x = [-3.422319443527482, -0.3670766780148629, 0.42810463566372503, 7.971751432584803, 5.561623301852658, 5.122631477269733, 7.9380784789008345, 5.724443001813307, 3.9453729476132224, 7.805691945653483, 7.103981420312309, 3.6438350482459896, 2.307982454031695, 3.700120851211732, 2.659684755972816, -3.5930177497743228, -3.5940690041937104, -7.197420705291179, -3.320401730928432, -9.86609819752164, -7.080369683242467, -6.127129784273851, -8.85849826178098, -6.263547220912284, -8.512470940768917, -4.743424343342259, -5.395873837722023, -7.8941877456860885, -2.6621170295142553, -6.09164203736454, -1.320245983651153] y = [14.16729015960461, 18.105403721893914, 11.857680736011378, 11.965905696440576, 12.378397170235502, 17.813946679056244, 20.347459881502157, 24.746811440113035, 25.764577855024726, 29.784943985662252, 22.03292253903822, 26.837906066246074, 27.727039870771407, 27.711933191437307, 34.07656757469763, 28.72982212627928, 28.284308305957804, 29.184032547849366, 26.636778632724987, 27.76168258806925, 27.716226098275158, 25.345843624215934, 14.664004496070973, 20.461258499585742, 18.967501506449352, 14.098137295901621, 14.072033819348718, 15.520102110851612, 16.343599524337055, 15.096377045760601, 15.52746451234623] x_actual = [0.0, 1.4672214011007085, 2.83753958756461, 4.0510650791270315, 5.054760988665319, 5.80476098866532, 6.268286480227742, 6.4250791751292216, 6.268286480227741, 5.804760988665318, 5.054760988665317, 4.051065079127029, 2.837539587564608, 1.4672214011007072, -8.881784197001252e-16, -1.5000000000000009, -2.9672214011007103, -4.337539587564613, -5.551065079127038, -6.554760988665329, -7.304760988665335, -7.768286480227762, -7.925079175129248, -7.768286480227774, -7.3047609886653575, -6.55476098866536, -5.551065079127074, -4.3375395875646525, -2.9672214011007503, -1.500000000000041, -4.107825191113079e-14] y_actual = [15.0, 15.31186753622664, 15.92197250084034, 16.80365037927905, 17.91836761749514, 19.217405723171797, 20.643990497614528, 22.135773340666937, 23.627556183719346, 25.054140958162076, 26.353179063838734, 27.467896302054825, 28.349574180493534, 28.959679145107234, 29.271546681333874, 29.271546681333877, 28.95967914510724, 28.349574180493544, 27.46789630205484, 26.353179063838752, 25.054140958162098, 23.62755618371937, 22.13577334066696, 20.643990497614553, 19.217405723171822, 17.918367617495164, 16.803650379279073, 15.921972500840363, 15.311867536226664, 15.000000000000028, 15.000000000000032] points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0, subtract=residual_x) ukf = UKF(dim_x = 3, dim_z = 2, 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([1.3905235974633778, 7.832049402391674, 0.]) ukf.P = np.diag([.1, .1, .05]) ukf.R = np.diag( [sigma_range**2, sigma_bearing**2] ) ukf.Q = np.eye(3) * 0.0001 state = ukf.x.copy() for i in range(1, len(x)): state = x_actual[i], y_actual[i] #state = move(state, dt, turning) ukf.predict(dt = 1., fx_args = (1.5, 2*pi / 30))
F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): # measurement function - convert state into a measurement # where measurements are [x_pos, y_pos] return np.array([x[0], x[2]]) dt = 1 # create sigma points to use in the filter. This is standard for Gaussian processes points = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1) kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([0., 0., 0., 0]) # initial state kf.P *= 0.1 # initial uncertainty z_std = 0.1 kf.R = np.diag([z_std**2, z_std**2]) # 1 standard kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01**2, block_size=2) errs=0 count=0 P=lx.Project(mode='2D',solver='LSE') filepath="lowlow" vel = {} position = {}
def test_linear_rts(): """ for a linear model the Kalman filter and UKF should produce nearly identical results. Test code mostly due to user gboehl as reported in GitHub issue #97, though I converted it from an AR(1) process to constant velocity kinematic model. """ dt = 1.0 F = np.array([[1., dt], [.0, 1]]) H = np.array([[1., .0]]) def t_func(x, dt): F = np.array([[1., dt], [.0, 1]]) return np.dot(F, x) def o_func(x): return np.dot(H, x) sig_t = .1 # peocess sig_o = .0001 # measurement N = 50 X_true, X_obs = [], [] for i in range(N): X_true.append([i + 1, 1.]) X_obs.append(i + 1 + np.random.normal(scale=sig_o)) X_true = np.array(X_true) X_obs = np.array(X_obs) oc = np.ones((1, 1)) * sig_o**2 tc = np.zeros((2, 2)) tc[1, 1] = sig_t**2 tc = Q_discrete_white_noise(dim=2, dt=dt, var=sig_t**2) points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=2., kappa=1) ukf = UnscentedKalmanFilter(dim_x=2, dim_z=1, dt=dt, hx=o_func, fx=t_func, points=points) ukf.x = np.array([0., 1.]) ukf.R = np.copy(oc) ukf.Q = np.copy(tc) s = Saver(ukf) s.save() s.to_array() kf = KalmanFilter(dim_x=2, dim_z=1) kf.x = np.array([[0., 1]]).T kf.R = np.copy(oc) kf.Q = np.copy(tc) kf.H = np.copy(H) kf.F = np.copy(F) mu_ukf, cov_ukf = ukf.batch_filter(X_obs) x_ukf, _, _ = ukf.rts_smoother(mu_ukf, cov_ukf) mu_kf, cov_kf, _, _ = kf.batch_filter(X_obs) x_kf, _, _, _ = kf.rts_smoother(mu_kf, cov_kf) # check results of filtering are correct kfx = mu_kf[:, 0, 0] ukfx = mu_ukf[:, 0] kfxx = mu_kf[:, 1, 0] ukfxx = mu_ukf[:, 1] dx = kfx - ukfx dxx = kfxx - ukfxx # error in position should be smaller then error in velocity, hence # atol is different for the two tests. assert np.allclose(dx, 0, atol=1e-7) assert np.allclose(dxx, 0, atol=1e-6) # now ensure the RTS smoothers gave nearly identical results kfx = x_kf[:, 0, 0] ukfx = x_ukf[:, 0] kfxx = x_kf[:, 1, 0] ukfxx = x_ukf[:, 1] dx = kfx - ukfx dxx = kfxx - ukfxx assert np.allclose(dx, 0, atol=1e-7) assert np.allclose(dxx, 0, atol=1e-6) return ukf
x[1] = x[1] + x[3]*dt x[2] = x[2] x[3] = x[3] + ((0.0034 * g * np.exp(-x[1] / 22000) * x[3] ** 2) / (2 * b) - g)*dt return x def h_cv(x): """Measurement function - measuring only position""" return np.array([x[0], x[1]]) starting_conditions = [100., 1.00e4, 0., 0., 200.] points = MerweScaledSigmaPoints(n=4, alpha=.0001, beta=2., kappa=-1) ukf = UKF(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, points=points) ukf.x = np.array([1000., 1.00e3, 0., 0.]) ukf.R = np.diag([[std_x[0]**2, std_x[0]**2]]) #ukf.H = np.array([[1]]) ukf.Q = np.diag([100., 100., 1000., 1000.]) uxs = [] trux = [] truv = [] measx = [] time = [] covarx = [] covarv = [] zs = np.arange(0, 1435 + dt, dt) f = falling_object(starting_conditions[0:2], starting_conditions[2:4], starting_conditions[4], std_x, std_model) t = 0
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=move, 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, dt / step, u, wheelbase) track.append(sim_pos) if i % step == 0: 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 = 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, landmarks=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
F = np.array([[1., dt, 0.], [0., 1., 0.], [0., 0., 1.]]) return np.dot(F, x) def hx(x): """ returns slant range based on downrange distance and altitude""" return (x[0]**2 + x[2]**2)**.5 if __name__ == "__main__": dt = 0.05 radarUKF = UKF(dim_x=3, dim_z=1, dt=dt, kappa=0.) radarUKF.Q *= Q_discrete_white_noise(3, 1, .01) radarUKF.R *= 10 radarUKF.x = np.array([0., 90., 1100.]) radarUKF.P *= 100. t = np.arange(0, 20 + dt, dt) n = len(t) xs = [] rs = [] for i in range(n): r = GetRadar(dt) rs.append(r) radarUKF.update(r, hx, fx)
# define display window name windowName = "Kalman Object Tracking" # window name windowName2 = "Hue histogram back projection" # window name windowNameSelection = "initial selected region" # init kalman filter object from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.kalman import MerweScaledSigmaPoints points = MerweScaledSigmaPoints(n=4, alpha=.1, beta=2., kappa=-1) dt = 1.0 ukf = UKF(dim_x=4, dim_z=2, fx=state_transistion_function, hx=measurement_Matrix, dt=dt, points=points) ukf.x = np.array([0., 0., 0., 0.]) ukf.Q = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32) * 0.03 kalman = cv2.KalmanFilter(4, 2) kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32) kalman.transitionMatrix = np.array( [[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32) kalman.processNoiseCov = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32) * 0.03
[0, 0, 0, 1]]) return np.dot(F, x) def h_cv(x): return np.array([x[0], x[2]]) def e(x): res = [] for n in range(x.shape[0]): res.append(np.sqrt(x[n][0]**2+x[n][2]**2)) return res dt = 1.0 random.seed(1234) ukf = UnscentedKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, kappa=0) ukf.x = np.array([100., 0., 0., 0.]) ukf.R = np.diag([25, 25]) ukf.Q[0:2,0:2] = Q_discrete_white_noise(2, dt, var=0.04) ukf.Q[2:4,2:4] = Q_discrete_white_noise(2, dt, var=0.04) ckf = CubatureKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt) ckf.x = np.array([100., 0., 0., 0.]) ckf.R = np.diag([25, 25]) ckf.Q[0:2,0:2] = Q_discrete_white_noise(2, dt, var=0.04) ckf.Q[2:4,2:4] = Q_discrete_white_noise(2, dt, var=0.04) uxs = [] pxs = [] zs = [] txs = []
class Model: """ SIRD model of Covid-19. """ __CONFIRMED_URL = 'https://bit.ly/35yJO0d' __RECOVERED_URL = 'https://bit.ly/2L6jLE9' __DEATHS_URL = 'https://bit.ly/2L0hzxQ' __POPULATION_URL = 'https://bit.ly/2WYjZCD' __JHU_DATA_SHIFT = 4 __N_FILTERED = 7 # Number of state variables to filter (I, R, D, β, γ, μ and n, the population). __N_MEASURED = 3 # Number of measured variables (I, R and D). __NB_OF_STEPS = 100 __DELTA_T = 1 / __NB_OF_STEPS __FIG_SIZE = (11, 13) __S_COLOR = '#0072bd' __I_COLOR = '#d95319' __R_COLOR = '#edb120' __D_COLOR = '#7e2f8e' __BETA_COLOR = '#77ac30' __GAMMA_COLOR = '#4dbeee' __MU_COLOR = '#a2142f' __DATA_ALPHA = 0.3 __DATA = None __POPULATION = None class Use(Enum): WIKIPEDIA = auto() DATA = auto() def __init__(self, use=Use.DATA, country='New Zealand', max_data=0): """ Initialise our Model object. """ # Retrieve the data (if requested and needed). if use == Model.Use.DATA and Model.__DATA is None: confirmed_data, confirmed_data_start = self.__jhu_data(Model.__CONFIRMED_URL, country) recovered_data, recovered_data_start = self.__jhu_data(Model.__RECOVERED_URL, country) deaths_data, deaths_data_start = self.__jhu_data(Model.__DEATHS_URL, country) data_start = min(confirmed_data_start, recovered_data_start, deaths_data_start) - Model.__JHU_DATA_SHIFT for i in range(data_start, confirmed_data.shape[1]): c = confirmed_data.iloc[0][i] r = recovered_data.iloc[0][i] d = deaths_data.iloc[0][i] data = [c - r - d, r, d] if Model.__DATA is None: Model.__DATA = np.array(data) else: Model.__DATA = np.vstack((Model.__DATA, data)) if use == Model.Use.DATA: self.__data = Model.__DATA else: self.__data = None if self.__data is not None: if not isinstance(max_data, int): sys.exit('Error: \'max_data\' must be an integer value.') if max_data > 0: self.__data = self.__data[:max_data] # Retrieve the population (if needed). if use == Model.Use.DATA and Model.__POPULATION is None: Model.__POPULATION = {} response = requests.get(Model.__POPULATION_URL) soup = BeautifulSoup(response.text, 'html.parser') data = soup.select('div div div div div tbody tr') for i in range(len(data)): country_soup = BeautifulSoup(data[i].prettify(), 'html.parser') country_value = country_soup.select('tr td a')[0].get_text().strip() population_value = country_soup.select('tr td')[2].get_text().strip().replace(',', '') Model.__POPULATION[country_value] = int(population_value) if use == Model.Use.DATA: if country in Model.__POPULATION: self.__population = Model.__POPULATION[country] else: sys.exit('Error: no population data is available for {}.'.format(country)) # Keep track of whether to use the data. self.__use_data = use == Model.Use.DATA # Declare some internal variables (that will then be initialised through our call to reset()). self.__beta = None self.__gamma = None self.__mu = None self.__ukf = None self.__x = None self.__n = None self.__data_s_values = None self.__data_i_values = None self.__data_r_values = None self.__data_d_values = None self.__s_values = None self.__i_values = None self.__r_values = None self.__d_values = None self.__beta_values = None self.__gamma_values = None self.__mu_values = None # Initialise (i.e. reset) our SIRD model. self.reset() @staticmethod def __jhu_data(url, country): data = pd.read_csv(url) data = data[(data['Country/Region'] == country) & data['Province/State'].isnull()] if data.shape[0] == 0: sys.exit('Error: no Covid-19 data is available for {}.'.format(country)) data = data.drop(data.columns[list(range(Model.__JHU_DATA_SHIFT))], axis=1) # Skip non-data columns. start = None for i in range(data.shape[1]): if data.iloc[0][i] != 0: start = Model.__JHU_DATA_SHIFT + i break return data, start def __data_x(self, day, index): """ Return the I/R/D value for the given day. """ return self.__data[day][index] if self.__use_data else math.nan def __data_s(self, day): """ Return the S value for the given day. """ if self.__use_data: return self.__population - self.__data_i(day) - self.__data_r(day) - self.__data_d(day) else: return math.nan def __data_i(self, day): """ Return the I value for the given day. """ return self.__data_x(day, 0) def __data_r(self, day): """ Return the R value for the given day. """ return self.__data_x(day, 1) def __data_d(self, day): """ Return the D value for the given day. """ return self.__data_x(day, 2) def __data_available(self, day): """ Return whether some data is available for the given day. """ return day <= self.__data.shape[0] - 1 if self.__use_data else False def __s_value(self): """ Return the S value based on the values of I, R, D and N. """ return self.__n - self.__x.sum() def __i_value(self): """ Return the I value. """ return self.__x[0] def __r_value(self): """ Return the R value. """ return self.__x[1] def __d_value(self): """ Return the D value. """ return self.__x[2] @staticmethod def __f(x, dt, **kwargs): """ State function. The ODE system to solve is: dI/dt = βIS/N - γI - μI dR/dt = γI dD/dt = μI """ model_self = kwargs.get('model_self') with_ukf = kwargs.get('with_ukf', True) if with_ukf: s = x[6] - x[:3].sum() beta = x[3] gamma = x[4] mu = x[5] n = x[6] else: s = model_self.__n - x.sum() beta = model_self.__beta gamma = model_self.__gamma mu = model_self.__mu n = model_self.__n a = np.array([[1 + dt * (beta * s / n - gamma - mu), 0, 0, 0, 0, 0, 0], [dt * gamma, 1, 0, 0, 0, 0, 0], [dt * mu, 0, 1, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 1]]) if with_ukf: return a @ x else: return a[:3, :3] @ x @staticmethod def __h(x): """ Measurement function. """ return x[:Model.__N_MEASURED] def reset(self): """ Reset our SIRD model. """ # Reset β, γ and μ to the values mentioned on Wikipedia (see https://bit.ly/2VMvb6h). self.__beta = 0.4 self.__gamma = 0.035 self.__mu = 0.005 # Reset I, R and D to the data at day 0 or the values mentioned on Wikipedia (see https://bit.ly/2VMvb6h). if self.__use_data: self.__x = np.array([self.__data_i(0), self.__data_r(0), self.__data_d(0)]) self.__n = self.__population else: self.__x = np.array([3, 0, 0]) self.__n = 1000 # Reset our Unscented Kalman filter (if required). Note tat we use a dt value of 1 (day) and not the value of # Model.__DELTA_T. if self.__use_data: points = MerweScaledSigmaPoints(Model.__N_FILTERED, 1e-3, # Alpha value (usually a small positive value like 1e-3). 2, # Beta value (a value of 2 is optimal for a Gaussian distribution). 0, # Kappa value (usually, either 0 or 3-n). ) self.__ukf = UnscentedKalmanFilter(Model.__N_FILTERED, Model.__N_MEASURED, 1, self.__h, Model.__f, points) self.__ukf.x = np.array([self.__data_i(0), self.__data_r(0), self.__data_d(0), self.__beta, self.__gamma, self.__mu, self.__n]) # Reset our data (if requested). if self.__use_data: self.__data_s_values = np.array([self.__data_s(0)]) self.__data_i_values = np.array([self.__data_i(0)]) self.__data_r_values = np.array([self.__data_r(0)]) self.__data_d_values = np.array([self.__data_d(0)]) # Reset our predicted/estimated values. self.__s_values = np.array([self.__s_value()]) self.__i_values = np.array([self.__i_value()]) self.__r_values = np.array([self.__r_value()]) self.__d_values = np.array([self.__d_value()]) # Reset our estimated SIRD model parameters. self.__beta_values = np.array([self.__beta]) self.__gamma_values = np.array([self.__gamma]) self.__mu_values = np.array([self.__mu]) def run(self, batch_filter=True, nb_of_days=100): """ Run our SIRD model for the given number of days, taking advantage of the data (if requested) to estimate the values of β, γ and μ. """ # Make sure that we were given a valid number of days. if not isinstance(nb_of_days, int) or nb_of_days <= 0: sys.exit('Error: \'nb_of_days\' must be an integer value greater than zero.') # Run our SIRD simulation, which involves computing our predicted/estimated state by computing our SIRD model / # Unscented Kalman filter in batch filter mode, if required. if self.__use_data and batch_filter: mu, cov = self.__ukf.batch_filter(self.__data) batch_filter_x, _, _ = self.__ukf.rts_smoother(mu, cov) # Override the first value of S, I, R and D. x = batch_filter_x[0][:3] self.__s_values = np.array([self.__n - x.sum()]) self.__i_values = np.array([x[0]]) self.__r_values = np.array([x[1]]) self.__d_values = np.array([x[2]]) for i in range(1, nb_of_days + 1): # Compute our predicted/estimated state by computing our SIRD model / Unscented Kalman filter for one day. if self.__use_data and self.__data_available(i): if batch_filter: self.__x = batch_filter_x[i][:3] self.__beta = batch_filter_x[i][3] self.__gamma = batch_filter_x[i][4] self.__mu = batch_filter_x[i][5] else: self.__ukf.predict(model_self=self) self.__ukf.update(np.array([self.__data_i(i), self.__data_r(i), self.__data_d(i)])) self.__x = self.__ukf.x[:3] self.__beta = self.__ukf.x[3] self.__gamma = self.__ukf.x[4] self.__mu = self.__ukf.x[5] else: for j in range(1, Model.__NB_OF_STEPS + 1): self.__x = Model.__f(self.__x, Model.__DELTA_T, model_self=self, with_ukf=False) # Keep track of our data (if requested). if self.__use_data: if self.__data_available(i): self.__data_s_values = np.append(self.__data_s_values, self.__data_s(i)) self.__data_i_values = np.append(self.__data_i_values, self.__data_i(i)) self.__data_r_values = np.append(self.__data_r_values, self.__data_r(i)) self.__data_d_values = np.append(self.__data_d_values, self.__data_d(i)) else: self.__data_s_values = np.append(self.__data_s_values, math.nan) self.__data_i_values = np.append(self.__data_i_values, math.nan) self.__data_r_values = np.append(self.__data_r_values, math.nan) self.__data_d_values = np.append(self.__data_d_values, math.nan) # Keep track of our predicted/estimated values. self.__s_values = np.append(self.__s_values, self.__s_value()) self.__i_values = np.append(self.__i_values, self.__i_value()) self.__r_values = np.append(self.__r_values, self.__r_value()) self.__d_values = np.append(self.__d_values, self.__d_value()) # Keep track of our estimated SIRD model parameters. self.__beta_values = np.append(self.__beta_values, self.__beta) self.__gamma_values = np.append(self.__gamma_values, self.__gamma) self.__mu_values = np.append(self.__mu_values, self.__mu) def plot(self, figure=None, two_axes=False): """ Plot the results using five subplots for 1) S, 2) I and R, 3) D, 4) β, and 5) γ and μ. In each subplot, we plot the data (if requested) as bars and the computed value as a line. """ days = range(self.__s_values.size) nrows = 5 if self.__use_data else 3 ncols = 1 if figure is None: show_figure = True figure, axes = plt.subplots(nrows, ncols, figsize=Model.__FIG_SIZE) else: figure.clf() show_figure = False axes = figure.subplots(nrows, ncols) figure.canvas.set_window_title('SIRD model fitted to data' if self.__use_data else 'Wikipedia SIRD model') # First subplot: S. axis1 = axes[0] axis1.plot(days, self.__s_values, Model.__S_COLOR, label='S') axis1.legend(loc='best') if self.__use_data: axis2 = axis1.twinx() if two_axes else axis1 axis2.bar(days, self.__data_s_values, color=Model.__S_COLOR, alpha=Model.__DATA_ALPHA) data_s_range = self.__population - min(self.__data_s_values) data_block = 10 ** (math.floor(math.log10(data_s_range)) - 1) s_values_shift = data_block * math.ceil(data_s_range / data_block) axis2.set_ylim(min(min(self.__s_values), self.__population - s_values_shift), self.__population) # Second subplot: I and R. axis1 = axes[1] axis1.plot(days, self.__i_values, Model.__I_COLOR, label='I') axis1.plot(days, self.__r_values, Model.__R_COLOR, label='R') axis1.legend(loc='best') if self.__use_data: axis2 = axis1.twinx() if two_axes else axis1 axis2.bar(days, self.__data_i_values, color=Model.__I_COLOR, alpha=Model.__DATA_ALPHA) axis2.bar(days, self.__data_r_values, color=Model.__R_COLOR, alpha=Model.__DATA_ALPHA) # Third subplot: D. axis1 = axes[2] axis1.plot(days, self.__d_values, Model.__D_COLOR, label='D') axis1.legend(loc='best') if self.__use_data: axis2 = axis1.twinx() if two_axes else axis1 axis2.bar(days, self.__data_d_values, color=Model.__D_COLOR, alpha=Model.__DATA_ALPHA) # Fourth subplot: β. if self.__use_data: axis1 = axes[3] axis1.plot(days, self.__beta_values, Model.__BETA_COLOR, label='β') axis1.legend(loc='best') # Fourth subplot: γ and μ. if self.__use_data: axis1 = axes[4] axis1.plot(days, self.__gamma_values, Model.__GAMMA_COLOR, label='γ') axis1.plot(days, self.__mu_values, Model.__MU_COLOR, label='μ') axis1.legend(loc='best') plt.xlabel('time (day)') if show_figure: plt.show() def movie(self, filename, batch_filter=True, nb_of_days=100): """ Generate, if using the data, a movie showing the evolution of our SIRD model throughout time. """ if self.__use_data: data_size = Model.__DATA.shape[0] figure = plt.figure(figsize=Model.__FIG_SIZE) backend = matplotlib.get_backend() writer = manimation.writers['ffmpeg']() matplotlib.use("Agg") with writer.saving(figure, filename, 96): for i in range(1, data_size + 1): print('Processing frame #', i, '/', data_size, '...', sep='') self.__data = Model.__DATA[:i] self.reset() self.run(batch_filter=batch_filter, nb_of_days=nb_of_days) self.plot(figure=figure) writer.grab_frame() print('All done!') matplotlib.use(backend) def s(self, day=-1): """ Return all the S values (if day=-1) or its value for a given day. """ if day == -1: return self.__s_values else: return self.__s_values[day] def i(self, day=-1): """ Return all the I values (if day=-1) or its value for a given day. """ if day == -1: return self.__i_values else: return self.__i_values[day] def r(self, day=-1): """ Return all the R values (if day=-1) or its value for a given day. """ if day == -1: return self.__r_values else: return self.__r_values[day] def d(self, day=-1): """ Return all the D values (if day=-1) or its value for a given day. """ if day == -1: return self.__d_values else: return self.__d_values[day]
class Follower(BehaviorBase): def __init__(self, g, zeta, leader, trajectory_delay=2.0, update_delta_t=0.01, orig_leader=None, orig_leader_delay=None, noise_sigma=0.0, log_file=None, visibility_fov=config.FOV_ANGLE, visibility_radius=None, id=None, dt=0.02): """ Construct a follower Behavior. leader is the Bot to be followed g > 0 is a tuning parameter zeta in (0, 1) is a damping coefficient trajectory_delay is the time interval between leader and follower """ super(Follower, self).__init__() assert(isinstance(leader, Bot)) assert(isinstance(orig_leader, Bot)) self.radius = config.BOT_RADIUS self.leader = leader self.orig_leader = orig_leader self.trajectory_delay = trajectory_delay assert g > 0, "Follower: g parameter must be positive" self.g = g assert 0 < zeta < 1, "Follower: zeta parameter must be in (0, 1)" self.zeta = zeta # leader_positions stores config.POSITIONS_BUFFER_SIZE tuples; # tuple's first field is time, second is the # corresponding position of the leader self.leader_positions = RingBuffer(config.POSITIONS_BUFFER_SIZE) self.noisy_leader_positions = RingBuffer(config.POSITIONS_BUFFER_SIZE) self.leader_is_visible = False # precise_orig_leader_states stores the first leader's precise state; # used to calculate "real" errors (as opposed to calculations # w.r.t. the approximation curve) self.precise_orig_leader_states = [] # precise_leader_states is used for the same purpose, but with the bot's own leader self.precise_leader_states = [] self.update_delta_t = update_delta_t needed_buffer_size = config.SAMPLE_COUNT // 2 + self.trajectory_delay / self.update_delta_t if needed_buffer_size > config.POSITIONS_BUFFER_SIZE: sys.stderr.write("leader_positions buffer is too small for update_delta_t = {}".format(self.update_delta_t) + " (current = {}, required = {})".format(config.POSITIONS_BUFFER_SIZE, needed_buffer_size)) raise RuntimeError, "leader_positions buffer is too small" self.last_update_time = 0.0 self.noise_sigma = noise_sigma self.log_file = log_file self.visibility_fov = visibility_fov if visibility_radius is None: visibility_radius = 2.0 * trajectory_delay * config.BOT_VEL_CAP self.visibility_radius = visibility_radius self.id = id; if orig_leader_delay is None: orig_leader_delay = trajectory_delay self.orig_leader_delay = orig_leader_delay if self.log_file is not None: log_dict = {"id": self.id, "g": self.g, "zeta": self.zeta, "noise_sigma": self.noise_sigma, "reference_points_cnt": config.SAMPLE_COUNT, "trajectory_delay": trajectory_delay} print >> self.log_file, log_dict q = 0.01 # std of process r = 0.05 # std of measurement self.delta_time = dt if not config.DISABLE_UKF: points = MerweScaledSigmaPoints(n=6, alpha=.1, beta=2., kappa=-3) self.ukf = UKF(dim_x=6, dim_z=2, fx=f, hx=h, dt=dt, points=points) self.ukf_x_initialized = 0 #self.ukf.x = np.array([0, 0, 1, pi/4, 0, 0]) self.ukf.R = np.diag([r, r]); self.ukf.Q = np.diag([0, 0, 0, 0, q, q]) def point_in_fov(self, p): if length(p - self.pos) > self.visibility_radius: return False d = p - self.pos ang = atan2(cross(self.real_dir, d), dot(self.real_dir, d)) return -0.5 * self.visibility_fov < ang < 0.5 * self.visibility_fov def point_is_visible(self, p, obstacles): if not self.point_in_fov(p): return False try: ray = Ray(self.pos, p - self.pos) except NormalizationError: ray = Ray(self.pos, Vector(1.0, 0.0)) i = first_intersection(ray, obstacles) return (i is None) or (length(i - self.pos) > length(p - self.pos)) def store_leaders_state(self, engine, obstacles): if self.point_is_visible(self.leader.real.pos, obstacles): self.leader_is_visible = True noisy_pos = self.leader.real.pos noisy_pos += Vector(gauss(0.0, self.noise_sigma), gauss(0.0, self.noise_sigma)) if config.DISABLE_UKF: filtered_pos = noisy_pos else: if (self.ukf_x_initialized == 0): self.ukf_x1 = noisy_pos self.ukf_x_initialized += 1 filtered_pos = noisy_pos self.ukf.x = np.array([noisy_pos.x, noisy_pos.y, 0, pi/2, 0, 0]) #elif (self.ukf_x_initialized == 1): # self.ukf_x2 = noisy_pos # self.ukf_x_initialized += 1 # self.ukf.x = np.array([noisy_pos.x, noisy_pos.y, # length((noisy_pos - self.ukf_x1) / self.delta_time), # atan2(noisy_pos.y - self.ukf_x1.y, # noisy_pos.x - self.ukf_x1.x), # 0, 0]) # filtered_pos = noisy_pos else: # UKF is initialized self.ukf.predict() self.ukf.update(np.array(noisy_pos)) filtered_pos = Point(self.ukf.x[0], self.ukf.x[1]) self.leader_noisy_pos = noisy_pos self.noisy_leader_positions.append(TimedPosition(engine.time, noisy_pos)) self.leader_positions.append(TimedPosition(engine.time, filtered_pos)) self.last_update_time = engine.time else: self.leader_is_visible = False if not config.DISABLE_UKF: self.ukf.predict() #TODO: do magic with UKF? orig_leader_theta = atan2(self.orig_leader.real.dir.y, self.orig_leader.real.dir.x) self.precise_orig_leader_states.append(TimedState(time=engine.time, pos=self.orig_leader.real.pos, theta=orig_leader_theta)) leader_theta = atan2(self.leader.real.dir.y, self.leader.real.dir.x) self.precise_leader_states.append(TimedState(time=engine.time, pos=self.leader.real.pos, theta=leader_theta)) def polyfit_trajectory(self, pos_data, t): x_pos = np.array([el.pos.x for el in pos_data]) y_pos = np.array([el.pos.y for el in pos_data]) times = np.array([el.time for el in pos_data]) # needed for trajectory rendering self.t_st = times[0] self.t_fn = max(times[-1], t) # calculate quadratic approximation of the reference trajectory x_poly = np.polyfit(times, x_pos, deg=2) y_poly = np.polyfit(times, y_pos, deg=2) known = Trajectory.from_poly(x_poly, y_poly) return known, x_pos[-1], y_pos[-1], times[-1] def extend_trajectory(self, known, last_x, last_y, last_t): # now adding a circle to the end of known trajectory # k is signed curvature of the trajectry at t_fn try: k = known.curvature(last_t) except (ValueError, ZeroDivisionError): k = config.MIN_CIRCLE_CURVATURE if abs(k) < config.MIN_CIRCLE_CURVATURE: k = copysign(config.MIN_CIRCLE_CURVATURE, k) if abs(k) > config.MAX_CURVATURE: k = copysign(config.MAX_CURVATURE, k) radius = abs(1.0/k) # trajectory direction at time t_fn try: d = normalize(Vector(known.dx(last_t), known.dy(last_t))) except NormalizationError: d = self.real_dir r = Vector(-d.y, d.x) / k center = Point(last_x, last_y) + r phase = atan2(-r.y, -r.x) #freq = known.dx(last_t) / r.y freq = k self.x_approx = lambda time: known.x(time) if time < last_t else \ center.x + radius * cos(freq * (time - last_t) + phase) self.y_approx = lambda time: known.y(time) if time < last_t else \ center.y + radius * sin(freq * (time - last_t) + phase) dx = lambda time: known.dx(time) if time < last_t else \ -radius * freq * sin(freq * (time - last_t) + phase) dy = lambda time: known.dy(time) if time < last_t else \ radius * freq * cos(freq * (time - last_t) + phase) ddx = lambda time: known.ddx(time) if time < last_t else \ -radius * freq * freq * cos(freq * (time - last_t) + phase) ddy = lambda time: known.ddy(time) if time < last_t else \ -radius * freq * freq * sin(freq * (time - last_t) + phase) # FIXME: don't use division by y if y == 0 try: if isnan(self.x_approx(last_t + 1)): return known except Exception: return known return Trajectory(x=self.x_approx, y=self.y_approx, dx=dx, dy=dy, ddx=ddx, ddy=ddy) def generate_trajectory(self, leader_positions, t): arr = get_interval(self.leader_positions, t, config.SAMPLE_COUNT) self.traj_interval = arr if len(arr) == 0: return None known, last_x, last_y, last_t = self.polyfit_trajectory(arr, t) if config.DISABLE_CIRCLES: return known else: return self.extend_trajectory(known, last_x, last_y, last_t) def calc_desired_velocity(self, bots, obstacles, targets, engine): # update trajectory if engine.time - self.last_update_time > self.update_delta_t: self.store_leaders_state(engine, obstacles) # reduce random movements at the start # TODO: this causes oscillations that smash bots into each other. # Change to something smoother. PID control? #if self.leader_is_visible and length(self.pos - self.leader_noisy_pos) < MIN_DISTANCE_TO_LEADER: # return Instr(0.0, 0.0) t = engine.time - self.trajectory_delay self.trajectory = self.generate_trajectory(self.leader_positions, t) if self.trajectory is None: return Instr(0.0, 0.0) dx = self.trajectory.dx dy = self.trajectory.dy ddx = self.trajectory.ddx ddy = self.trajectory.ddy # calculate the feed-forward velocities v_fun = lambda time: sqrt(dx(time)**2 + dy(time)**2) #omega_fun = lambda time: (dx(time) * 2 * y_poly[0] - dy(time) * 2 * x_poly[0]) / (dx(time)**2 + dy(time)**2) omega_fun = lambda time: (dx(time) * ddy(time) - dy(time) * ddx(time)) / (dx(time)**2 + dy(time)**2) v_ff = v_fun(t) omega_ff = omega_fun(t) # x_r, y_r and theta_r denote the reference robot's state r = State(x=self.trajectory.x(t), y=self.trajectory.y(t), theta=atan2(self.trajectory.dy(t), self.trajectory.dx(t))) self.target_point = Point(r.x, r.y) if isnan(v_ff): v_ff = 0.0 if isnan(omega_ff): omega_ff = 0.0 # cur is the current state cur = State(x=self.pos.x, y=self.pos.y, theta=atan2(self.real_dir.y, self.real_dir.x)) # error in the global (fixed) reference frame delta = State(x=r.x - cur.x, y=r.y - cur.y, theta=normalize_angle(r.theta - cur.theta)) # translate error into the follower's reference frame e = State(x= cos(cur.theta) * delta.x + sin(cur.theta) * delta.y, y=-sin(cur.theta) * delta.x + cos(cur.theta) * delta.y, theta=delta.theta) # calculate gains k_x, k_y, k_theta # these are just parameters, not a state in any sense! omega_n = sqrt(omega_ff**2 + self.g * v_ff**2) k_x = 2 * self.zeta * omega_n k_y = self.g k_theta = 2 * self.zeta * omega_n # calculate control velocities v = v_ff * cos(e.theta) + k_x * e.x se = sin(e.theta) / e.theta if e.theta != 0 else 1.0 omega = omega_ff + k_y * v_ff * se * e.y + k_theta * e.theta if config.DISABLE_FEEDBACK: v = v_ff omega = omega_ff real_e = State(0.0, 0.0, 0.0) try: orig_t = engine.time - self.orig_leader_delay orig_leader_state = lerp_precise_states(orig_t, self.precise_orig_leader_states) real_e = State(x=orig_leader_state.x - cur.x, y=orig_leader_state.y - cur.y, theta=normalize_angle(orig_leader_state.theta - cur.theta)) except LerpError: # not enough data is yet available to calculate error pass try: leader_t = engine.time - self.trajectory_delay leader_state = lerp_precise_states(leader_t, self.precise_leader_states) approx_e = State(x=leader_state.x - cur.x, y=leader_state.y - cur.y, theta=normalize_angle(leader_state.theta - cur.theta)) except LerpError: # same as above pass if self.log_file is not None: log_dict = {"id": self.id, "time": engine.time, "delta_time": engine.time_since_last_bot_update, "x": cur.x, "y": cur.y, "theta": cur.theta, "v": v, "omega": omega, "v_ff": v_ff, "omega_ff": omega_ff, "e_x": e.x, "e_y": e.y, "e_theta": e.theta, "real_e_x": real_e.x, "real_e_y": real_e.y, "real_e_theta": real_e.theta, "approx_e_x": approx_e.x, "approx_e_y": approx_e.y, "approx_e_theta": approx_e.theta, "leader_is_visible": self.leader_is_visible } print >> self.log_file, log_dict return Instr(v, omega) def draw(self, screen, field): if config.DISPLAYED_POINTS_COUNT > 0: k = config.POSITIONS_BUFFER_SIZE / config.DISPLAYED_POINTS_COUNT if k == 0: k = 1 for index, (time, point) in enumerate(self.leader_positions): if index % k == 0: draw_circle(screen, field, config.TRAJECTORY_POINTS_COLOR, point, 0.03, 1) for index, (time, point) in enumerate(self.noisy_leader_positions): if index % k == 0: draw_circle(screen, field, config.NOISY_TRAJECTORY_POINTS_COLOR, point, 0.03, 1) if config.DISPLAYED_USED_POINTS_COUNT > 0: k = len(self.traj_interval) / config.DISPLAYED_USED_POINTS_COUNT if k == 0: k = 1 for index, (time, point) in enumerate(self.traj_interval): if index % k == 0: draw_circle(screen, field, config.USED_TRAJECTORY_POINTS_COLOR, point, 0.03, 1) if config.DRAW_DELAYED_LEADER_POS: try: orig_leader_dir = Vector(cos(self.orig_leader_theta), sin(self.orig_leader_theta)) draw_directed_circle(screen, field, (0, 255, 0), self.orig_leader_pos, 0.2, orig_leader_dir, 1) except AttributeError: pass if config.DRAW_SENSOR_RANGE: ang = atan2(self.real_dir.y, self.real_dir.x) draw_arc(screen, field, config.SENSOR_COLOR, self.pos, self.visibility_radius, ang - 0.5 * self.visibility_fov, ang + 0.5 * self.visibility_fov, 1) draw_line(screen, field, config.SENSOR_COLOR, self.pos, self.pos + rotate(self.real_dir * self.visibility_radius, 0.5 * self.visibility_fov), 1) draw_line(screen, field, config.SENSOR_COLOR, self.pos, self.pos + rotate(self.real_dir * self.visibility_radius, -0.5 * self.visibility_fov), 1) try: if config.DRAW_VISIBILITY and self.leader_is_visible: draw_circle(screen, field, config.config.VISIBILITY_COLOR, self.leader.real.pos, 0.5 * config.BOT_RADIUS) except AttributeError: pass try: if config.DRAW_REFERENCE_POS: draw_circle(screen, field, config.TARGET_POINT_COLOR, self.target_point, 0.2) if config.DRAW_APPROX_TRAJECTORY and self.trajectory is not None: #if len(self.traj_interval) > 1: # p2 = Point(self.traj_interval[0].pos.x, # self.traj_interval[0].pos.y) # for t, p in self.traj_interval: # draw_line(screen, field, config.APPROX_TRAJECTORY_COLOR, p, p2) # p2 = p step = (self.t_fn - self.t_st) / config.TRAJECTORY_SEGMENT_COUNT for t in (self.t_st + k * step for k in xrange(config.TRAJECTORY_SEGMENT_COUNT)): p = Point(self.trajectory.x(t), self.trajectory.y(t)) p2 = Point(self.trajectory.x(t + step), self.trajectory.y(t + step)) draw_line(screen, field, config.APPROX_TRAJECTORY_COLOR, p, p2) p_st = Point(self.trajectory.x(self.t_st), self.trajectory.y(self.t_st)) p_fn = Point(self.trajectory.x(self.t_fn), self.trajectory.y(self.t_fn)) step = 0.5 / config.TRAJECTORY_SEGMENT_COUNT p = p_fn p2 = p t = self.t_fn it = 0 while it < config.TRAJECTORY_SEGMENT_COUNT and min(dist(p, p_fn), dist(p, p_st)) < 1.0: it += 1 t += step p2 = p p = Point(self.trajectory.x(t), self.trajectory.y(t)) draw_line(screen, field, config.APPROX_TRAJECTORY_COLOR, p, p2) except AttributeError as e: # approximation hasn't been calculated yet pass
def test_rts(): def fx(x, dt): A = np.eye(3) + dt * np.array([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) f = np.dot(A, x) return f def hx(x): return [np.sqrt(x[0]**2 + x[2]**2)] dt = 0.05 sp = JulierSigmaPoints(n=3, kappa=1.) kf = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp) kf.Q *= 0.01 kf.R = 10 kf.x = np.array([0., 90., 1100.]) kf.P *= 100. radar = RadarSim(dt) t = np.arange(0, 20 + dt, dt) n = len(t) xs = np.zeros((n, 3)) random.seed(200) rs = [] for i in range(len(t)): r = radar.get_range() kf.predict() kf.update(z=[r]) xs[i, :] = kf.x rs.append(r) kf.x = np.array([0., 90., 1100.]) kf.P = np.eye(3) * 100 M, P = kf.batch_filter(rs) assert np.array_equal(M, xs), "Batch filter generated different output" Qs = [kf.Q] * len(t) M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs) if DO_PLOT: print(xs[:, 0].shape) plt.figure() plt.subplot(311) plt.plot(t, xs[:, 0]) plt.plot(t, M2[:, 0], c='g') plt.subplot(312) plt.plot(t, xs[:, 1]) plt.plot(t, M2[:, 1], c='g') plt.subplot(313) plt.plot(t, xs[:, 2]) plt.plot(t, M2[:, 2], c='g')
Qd = calc_Qd(rhat, xhat) Q_r = Qd[0:PARTICLE_DIM, 0:PARTICLE_DIM] Q_x = Qd[PARTICLE_DIM:STATE_DIM, PARTICLE_DIM:STATE_DIM] S = Qd[PARTICLE_DIM:STATE_DIM, 0:PARTICLE_DIM] T_k0 = S.dot(np.linalg.inv(Q_r)) Q_x -= T_k0.dot(Q_r.dot(T_k0.T)) # noise decorrelation return Q_x, Q_r, T_k0 x_points = MerweScaledSigmaPoints(MARGIN_DIM, alpha=.001, beta=2., kappa=0) for i in range(N): ukf = UnscentedKalmanFilter(MARGIN_DIM, PARTICLE_DIM, dt, x_measurement, x_x_noiseless, x_points) # x estimate ukf.x = xhat0.reshape(MARGIN_DIM) ukf.x_prior = ukf.x # covariance of estimate ukf.P_prior = P0_x # get around needing to call predict() before update() (not applicable here) ukf.sigmas_f = ukf.points_fn.sigma_points(ukf.x, ukf.P_prior) kfs.append(ukf) Q_m, R_m, _ = calc_marginalized_Q_R_T(particles[:, i:i + 1], xhat0) ukf.Q = Q_m ukf.R = R_m
def test_fixed_lag(): def fx(x, dt): A = np.eye(3) + dt * np.array ([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) f = np.dot(A, x) return f def hx(x): return np.sqrt (x[0]**2 + x[2]**2) dt = 0.05 kf = UKF(3, 1, dt, fx=fx, hx=hx, kappa=0.) kf.Q *= 0.01 kf.R = 10 kf.x = np.array([0., 90., 1100.]) kf.P *= 1. radar = RadarSim(dt) t = np.arange(0,20+dt, dt) n = len(t) xs = np.zeros((n,3)) random.seed(200) rs = [] #xs = [] M = [] P = [] N =10 flxs = [] for i in range(len(t)): r = radar.get_range() #r = GetRadar(dt) kf.predict() kf.update(z=[r]) xs[i,:] = kf.x flxs.append(kf.x) rs.append(r) M.append(kf.x) P.append(kf.P) print(i) #print(i, np.asarray(flxs)[:,0]) if i == 20 and len(M) >= N: try: M2, P2, K = kf.rts_smoother(Xs=np.asarray(M)[-N:], Ps=np.asarray(P)[-N:]) flxs[-N:] = M2 #flxs[-N:] = [20]*N except: print('except', i) #P[-N:] = P2 kf.x = np.array([0., 90., 1100.]) kf.P = np.eye(3)*100 M, P = kf.batch_filter(rs) Qs = [kf.Q]*len(t) M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs) flxs = np.asarray(flxs) print(xs[:,0].shape) plt.figure() plt.subplot(311) plt.plot(t, xs[:,0]) plt.plot(t, flxs[:,0], c='r') plt.plot(t, M2[:,0], c='g') plt.subplot(312) plt.plot(t, xs[:,1]) plt.plot(t, flxs[:,1], c='r') plt.plot(t, M2[:,1], c='g') plt.subplot(313) plt.plot(t, xs[:,2]) plt.plot(t, flxs[:,2], c='r') plt.plot(t, M2[:,2], c='g')
def two_radar(): # code is not complete - I was using to test RTS smoother. very similar # to two_radary.py in book. import numpy as np import matplotlib.pyplot as plt from numpy import array from numpy.linalg import norm from numpy.random import randn from math import atan2 from filterpy.common import Q_discrete_white_noise class RadarStation(object): def __init__(self, pos, range_std, bearing_std): self.pos = asarray(pos) self.range_std = range_std self.bearing_std = bearing_std def reading_of(self, ac_pos): """ Returns range and bearing to aircraft as tuple. bearing is in radians. """ diff = np.subtract(self.pos, ac_pos) rng = norm(diff) brg = atan2(diff[1], diff[0]) return rng, brg def noisy_reading(self, ac_pos): rng, brg = self.reading_of(ac_pos) rng += randn() * self.range_std brg += randn() * self.bearing_std return rng, brg class ACSim(object): def __init__(self, pos, vel, vel_std): self.pos = asarray(pos, dtype=float) self.vel = asarray(vel, dtype=float) self.vel_std = vel_std def update(self): vel = self.vel + (randn() * self.vel_std) self.pos += vel return self.pos dt = 1. def hx(x): r1, b1 = hx.R1.reading_of((x[0], x[2])) r2, b2 = hx.R2.reading_of((x[0], x[2])) return array([r1, b1, r2, b2]) def fx(x, dt): x_est = x.copy() x_est[0] += x[1]*dt x_est[2] += x[3]*dt return x_est vx, vy = 0.1, 0.1 f = UnscentedKalmanFilter(dim_x=4, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0) aircraft = ACSim((100, 100), (vx*dt, vy*dt), 0.00000002) range_std = 0.001 # 1 meter bearing_std = 1./1000 # 1mrad R1 = RadarStation((0, 0), range_std, bearing_std) R2 = RadarStation((200, 0), range_std, bearing_std) hx.R1 = R1 hx.R2 = R2 f.x = array([100, vx, 100, vy]) f.R = np.diag([range_std**2, bearing_std**2, range_std**2, bearing_std**2]) q = Q_discrete_white_noise(2, var=0.0002, dt=dt) f.Q[0:2, 0:2] = q f.Q[2:4, 2:4] = q f.P = np.diag([.1, 0.01, .1, 0.01]) track = [] zs = [] for i in range(int(300/dt)): pos = aircraft.update() r1, b1 = R1.noisy_reading(pos) r2, b2 = R2.noisy_reading(pos) z = np.array([r1, b1, r2, b2]) zs.append(z) track.append(pos.copy()) zs = asarray(zs) xs, Ps, Pxz, pM, pP = f.batch_filter(zs) ms, _, _ = f.rts_smoother(xs, Ps) track = asarray(track) time = np.arange(0, len(xs) * dt, dt) plt.figure() plt.subplot(411) plt.plot(time, track[:, 0]) plt.plot(time, xs[:, 0]) plt.legend(loc=4) plt.xlabel('time (sec)') plt.ylabel('x position (m)') plt.tight_layout() plt.subplot(412) plt.plot(time, track[:, 1]) plt.plot(time, xs[:, 2]) plt.legend(loc=4) plt.xlabel('time (sec)') plt.ylabel('y position (m)') plt.tight_layout() plt.subplot(413) plt.plot(time, xs[:, 1]) plt.plot(time, ms[:, 1]) plt.legend(loc=4) plt.ylim([0, 0.2]) plt.xlabel('time (sec)') plt.ylabel('x velocity (m/s)') plt.tight_layout() plt.subplot(414) plt.plot(time, xs[:, 3]) plt.plot(time, ms[:, 3]) plt.ylabel('y velocity (m/s)') plt.legend(loc=4) plt.xlabel('time (sec)') plt.tight_layout() plt.show()
def test_circle(): from filterpy.kalman import KalmanFilter from math import radians def hx(x): radius = x[0] angle = x[1] x = cos(radians(angle)) * radius y = sin(radians(angle)) * radius return np.array([x, y]) def fx(x, dt): return np.array([x[0], x[1]+x[2], x[2]]) std_noise = .1 f = UKF(dim_x=3, dim_z=2, dt=.01, hx=hx, fx=fx, kappa=0) f.x = np.array([50., 90., 0]) f.P *= 100 f.R = np.eye(2)*(std_noise**2) f.Q = np.eye(3)*.001 f.Q[0,0]=0 f.Q[2,2]=0 kf = KalmanFilter(dim_x=6, dim_z=2) kf.x = np.array([50., 0., 0, 0, .0, 0.]) F = np.array([[1., 1., .5, 0., 0., 0.], [0., 1., 1., 0., 0., 0.], [0., 0., 1., 0., 0., 0.], [0., 0., 0., 1., 1., .5], [0., 0., 0., 0., 1., 1.], [0., 0., 0., 0., 0., 1.]]) kf.F = F kf.P*= 100 kf.H = np.array([[1,0,0,0,0,0], [0,0,0,1,0,0]]) kf.R = f.R kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001) kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001) measurements = [] results = [] zs = [] kfxs = [] for t in range (0,12000): a = t / 30 + 90 x = cos(radians(a)) * 50.+ randn() * std_noise y = sin(radians(a)) * 50. + randn() * std_noise # create measurement = t plus white noise z = np.array([x,y]) zs.append(z) f.predict() f.update(z) kf.predict() kf.update(z) # save data results.append (hx(f.x)) kfxs.append(kf.x) #print(f.x) results = np.asarray(results) zs = np.asarray(zs) kfxs = np.asarray(kfxs) print(results) if DO_PLOT: plt.plot(zs[:,0], zs[:,1], c='r', label='z') plt.plot(results[:,0], results[:,1], c='k', label='UKF') plt.plot(kfxs[:,0], kfxs[:,3], c='g', label='KF') plt.legend(loc='best') plt.axis('equal')
def main(): # Create a random points to interpolate into a path x = np.arange(0, 30, 1) # x coord x = np.append(x, x[::-1]) y = np.ones(len(x)) # designing course # TODO: function to create courses y = [x * 2 for x in x[:15]] y.extend([x * 3 for x in x[15:30]]) y.extend([x * 1.2 for x in x[30:55]]) y.extend([x % 5 for x in x[55:60]]) y[-5:] = [-1, -1, -1, -1, -2] # prepare map for func map_ = np.vstack((x, y)) # draw map,landmarks & get trajectory path = make_map(map_, landmarks=5, random_seed=42) lmark_pos = np.array(make_map.landmarks) # round path for controller x_path = [round(x, 4) for x in path[0][:]] y_path = [round(y, 4) for y in path[1][:]] # state and PID object state = State(x=x_path[0], y=y_path[0], yaw=np.radians(90.0), v=0.0) pd = PID(Kp=0.5, Ki=0.1, Kd=-0.15) target_speed = 30.0 / 3.6 # km/h - > [m/s] # Lists to store x = [state.x] y = [state.y] yaw = [state.yaw] v = [state.v] t = [0.0] lat_error = [0.0] # Setup Initial values target_index, _ = calc_target_index(state, x_path, y_path) last_index = len(x_path) - 1 max_sim_time = 50.0 dt = 0.1 time = 0.0 show_animation = True # setup UKF points = MerweScaledSigmaPoints(n=3, alpha=1e-4, kappa=0.0, beta=2, subtract=residual_x) sigma_range = 0.3 sigma_bearing = 0.1 ukf = UKF(dim_x=3, dim_z=2 * len(lmark_pos), fx=move, 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] * 5) # 5 landmarks ukf.Q = np.eye(3) * 0.0001 while time <= max_sim_time and last_index > target_index: #TODO: make stanley model work with ukf ukf.predict(u=u, wheelbase=wheelbase) if time % 10 == 0: plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6, facecolor='k', alpha=0.3) # set up controls ai = pd.pid_control(target_speed, state.v, lat_error[-1], time) di, target_index = stanley(state, x_path, y_path, target_index) state.update(ai, di) time += dt # store data for plotting x.append(state.x) y.append(state.y) yaw.append(state.yaw) v.append(state.v) t.append(time) lat_error.append(stanley.lat_error) # speed up time if oscilaltions if stanley.lat_error > abs(1.5): time += 1 if show_animation: simple_animation(path, [x, y], lmark_pos, time, max_sim_time) assert last_index >= target_index, "Cannot reach goal" if show_animation: #plt.plot(path[0][:],path[1][:], ".r", label = 'course') #plt.plot(x,y,'-b',label='trajectory') plt.legend() plt.xlabel("x[m]") plt.ylabel("y[m]") plt.grid(True) _, (ax1, ax2) = plt.subplots(2, sharex='row') ax1.plot(t, [iv * 3.6 for iv in v], "-r") ax1.set_ylabel("Speed[km/h]") ax1.grid(True) ax2.plot(t, lat_error, label='lateral error to next [m]') ax2.set_ylabel("Lateral Error") ax2.set_xlabel("Time[s]") plt.grid(True) plt.show()
def h_cv(x): return np.array([x[0], x[2]]) def e(x): res = [] for n in range(x.shape[0]): res.append(np.sqrt(x[n][0]**2 + x[n][2]**2)) return res dt = 1.0 random.seed(1234) ukf = UnscentedKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, kappa=0) ukf.x = np.array([100., 0., 0., 0.]) ukf.R = np.diag([25, 25]) ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt, var=0.04) ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt, var=0.04) ckf = CubatureKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt) ckf.x = np.array([100., 0., 0., 0.]) ckf.R = np.diag([25, 25]) ckf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt, var=0.04) ckf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt, var=0.04) uxs = [] pxs = [] zs = [] txs = []
def hx(x): return (x[0]**2 + x[2]**2)**.5 pass def fx(x, dt): result = x.copy() result[0] += x[1]*dt return result f = UKF(3, 1, dt= dt, hx=hx, fx=fx, kappa=1) radar = RadarSim(dt, pos=-1000., vel=100., alt=1000.) f.x = array([0, 90, 1005]) f.R = 0.1 f.Q *= 0.002 xs = [] track = [] for i in range(int(20/dt)): z = radar.get_range() track.append((radar.pos, radar.vel, radar.alt))
class StateEstimation(object): ''' Class that estimates the state of the drone using an Unscented Kalman Filter (UKF) applied to raw sensor data. ''' # TODO: Make a reference to the UKF math document that is being written up, # once it is in a complete enough state and can be placed in a shared # location. def __init__(self, ir_throttled=False, imu_throttled=False, optical_flow_throttled=False): # self.ready_to_filter is False until we get initial measurements in # order to be able to initialize the filter's state vector x and # covariance matrix P. self.ready_to_filter = False self.printed_filter_start_notice = False self.got_ir = False self.got_optical_flow = False self.ir_topic_str = '/pidrone/infrared' self.imu_topic_str = '/pidrone/imu' self.optical_flow_topic_str = '/pidrone/picamera/twist' throttle_suffix = '_throttle' if ir_throttled: self.ir_topic_str += throttle_suffix if imu_throttled: self.imu_topic_str += throttle_suffix if optical_flow_throttled: self.optical_flow_topic_str += throttle_suffix self.in_callback = False self.initialize_ukf() # The last time that we received an input and formed a prediction with # the state transition function self.last_state_transition_time = None # Time in seconds between consecutive state transitions, dictated by # when the inputs come in self.dt = None # Initialize the last control input as 0 m/s^2 along each axis self.last_control_input = np.array([0.0, 0.0, 0.0]) self.initialize_ros() def initialize_ros(self): ''' Initialize ROS-related objects, e.g., the node, subscribers, etc. ''' self.node_name = 'state_estimator_ukf_test_2' print 'Initializing {} node...'.format(self.node_name) rospy.init_node(self.node_name) # Subscribe to topics to which the drone publishes in order to get raw # data from sensors, which we can then filter rospy.Subscriber(self.imu_topic_str, Imu, self.imu_data_callback) rospy.Subscriber(self.ir_topic_str, Range, self.ir_data_callback) rospy.Subscriber(self.optical_flow_topic_str, TwistStamped, self.optical_flow_data_callback) # Create the publisher to publish state estimates self.state_pub = rospy.Publisher('/pidrone/state', State, queue_size=1, tcp_nodelay=False) def initialize_ukf(self): ''' Initialize the parameters of the Unscented Kalman Filter (UKF) that is used to estimate the state of the drone. ''' # Number of state variables being tracked self.state_vector_dim = 6 # The state vector consists of the following column vector. # Note that FilterPy initializes the state vector with zeros. # [[x], # [y], # [z], # [x_vel], # [y_vel], # [z_vel]] # Number of measurement variables that the drone receives self.measurement_vector_dim = 3 # The measurement variables consist of the following vector: # [[slant_range], # [x_vel], # [y_vel]] # Function to generate sigma points for the UKF # TODO: Modify these sigma point parameters appropriately. Currently # just guesses sigma_points = MerweScaledSigmaPoints(n=self.state_vector_dim, alpha=0.1, beta=2.0, kappa=(3.0-self.state_vector_dim)) # Create the UKF object # Note that dt will get updated dynamically as sensor data comes in, # as will the measurement function, since measurements come in at # distinct rates. Setting compute_log_likelihood=False saves some # computation. self.ukf = UnscentedKalmanFilter(dim_x=self.state_vector_dim, dim_z=self.measurement_vector_dim, dt=1.0, hx=self.measurement_function, fx=self.state_transition_function, points=sigma_points, compute_log_likelihood=False) self.initialize_ukf_matrices() def initialize_ukf_matrices(self): ''' Initialize the covariance matrices of the UKF ''' # Initialize state covariance matrix P: # TODO: Tune these initial values appropriately. Currently these are # just guesses self.ukf.P = np.diag([0.1, 0.1, 0.1, 0.2, 0.2, 0.2]) # Initialize the process noise covariance matrix Q: # TODO: Tune appropriately. Currently just a guess self.ukf.Q = np.diag([0.01, 0.01, 0.01, 1.0, 1.0, 1.0])*0.005 # Initialize the measurement covariance matrix R # IR slant range variance (m^2), determined experimentally in a static # setup with mean range around 0.335 m: self.measurement_cov_ir = np.array([2.2221e-05]) self.measurement_cov_optical_flow = np.diag([0.01, 0.01]) def update_input_time(self, msg): ''' Update the time at which we have received the most recent input, based on the timestamp in the header of a ROS message msg : a ROS message that includes a header with a timestamp that indicates the time at which the respective input was originally recorded ''' self.last_time_secs = msg.header.stamp.secs self.last_time_nsecs = msg.header.stamp.nsecs new_time = self.last_time_secs + self.last_time_nsecs*1e-9 # Compute the time interval since the last state transition / input self.dt = new_time - self.last_state_transition_time # Set the current time at which we just received an input # to be the last input time self.last_state_transition_time = new_time def initialize_input_time(self, msg): ''' Initialize the input time (self.last_state_transition_time) based on the timestamp in the header of a ROS message. This is called before we start filtering in order to attain an initial time value, which enables us to then compute a time interval self.dt by calling self.update_input_time() msg : a ROS message that includes a header with a timestamp ''' self.last_time_secs = msg.header.stamp.secs self.last_time_nsecs = msg.header.stamp.nsecs self.last_state_transition_time = (self.last_time_secs + self.last_time_nsecs*1e-9) def ukf_predict(self): ''' Compute the prior for the UKF based on the current state, a control input, and a time step. ''' self.ukf.predict(dt=self.dt, u=self.last_control_input) def print_notice_if_first(self): if not self.printed_filter_start_notice: print 'Starting filter' self.printed_filter_start_notice = True def imu_data_callback(self, data): ''' Handle the receipt of an Imu message. Only take the linear acceleration along the z-axis. This method PREDICTS with a control input. ''' if self.in_callback: return self.in_callback = True self.last_control_input = np.array([data.linear_acceleration.x, data.linear_acceleration.y, data.linear_acceleration.z]) # self.last_control_input = np.array([0.0, # 0.0, # data.linear_acceleration.z]) if self.ready_to_filter: # Wait to predict until we get an initial IR measurement to # initialize our state vector self.print_notice_if_first() self.update_input_time(data) self.ukf_predict() self.publish_current_state() self.in_callback = False def ir_data_callback(self, data): ''' Handle the receipt of a Range message from the IR sensor. This method PREDICTS with the most recent control input and UPDATES. ''' if self.in_callback: return self.in_callback = True if self.ready_to_filter: self.print_notice_if_first() self.update_input_time(data) self.ukf_predict() # Now that a prediction has been formed to bring the current prior # state estimate to the same point in time as the measurement, # perform a measurement update with the slant range reading measurement_z = np.array([data.range]) self.ukf.update(measurement_z, hx=self.measurement_function_ir, R=self.measurement_cov_ir) self.publish_current_state() else: self.initialize_input_time(data) # Got a raw slant range reading, so update the initial state # vector of the UKF self.ukf.x[2] = data.range self.ukf.x[5] = 0.0 # initialize velocity as 0 m/s # Update the state covariance matrix to reflect estimated # measurement error. Variance of the measurement -> variance of # the corresponding state variable self.ukf.P[2, 2] = self.measurement_cov_ir[0] self.got_ir = True self.check_if_ready_to_filter() self.in_callback = False def optical_flow_data_callback(self, data): ''' Handle the receipt of a TwistStamped message from optical flow. The message parts that we will be using: - x velocity (m/s) - y velocity (m/s) This method PREDICTS with the most recent control input and UPDATES. ''' if self.in_callback: return self.in_callback = True if self.ready_to_filter: self.print_notice_if_first() self.update_input_time(data) self.ukf_predict() # Now that a prediction has been formed to bring the current prior # state estimate to the same point in time as the measurement, # perform a measurement update with x velocity, y velocity, and yaw # velocity data in the TwistStamped message # TODO: Verify the units of these velocities that are being # published measurement_z = np.array([data.twist.linear.x, # x velocity data.twist.linear.y]) # y velocity # Ensure that we are using subtraction to compute the residual #self.ukf.residual_z = np.subtract self.ukf.update(measurement_z, hx=self.measurement_function_optical_flow, R=self.measurement_cov_optical_flow) self.publish_current_state() else: self.initialize_input_time(data) # Update the initial state vector of the UKF self.ukf.x[3] = data.twist.linear.x # x velocity self.ukf.x[4] = data.twist.linear.y # y velocity # Update the state covariance matrix to reflect estimated # measurement error. Variance of the measurement -> variance of # the corresponding state variable self.ukf.P[3, 3] = self.measurement_cov_optical_flow[0, 0] self.ukf.P[4, 4] = self.measurement_cov_optical_flow[1, 1] self.got_optical_flow = True self.check_if_ready_to_filter() self.in_callback = False def check_if_ready_to_filter(self): self.ready_to_filter = (self.got_ir and self.got_optical_flow) def publish_current_state(self): ''' Publish the current state estimate and covariance from the UKF. This is a State message containing: - Header - PoseWithCovariance - TwistWithCovariance Note that a lot of these ROS message fields will be left empty, as the 1D UKF does not track information on the entire state space of the drone. ''' state_msg = State() state_msg.header.stamp.secs = self.last_time_secs state_msg.header.stamp.nsecs = self.last_time_nsecs state_msg.header.frame_id = 'global' # Get the current state estimate from self.ukf.x, and fill the rest of # the message with NaN state_msg.pose_with_covariance.pose.position.x = self.ukf.x[0] state_msg.pose_with_covariance.pose.position.y = self.ukf.x[1] state_msg.pose_with_covariance.pose.position.z = self.ukf.x[2] # TODO: Leave commented if JS interface Top View animation is desired # state_msg.pose_with_covariance.pose.orientation.x = np.nan # state_msg.pose_with_covariance.pose.orientation.y = np.nan # state_msg.pose_with_covariance.pose.orientation.z = np.nan # state_msg.pose_with_covariance.pose.orientation.w = np.nan # TODO: Leave following line if JS interface Top View animation is desired state_msg.pose_with_covariance.pose.orientation.w = 1 state_msg.twist_with_covariance.twist.linear.x = self.ukf.x[3] state_msg.twist_with_covariance.twist.linear.y = self.ukf.x[4] state_msg.twist_with_covariance.twist.linear.z = self.ukf.x[5] state_msg.twist_with_covariance.twist.angular.x = np.nan state_msg.twist_with_covariance.twist.angular.y = np.nan state_msg.twist_with_covariance.twist.angular.z = np.nan # Prepare covariance matrices # TODO: Finish populating these matrices, if deemed necessary # 36-element array, in a row-major order, according to ROS msg docs pose_cov_mat = np.full((36,), np.nan) twist_cov_mat = np.full((36,), np.nan) pose_cov_mat[14] = self.ukf.P[2, 2] # z variance twist_cov_mat[14] = self.ukf.P[5, 5] # z velocity variance # Add covariances to message state_msg.pose_with_covariance.covariance = pose_cov_mat state_msg.twist_with_covariance.covariance = twist_cov_mat self.state_pub.publish(state_msg) def state_transition_function(self, x, dt, u): ''' The state transition function to compute the prior in the prediction step, propagating the state to the next time step. x : current state. A NumPy array dt : time step. A float u : control input. A NumPy array ''' dt2 = dt**2.0 return x + np.array([x[3]*dt + 0.5*u[0]*dt2, x[4]*dt + 0.5*u[1]*dt2, x[5]*dt + 0.5*u[2]*dt2, u[0]*dt, u[1]*dt, u[2]*dt]) def measurement_function(self, x): ''' Transform the state x into measurement space. In this simple model, we assume that the range measurement corresponds exactly to position along the z-axis, as we are assuming there is no pitch and roll. x : current state. A NumPy array ''' pass def measurement_function_ir(self, x): return np.array([x[2]]) def measurement_function_optical_flow(self, x): return np.array([x[3], x[4]])
def test_circle(): from filterpy.kalman import KalmanFilter from math import radians def hx(x): radius = x[0] angle = x[1] x = cos(radians(angle)) * radius y = sin(radians(angle)) * radius return np.array([x, y]) def fx(x, dt): return np.array([x[0], x[1] + x[2], x[2]]) std_noise = .1 sp = JulierSigmaPoints(n=3, kappa=0.) f = UnscentedKalmanFilter(dim_x=3, dim_z=2, dt=.01, hx=hx, fx=fx, points=sp) f.x = np.array([50., 90., 0]) f.P *= 100 f.R = np.eye(2)*(std_noise**2) f.Q = np.eye(3)*.001 f.Q[0, 0] = 0 f.Q[2, 2] = 0 kf = KalmanFilter(dim_x=6, dim_z=2) kf.x = np.array([50., 0., 0, 0, .0, 0.]) F = np.array([[1., 1., .5, 0., 0., 0.], [0., 1., 1., 0., 0., 0.], [0., 0., 1., 0., 0., 0.], [0., 0., 0., 1., 1., .5], [0., 0., 0., 0., 1., 1.], [0., 0., 0., 0., 0., 1.]]) kf.F = F kf.P *= 100 kf.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]]) kf.R = f.R kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001) kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001) results = [] zs = [] kfxs = [] for t in range(12000): a = t / 30 + 90 x = cos(radians(a)) * 50. + randn() * std_noise y = sin(radians(a)) * 50. + randn() * std_noise # create measurement = t plus white noise z = np.array([x, y]) zs.append(z) f.predict() f.update(z) kf.predict() kf.update(z) # save data results.append(hx(f.x)) kfxs.append(kf.x) results = np.asarray(results) zs = np.asarray(zs) kfxs = np.asarray(kfxs) print(results) if DO_PLOT: plt.plot(zs[:, 0], zs[:, 1], c='r', label='z') plt.plot(results[:, 0], results[:, 1], c='k', label='UKF') plt.plot(kfxs[:, 0], kfxs[:, 3], c='g', label='KF') plt.legend(loc='best') plt.axis('equal')
def __init__(self, dim, limit, dim_z=2, fx=None, W=None, obs_noise_func=None, collision_func=None, sampling_period=0.5, kappa=1): """ dim : dimension of state ***Assuming dim==3: (x,y,theta), dim==4: (x,y,xdot,ydot), dim==5: (x,y,theta,v,w) limit : An array of two vectors. limit[0] = minimum values for the state, limit[1] = maximum value for the state dim_z : dimension of observation, fx : x_tp1 = fx(x_t, dt), state dynamic function W : state noise matrix obs_noise_func : observation noise matrix function of z collision_func : collision checking function n : the number of sigma points """ self.dim = dim self.limit = limit self.W = W if W is not None else np.zeros((self.dim, self.dim)) self.obs_noise_func = obs_noise_func self.collision_func = collision_func def hx(y, agent_state, measure_func=util.relative_measure): r_pred, alpha_pred, _ = measure_func(y, agent_state) return np.array([r_pred, alpha_pred]) def x_mean_fn_(sigmas, Wm): if dim == 3: x = np.zeros(dim) sum_sin, sum_cos = 0., 0. for i in range(len(sigmas)): s = sigmas[i] x[0] += s[0] * Wm[i] x[1] += s[1] * Wm[i] sum_sin += np.sin(s[2])*Wm[i] sum_cos += np.cos(s[2])*Wm[i] x[2] = np.arctan2(sum_sin, sum_cos) return x elif dim == 5: x = np.zeros(dim) sum_sin, sum_cos = 0., 0. for i in range(len(sigmas)): s = sigmas[i] x[0] += s[0] * Wm[i] x[1] += s[1] * Wm[i] x[3] += s[3] * Wm[i] x[4] += s[4] * Wm[i] sum_sin += np.sin(s[2])*Wm[i] sum_cos += np.cos(s[2])*Wm[i] x[2] = np.arctan2(sum_sin, sum_cos) return x else: return None def z_mean_fn_(sigmas, Wm): x = np.zeros(dim_z) sum_sin, sum_cos = 0., 0. for i in range(len(sigmas)): s = sigmas[i] x[0] += s[0] * Wm[i] sum_sin += np.sin(s[1])*Wm[i] sum_cos += np.cos(s[1])*Wm[i] x[1] = np.arctan2(sum_sin, sum_cos) return x def residual_x_(x, xp): """ x : state, [x, y, theta] xp : predicted state """ if dim == 3 or dim == 5: r_x = x - xp r_x[2] = util.wrap_around(r_x[2]) return r_x else: return None def residual_z_(z, zp): """ z : observation, [r, alpha] zp : predicted observation """ r_z = z - zp r_z[1] = util.wrap_around(r_z[1]) return r_z sigmas = JulierSigmaPoints(n=dim, kappa=kappa) self.ukf = UnscentedKalmanFilter(dim, dim_z, sampling_period, fx=fx, hx=hx, points=sigmas, x_mean_fn=x_mean_fn_, z_mean_fn=z_mean_fn_, residual_x=residual_x_, residual_z=residual_z_)
def hx(x): """ returns slant range based on downrange distance and altitude""" return (x[0]**2 + x[2]**2)**.5 from filterpy.kalman.sigma_points import JulierSigmaPoints as sigmapoints if __name__ == "__main__": dt = 0.05 points = sigmapoints(n=3, kappa = 0); radarUKF = UKF(dim_x=3, dim_z=1, dt=dt, hx = hx, fx = fx, points = points); radarUKF.Q *= Q_discrete_white_noise(3, 1, .01) radarUKF.R *= 10 radarUKF.x = np.array([0., 90., 1100.]) radarUKF.P *= 100. t = np.arange(0, 20+dt, dt) n = len(t) xs = [] rs = [] for i in range(n): r = GetRadar(dt) rs.append(r) #rs = r; radarUKF.predict(); radarUKF.update(r)
from filterpy.common import stats import matplotlib.pyplot as plt p = (-10, -10) def hx(x): dx = x[0] - hx.p[0] dy = x[1] - hx.p[1] return np.array([atan2(dy,dx), (dx**2 + dy**2)**.5]) def fx(x,dt): return x kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, kappa=2.) kf.x = np.array([100, 100.]) kf.P *= 40 hx.p = kf.x - np.array([50,50]) d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5 stats.plot_covariance_ellipse( kf.x, cov=kf.P, axis_equal=True, facecolor='y', edgecolor=None, alpha=0.6) plt.scatter([100], [100], c='y', label='Initial') kf.R[0,0] = radians (1)**2 kf.R[1,1] = 2.**2
class RearEndKalman: def __init__(self, trueTrajectory, dt=.1, noise = 0.): from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints self.dt = dt sigmas = SigmaPoints(3, alpha=.1, beta=2., kappa=0.) self.KF = UKF(dim_x=3, dim_z=2, fx=f_kal_a, hx=h_kal, dt=dt, points=sigmas) self.KF.Q = np.diag([1., 0.5, 0.2]) self.KF.R = np.diag([2., 1.12])*noise + np.diag([.05, .05]) self.first = True def predict(self, vData, predictTimes): nobs = vData.shape[0] vcurrentState = vData.iloc[nobs-1].copy() # first check if vehicle has already stopped, don't need KF then if np.mean(vData['speed'].iloc[nobs-2:nobs]) <= 0: returnedStates = vData[vData['time']<0] # empty for time in predictTimes: vNextState = vcurrentState.copy() vNextState.time = time vNextState.y = -8.25 returnedStates = returnedStates.append(vNextState) return returnedStates # # train KF # self.KF.x[0] = vData.iloc[0].x # self.KF.x[1] = vData.iloc[0].speed # self.KF.predict() # # for time in np.arange(1,nobs-1): # vState = vData.iloc[time] # self.KF.update(np.array([vState.x,vState.speed])) # self.KF.predict() # self.KF.update(np.array([vData.iloc[nobs-1].x,vData.iloc[nobs-1].speed])) vState = vData.iloc[nobs-1] if self.first: self.KF.x[0] = vState.x self.KF.x[1] = vState.speed #self.KF.predict() self.first = False else: if vState.speed < 0: self.KF.update(np.array([vState.x, 0.])) else: self.KF.update(np.array([vState.x,vState.speed])) # now you can predict # return a dataframe of answers vcurrentState = vData.iloc[vData.shape[0]-1].copy() vcurrentState.x = self.KF.x[0] vcurrentState.speed = self.KF.x[1] returnedStates = vData[vData['time']<0] # empty for time in predictTimes: vNextState = movePhysics(vcurrentState, self.KF.x[2]/self.dt, time) returnedStates = returnedStates.append(vNextState) self.KF.predict() return returnedStates
class InMagPredictor: stateNum = 10 # x, y, z, q0, q1, q2, q3, wx, wy, wz measureNum = SLAVES * 3 + 2 # sensor*3 + IMU points = MerweScaledSigmaPoints(n=stateNum, alpha=0.3, beta=2., kappa=3 - stateNum) dt = 0.03 # 时间间隔[s] t0 = datetime.datetime.now() # 初始化时间戳 def __init__(self, sensor_std, state0, state): """ 初始化UKF滤波器 :param sensor_std:【float】sensor噪声标准差 [mG] :param state0:【np.array】初始值 (stateNum,) :param state: 【np.array】真实值 (stateNum,) """ # self.mp = MahonyPredictor() self.ukf = UKF(dim_x=self.stateNum, dim_z=self.measureNum, dt=self.dt, points=self.points, fx=self.f, hx=h) self.ukf.x = state0.copy() # 初始值 q0i, q1i, q2i, q3i = state0[3: 7] q0, q1, q2, q3 = state[3: 7] for i in range(6): self.ukf.R[i, i] = sensor_std for i in range(7, self.measureNum): self.ukf.R[i, i] = 5 self.ukf.P = np.eye(self.stateNum) * 0.01 for i in range(3): self.ukf.P[i, i] = 1.5 * (state0[i] - state[i]) ** 2 # 位置初始值的误差 # self.ukf.P[3: 7, 3: 7] = 1.5 * np.array([ # 姿态四元数初始值的误差 # [(q0i - q0) ** 2, (q0i - q0) * (q1i - q1), (q0i - q0) * (q2i - q2), (q0i - q0) * (q3i - q3)], # [(q0i - q0) * (q1i - q1), (q1i - q1) ** 2, 0, 0], # [(q0i - q0) * (q2i - q2), 0, (q2i - q2) ** 2, 0], # [(q0i - q0) * (q3i - q3), 0, 0, (q3i - q3) ** 2] # ]) self.ukf.P += np.eye(self.stateNum) * 0.001 self.ukf.Q = np.eye(self.stateNum) * 0.05 * self.dt # 将速度作为过程噪声来源,Qi = [v*dt] # for i in range(3, self.stateNum): # self.ukf.Q[i, i] = 0.0001 Qqii, Qqij = 0.01, 0.001 self.ukf.Q[3: 7, 3: 7] = np.array([ # 精细化定义姿态(四元数)的过程误差 [Qqii, Qqij, Qqij, Qqij], [Qqij, Qqii, 0, 0], [Qqij, 0, Qqii, 0], [Qqij, 0, 0, Qqij] ]) for i in range(7, self.stateNum): self.ukf.Q[i, i] = 0.001 # 角速度的过程误差 def f(self, x, dt): wx, wy, wz = self.ukf.x[-3:] A = np.eye(self.stateNum) A[3:7, 3:7] = np.eye(4) + 0.5 * dt * np.array([[0, -wx, -wy, -wz], [wx, 0, wz, -wy], [wy, -wz, 0, wx], [wz, wy, -wx, 0]]) return np.hstack(np.dot(A, x.reshape(self.stateNum, 1))) def h0(self, state): """ 使用互补滤波预测的结果(q)来估算预估量的四元数 :param state: 胶囊的位姿状态 :return: 预估量的四元数 """ H = np.zeros((7, 4)) for i in range(3): H[i + 4, i] = 0 return np.dot(state, H) def run(self, z, printBool): pos = np.round(self.ukf.x[:3], 3) em = q2R(self.ukf.x[3: 7])[:, -1] timeCost = (datetime.datetime.now() - self.t0).total_seconds() if printBool: print(r'pos={}m, em={}, w={}, timeCost={:.3f}s'.format(pos, np.round(em, 3), np.round(self.ukf.x[-3:], 3), timeCost)) self.t0 = datetime.datetime.now() # 使用IMU的数据更新滤波器 # for i in range(20): # self.mp.IMUupdate(z[6: 9], z[-3:]) # emq = q2R(self.mp.q)[-1] # print(r'IMU update: pos={}m, em={}, w={}'.format(pos, np.round(emq, 3), np.round(z[6: 9], 3))) # self.ukf.x[3: 7] = self.mp.q # self.ukf.x[7:] = z[6: 9] # 使用磁传感器的数据更新滤波器 self.ukf.predict() self.ukf.update(z) def generate_data(self, state, sensor_std, printBool): """ 生成模拟数据 :param state: 【np.array】模拟的胶囊状态 (m,) :param sensor_std:【float】磁传感器的噪声标准差 [mG] :param printBool: 【bool】是否打印输出 :return: 【np.array】模拟的B值 + 加速度计的数值, (num_data, ) """ Bmid = h(state)[:-2] # 模拟B值数据的中间值 Bsim = np.zeros(SLAVES * 3) for j in range(SLAVES * 3): Bsim[j] = np.random.normal(Bmid[j], sensor_std, 1) R = q2R(state[3: 7]) accSim = R[:, -1] if printBool: print('Bmid={}'.format(np.round(Bmid, 0))) print('Bsim={}'.format(np.round(Bsim, 0))) print('truth: pos={}m, e_moment={}\n'.format(state[:3], np.round(accSim, 3))) return np.concatenate((Bsim, accSim)) def sim(self, states, sensor_std, plotType, plotBool, printBool, maxIter=20): """ 使用模拟的观测值验证算法的准确性 :param states: 【list】模拟的真实状态,可以有多个不同的状态 :param sensor_std: 【float】sensor的噪声标准差[mG] :param plotType: 【tuple】描绘位置的分量 'xy' or 'yz' :param plotBool: 【bool】是否绘图 :param printBool: 【bool】是否打印输出 :param maxIter: 【int】最大迭代次数 :return: 【tuple】 位置[x, y, z]和姿态ez的误差百分比 """ # self.ukf.x = state0.copy() # 初始值 state = states[0] # 真实值 # simData = self.generate_data(state, sensor_std, printBool) simData = generate_data(self.measureNum, state, sensor_std, printBool) err_pos, err_em = (0, 0) for i in range(maxIter): if plotBool: print('=========={}=========='.format(i)) plt.ion() plotP(self, state, i, plotType) if i == maxIter - 1: plt.ioff() plt.show() self.run(simData, printBool) posTruth, emTruth = state[:3], q2R(state[3: 7])[:, -1] pos, em = self.ukf.x[:3], q2R(self.ukf.x[3: 7])[:, -1] err_pos = np.linalg.norm(pos - posTruth) / np.linalg.norm(posTruth) err_em = np.linalg.norm(em - emTruth) # 方向矢量本身是归一化的 print('err_pos={:.0%}, err_em={:.0%}'.format(err_pos, err_em)) return (err_pos, err_em)
g = -9.81 x[0] = x[0] + x[1] * dt x[1] = x[1] + g * dt return x def h_cv(x): """Measurement function - measuring only position""" return np.array([x[0]]) starting_conditions = [1e5, 0., 2000] points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=2., kappa=0) ukf = UKF(dim_x=2, dim_z=1, fx=f_cv, hx=h_cv, dt=dt, points=points) ukf.x = np.array([1.001e5, 0.]) ukf.R = np.array([[1e3]]) #ukf.H = np.array([[1]]) ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.5) uxs = [] trux = [] truv = [] measx = [] zs = np.arange(0, 465 + dt, dt) time = [] t = 0 f = falling_object(starting_conditions[0], starting_conditions[1], starting_conditions[2], std_x)
def __init__(self, g, zeta, leader, trajectory_delay=2.0, update_delta_t=0.01, orig_leader=None, orig_leader_delay=None, noise_sigma=0.0, log_file=None, visibility_fov=config.FOV_ANGLE, visibility_radius=None, id=None, dt=0.02): """ Construct a follower Behavior. leader is the Bot to be followed g > 0 is a tuning parameter zeta in (0, 1) is a damping coefficient trajectory_delay is the time interval between leader and follower """ super(Follower, self).__init__() assert(isinstance(leader, Bot)) assert(isinstance(orig_leader, Bot)) self.radius = config.BOT_RADIUS self.leader = leader self.orig_leader = orig_leader self.trajectory_delay = trajectory_delay assert g > 0, "Follower: g parameter must be positive" self.g = g assert 0 < zeta < 1, "Follower: zeta parameter must be in (0, 1)" self.zeta = zeta # leader_positions stores config.POSITIONS_BUFFER_SIZE tuples; # tuple's first field is time, second is the # corresponding position of the leader self.leader_positions = RingBuffer(config.POSITIONS_BUFFER_SIZE) self.noisy_leader_positions = RingBuffer(config.POSITIONS_BUFFER_SIZE) self.leader_is_visible = False # precise_orig_leader_states stores the first leader's precise state; # used to calculate "real" errors (as opposed to calculations # w.r.t. the approximation curve) self.precise_orig_leader_states = [] # precise_leader_states is used for the same purpose, but with the bot's own leader self.precise_leader_states = [] self.update_delta_t = update_delta_t needed_buffer_size = config.SAMPLE_COUNT // 2 + self.trajectory_delay / self.update_delta_t if needed_buffer_size > config.POSITIONS_BUFFER_SIZE: sys.stderr.write("leader_positions buffer is too small for update_delta_t = {}".format(self.update_delta_t) + " (current = {}, required = {})".format(config.POSITIONS_BUFFER_SIZE, needed_buffer_size)) raise RuntimeError, "leader_positions buffer is too small" self.last_update_time = 0.0 self.noise_sigma = noise_sigma self.log_file = log_file self.visibility_fov = visibility_fov if visibility_radius is None: visibility_radius = 2.0 * trajectory_delay * config.BOT_VEL_CAP self.visibility_radius = visibility_radius self.id = id; if orig_leader_delay is None: orig_leader_delay = trajectory_delay self.orig_leader_delay = orig_leader_delay if self.log_file is not None: log_dict = {"id": self.id, "g": self.g, "zeta": self.zeta, "noise_sigma": self.noise_sigma, "reference_points_cnt": config.SAMPLE_COUNT, "trajectory_delay": trajectory_delay} print >> self.log_file, log_dict q = 0.01 # std of process r = 0.05 # std of measurement self.delta_time = dt if not config.DISABLE_UKF: points = MerweScaledSigmaPoints(n=6, alpha=.1, beta=2., kappa=-3) self.ukf = UKF(dim_x=6, dim_z=2, fx=f, hx=h, dt=dt, points=points) self.ukf_x_initialized = 0 #self.ukf.x = np.array([0, 0, 1, pi/4, 0, 0]) self.ukf.R = np.diag([r, r]); self.ukf.Q = np.diag([0, 0, 0, 0, q, q])
pf = ParticleFilter(STATE_DIM, N, pdf_x0, noisyUpdate, measurement_pdf, q0_sampler) particles_thetas = [] particles_omegas = [] particles_times = [] particles_s = [] points = MerweScaledSigmaPoints(STATE_DIM, alpha=.1, beta=2., kappa=-1) def measurement(xhat): return [xhat[THETA]] ukf = UnscentedKalmanFilter(STATE_DIM, 1, dt, measurement, noiselessUpdate, points) ukf.x = xhat0 ukf.P = P0 ukf.R = R startTimer("main") NUM = 1 def drawEllipse(cov, ax, avg, K=3): angles = np.linspace(0, math.pi * 2, 100) w, v = np.linalg.eig(np.linalg.inv(cov)) lambda1 = w[0] lambda2 = w[1]