def plot_sigma_points(): x = np.array([0, 0]) P = np.array([[4, 2], [2, 4]]) sigmas = MerweScaledSigmaPoints(n=2, alpha=.3, beta=2., kappa=1.) S0 = sigmas.sigma_points(x, P) Wm0, Wc0 = sigmas.Wm, sigmas.Wc sigmas = MerweScaledSigmaPoints(n=2, alpha=1., beta=2., kappa=1.) S1 = sigmas.sigma_points(x, P) Wm1, Wc1 = sigmas.Wm, sigmas.Wc def plot_sigmas(s, w, **kwargs): min_w = min(abs(w)) scale_factor = 100 / min_w return plt.scatter(s[:, 0], s[:, 1], s=abs(w)*scale_factor, alpha=.5, **kwargs) plt.subplot(121) plot_sigmas(S0, Wc0, c='b') plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4]) plt.title('alpha=0.3') plt.subplot(122) plot_sigmas(S1, Wc1, c='b', label='Kappa=2') plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4]) plt.title('alpha=1') plt.show()
def show_sigma_transform(with_text=False): fig = plt.figure() ax=fig.gca() x = np.array([0, 5]) P = np.array([[4, -2.2], [-2.2, 3]]) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=9) sigmas = MerweScaledSigmaPoints(2, alpha=.5, beta=2., kappa=0.) S = sigmas.sigma_points(x=x, P=P) plt.scatter(S[:,0], S[:,1], c='k', s=80) x = np.array([15, 5]) P = np.array([[3, 1.2],[1.2, 6]]) plot_covariance_ellipse(x, P, facecolor='g', variance=9, alpha=0.3) ax.add_artist(arrow(S[0,0], S[0,1], 11, 4.1, 0.6)) ax.add_artist(arrow(S[1,0], S[1,1], 13, 7.7, 0.6)) ax.add_artist(arrow(S[2,0], S[2,1], 16.3, 0.93, 0.6)) ax.add_artist(arrow(S[3,0], S[3,1], 16.7, 10.8, 0.6)) ax.add_artist(arrow(S[4,0], S[4,1], 17.7, 5.6, 0.6)) ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) if with_text: plt.text(2.5, 1.5, r"$\chi$", fontsize=32) plt.text(13, -1, r"$\mathcal{Y}$", fontsize=32) #plt.axis('equal') plt.show()
def print_sigmas(n=1, mean=5, cov=3, alpha=.1, beta=2., kappa=2): points = MerweScaledSigmaPoints(n, alpha, beta, kappa) print('sigmas: ', points.sigma_points(mean, cov).T[0]) Wm, Wc = points.weights() print('mean weights:', Wm) print('cov weights:', Wc) print('lambda:', alpha**2 *(n+kappa) - n) print('sum cov', sum(Wc))
def test_scaled_weights(): for n in range(1,5): for alpha in np.linspace(0.99, 1.01, 100): for beta in range(0,2): for kappa in range(0,2): sp = MerweScaledSigmaPoints(n, alpha, 0, 3-n) Wm, Wc = sp.weights() assert abs(sum(Wm) - 1) < 1.e-1 assert abs(sum(Wc) - 1) < 1.e-1
def show_sigma_selections(): ax=plt.gca() ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) x = np.array([2, 5]) P = np.array([[3, 1.1], [1.1, 4]]) points = MerweScaledSigmaPoints(2, .09, 2., 1.) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') x = np.array([5, 5]) points = MerweScaledSigmaPoints(2, .15, 1., .15) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') x = np.array([8, 5]) points = MerweScaledSigmaPoints(2, .2, 3., 10) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') plt.axis('equal') plt.xlim(0,10); plt.ylim(0,10) plt.show()
def test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together sp0 = JulierSigmaPoints(n=2, kappa=kappa) sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000) sp2 = MerweScaledSigmaPoints(n=2, kappa=0, beta=2, alpha=1e-3) sp3 = SimplexSigmaPoints(n=2) w0, _ = sp0.weights() w1, _ = sp1.weights() w2, _ = sp2.weights() w3, _ = sp3.weights() Xi0 = sp0.sigma_points(x, P) Xi1 = sp1.sigma_points(x, P) Xi2 = sp2.sigma_points(x, P) Xi3 = sp3.sigma_points(x, P) assert max(Xi1[:,0]) > max(Xi0[:,0]) assert max(Xi1[:,1]) > max(Xi0[:,1]) if DO_PLOT: plt.figure() for i in range(Xi0.shape[0]): plt.scatter((Xi0[i,0]-x[0, 0])*w0[i] + x[0, 0], (Xi0[i,1]-x[0, 1])*w0[i] + x[0, 1], color='blue', label='Julier low $\kappa$') for i in range(Xi1.shape[0]): plt.scatter((Xi1[i, 0]-x[0, 0]) * w1[i] + x[0,0], (Xi1[i, 1]-x[0, 1]) * w1[i] + x[0,1], color='green', label='Julier high $\kappa$') # for i in range(Xi2.shape[0]): # plt.scatter((Xi2[i, 0] - x[0, 0]) * w2[i] + x[0, 0], # (Xi2[i, 1] - x[0, 1]) * w2[i] + x[0, 1], # color='red') for i in range(Xi3.shape[0]): plt.scatter((Xi3[i, 0] - x[0, 0]) * w3[i] + x[0, 0], (Xi3[i, 1] - x[0, 1]) * w3[i] + x[0, 1], color='black', label='Simplex') stats.plot_covariance_ellipse([1, 2], P)
def plot_ukf_vs_mc(alpha=0.001, beta=3., kappa=1.): def fx(x): return x**3 def dfx(x): return 3*x**2 mean = 1 var = .1 std = math.sqrt(var) data = normal(loc=mean, scale=std, size=50000) d_t = fx(data) points = MerweScaledSigmaPoints(1, alpha, beta, kappa) Wm, Wc = points.Wm, points.Wc sigmas = points.sigma_points(mean, var) sigmas_f = np.zeros((3, 1)) for i in range(3): sigmas_f[i] = fx(sigmas[i, 0]) ### pass through unscented transform ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc) ukf_mean = ukf_mean[0] ukf_std = math.sqrt(ukf_cov[0]) norm = scipy.stats.norm(ukf_mean, ukf_std) xs = np.linspace(-3, 5, 200) plt.plot(xs, norm.pdf(xs), ls='--', lw=2, color='b') try: plt.hist(d_t, bins=200, density=True, histtype='step', lw=2, color='g') except: # older versions of matplotlib don't have the density keyword plt.hist(d_t, bins=200, normed=True, histtype='step', lw=2, color='g') actual_mean = d_t.mean() plt.axvline(actual_mean, lw=2, color='g', label='Monte Carlo') plt.axvline(ukf_mean, lw=2, ls='--', color='b', label='UKF') plt.legend() plt.show() print('actual mean={:.2f}, std={:.2f}'.format(d_t.mean(), d_t.std())) print('UKF mean={:.2f}, std={:.2f}'.format(ukf_mean, ukf_std))
def show_sigma_selections(): ax=plt.gca() ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) x = np.array([2, 5]) P = np.array([[3, 1.1], [1.1, 4]]) points = MerweScaledSigmaPoints(2, .05, 2., 1.) sigmas = points.sigma_points(x, P) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5]) plt.scatter(sigmas[:,0], sigmas[:, 1], c='k', s=50) x = np.array([5, 5]) points = MerweScaledSigmaPoints(2, .15, 2., 1.) sigmas = points.sigma_points(x, P) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5]) plt.scatter(sigmas[:,0], sigmas[:, 1], c='k', s=50) x = np.array([8, 5]) points = MerweScaledSigmaPoints(2, .4, 2., 1.) sigmas = points.sigma_points(x, P) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5]) plt.scatter(sigmas[:,0], sigmas[:, 1], c='k', s=50) plt.axis('equal') plt.xlim(0,10); plt.ylim(0,10) plt.show()
def plot_ukf_vs_mc(alpha=0.001, beta=3.0, kappa=1.0): def fx(x): return x ** 3 def dfx(x): return 3 * x ** 2 mean = 1 var = 0.1 std = math.sqrt(var) data = normal(loc=mean, scale=std, size=50000) d_t = fx(data) points = MerweScaledSigmaPoints(1, alpha, beta, kappa) Wm, Wc = points.weights() sigmas = points.sigma_points(mean, var) sigmas_f = np.zeros((3, 1)) for i in range(3): sigmas_f[i] = fx(sigmas[i, 0]) ### pass through unscented transform ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc) ukf_mean = ukf_mean[0] ukf_std = math.sqrt(ukf_cov[0]) norm = scipy.stats.norm(ukf_mean, ukf_std) xs = np.linspace(-3, 5, 200) plt.plot(xs, norm.pdf(xs), ls="--", lw=2, color="b") plt.hist(d_t, bins=200, normed=True, histtype="step", lw=2, color="g") actual_mean = d_t.mean() plt.axvline(actual_mean, lw=2, color="g", label="Monte Carlo") plt.axvline(ukf_mean, lw=2, ls="--", color="b", label="UKF") plt.legend() plt.show() print("actual mean={:.2f}, std={:.2f}".format(d_t.mean(), d_t.std())) print("UKF mean={:.2f}, std={:.2f}".format(ukf_mean, ukf_std))
def test_linear_2d_merwe(): """ 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 # test __repr__ doesn't crash str(kf) 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: plt.figure() zs = np.asarray(zs) plt.plot(zs[:, 0], marker='+') plt.plot(Ms[:, 0], c='b') plt.plot(smooth_x[:, 0], smooth_x[:, 2], c='r') print(smooth_x)
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 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
def __init__(self, dt, wheel_base, var_rate_enc, meas_da): sp = MerweScaledSigmaPoints(n=8, alpha=0.3, beta=2., kappa=-1.) UnscentedKalmanFilter.__init__( self, dim_x=8, dim_z=4, dt=dt, fx=TankKalmanDelta3.f_func, residual_x=TankKalmanDelta3.x_residual_fn, x_mean_fn=TankKalmanDelta3.x_mean_fn, hx=TankKalmanDelta3.h_func, residual_z=TankKalmanDelta3.h_residual_fn, z_mean_fn=TankKalmanDelta3.h_mean_fn, points=sp) self.wheel_base = wheel_base self.var_a_s = 4.0 self.var_a_p = 1e-10 self.var_a_a = math.radians(45.0) # starting point and covariance self.x = np.array(8 * [ 0., ]) sig_a_start = math.radians(5.0) self.P = np.diag([ 0.5**2, 0.5**2, dt**2 * self.var_a_s, self.var_a_s, dt**2 * self.var_a_p, sig_a_start**2, dt**2 * self.var_a_a, self.var_a_a ]) self.meas_da = meas_da self.prev_x = None self.prev_encoders = [0, 0, 0] self.tank_encoder = TankEncoderTracker(wheel_base, var_rate_enc, var_rate_enc) return
def create_ukf(vehicle, landmarks, P0, Q, dt, extents): def f(x, dt, u=np.zeros(2), extents=extents): vehicle.x = x.copy() print(u, dt) vehicle.move(u=u, dt=dt, extents=extents) return vehicle.x def h(x, landmark=None): assert landmark is not None hx = np.empty(2) lx, ly = landmark hx[0] = np.sqrt((lx - x[0])**2 + (ly - x[1])**2) hx[1] = np.arctan2(ly - x[1], lx - x[0]) return hx def residual_h(z, h): """Subtract [range, bearing] vectors, accounting for angle""" y = z - h y[1] = mpi_to_pi(y[1]) wrap(y, extents) return y def residual_x(xa, xb): """Subtract [x, y, phi] vectors, accounting for angle""" dx = xa - xb dx[2] = mpi_to_pi(dx[2]) wrap(dx, extents) return dx def state_mean(sigmas, w_m): x = np.empty(3) # This is a hack (and not a very good one) to handle periodic boundary # crossings. Seems to help a little. std = np.std(sigmas[:, :2]) if std > (extents[1] - extents[0]) / 4: x[:2] = sigmas[0, :2] else: x[0] = np.dot(sigmas[:, 0], w_m) x[1] = np.dot(sigmas[:, 1], w_m) sum_sin = np.dot(np.sin(sigmas[:, 2]), w_m) sum_cos = np.dot(np.cos(sigmas[:, 2]), w_m) x[2] = np.arctan2(sum_sin, sum_cos) wrap(x, extents) return x def z_mean(sigmas, w_m): z_count = sigmas.shape[1] x = np.zeros(z_count) for z in range(0, z_count, 2): sum_sin = np.dot(np.sin(sigmas[:, z + 1]), w_m) sum_cos = np.dot(np.cos(sigmas[:, z + 1]), w_m) x[z] = np.dot(sigmas[:, z], w_m) x[z + 1] = np.arctan2(sum_sin, sum_cos) return x points = MerweScaledSigmaPoints(n=3, alpha=1e-3, beta=2, kappa=0, subtract=residual_x) ukf = UKF( dim_x=3, dim_z=2, fx=f, hx=h, dt=dt, points=points, x_mean_fn=state_mean, z_mean_fn=z_mean, residual_x=residual_x, residual_z=residual_h, ) ukf.x = vehicle.x.copy() ukf.P = P0.copy() ukf.Q = Q # This large scale factor is theoretically unjustified. # TODO: without it, the sim crashes. Figure this out! ukf.R = 1e5 * np.diag([vehicle.range_sigma**2, vehicle.bearing_sigma**2]) # ukf.R = np.diag([vehicle.range_sigma**2, vehicle.bearing_sigma**2]) return ukf
# from scipy.spatial import Voronoi, voronoi_plot_2d # import skfmm logger = logging.getLogger("vision") logger.addHandler(logging.StreamHandler()) logger.setLevel(logging.DEBUG) # import vision # import datetime # from scipy import interpolate # from vision import controllers # import filterpy from filterpy.kalman import MerweScaledSigmaPoints # from filterpy.kalman import UnscentedKalmanFilter as UKF dt = 0.1 sigmas = MerweScaledSigmaPoints(5, alpha=.1, beta=2., kappa=1.) from planebots.control import dwa import time # import vision.model as model def tick2thomega(ticks, dt): thomega = np.array( [0.5 * (ticks[0] + ticks[1]), (ticks[0] - ticks[1]) / 0.0408]) / 170 # wheelbase of 40.8mm return thomega class TestDwa(unittest.TestCase):
def fit(sysargv): """ Program to process Covid Data. :Example: For countries (European database) >> python ProcessSEIR1R2D.py >> python ProcessSEIR1R2D.py France 0 1 0 0 1 1 >> python ProcessSEIR1R2D.py France 2 3 8 0 1 1 # 3 périodes pour les femmes en France avec un décalage de 8 jours >> python ProcessSEIR1R2D.py France,Germany 1 1 0 0 1 1 # 1 période pour les hommes francais et les hommes allemands For French Region (French database) >> python ProcessSEIR1R2D.py FRANCE,D69 0 -1 13 0 1 1 # Code Insee Dpt 69 (Rhône) >> python ProcessSEIR1R2D.py FRANCE,R84 0 -1 13 0 1 1 # Tous les dpts de la Région dont le code Insee est >> python ProcessSEIR1R2D.py FRANCE,R32+ 0 -1 13 0 1 1 # Somme de tous les dpts de la Région 32 (Hauts-de-France) >> python ProcessSEIR1R2D.py FRANCE,MetropoleD 0 -1 13 0 1 1 # Tous les départements de la France métropolitaine >> python ProcessSEIR1R2D.py FRANCE,MetropoleD+ 0 -1 13 0 1 1 # Toute la France métropolitaine (en sommant les dpts) >> python ProcessSEIR1R2D.py FRANCE,MetropoleR+ 0 -1 13 0 1 1 # Somme des dpts de toutes les régions françaises Toute combinaison est possible de lieu : exemple FRANCE,R32+,D05,R84 argv[1] : List of countries (ex. France,Germany,Italy), or see above. Default: France argv[2] : Sex (male:1, female:2, male+female:0). Only for french database Default: 0 argv[3] : Periods ('1' -> 1 period ('all-in-on'), '!=1' -> severall periods). Default: -1 argv[4] : Delay (in days). Default: 13 argv[5] : UKF filtering of data (0/1). Default: 0 argv[6] : Verbose level (debug: 3, ..., almost mute: 0). Default: 1 argv[7] : Plot graphique (0/1). Default: 1 """ #Austria,Belgium,Croatia,Czechia,Finland,France,Germany,Greece,Hungary,Ireland,Italy,Lithuania,Poland,Portugal,Romania,Serbia,Spain,Switzerland,Ukraine #Austria,Belgium,Croatia,Czechia,Finland,France,Germany,Greece,Hungary,Ireland,Italy,Poland,Portugal,Romania,Serbia,Spain,Switzerland,Ukraine # Il y a 18 pays if len(sysargv) > 7: print(' CAUTION : bad number of arguments - see help') exit(1) # Constantes ######################################################@ fileLocalCopy = True # if we upload the file from the url (to get latest data) or from a local copy file readStartDateStr = "2020-03-01" # "2020-03-01" Le 8 mars, pour inclure un grand nombre de pays européens dont la date de premier était postérieur au 1er mars readStopDateStr = None recouvrement = -1 dt = 1 France = 'France' thresholdSignif = 1.5E-6 # Interpetation of arguments - reparation ######################################################@ # Default value for parameters listplaces = ['France'] sexe, sexestr = 0, 'male+female' nbperiodes = -1 decalage = 13 UKF_filt = False verbose = 1 plot = True # Parameters from argv if len(sysargv) > 0: liste = list(sysargv[0].split(',')) if len(sysargv) > 1: sexe = int(sysargv[1]) if len(sysargv) > 2: nbperiodes = int(sysargv[2]) if len(sysargv) > 3: decalage = int(sysargv[3]) if len(sysargv) > 4 and int(sysargv[4]) == 1: UKF_filt = True if len(sysargv) > 5: verbose = int(sysargv[5]) if len(sysargv) > 6 and int(sysargv[6]) == 0: plot = False if nbperiodes == 1: decalage = 0 # nécessairement pas de décalage (on compense le recouvrement) if sexe not in [0, 1, 2]: sexe, sexestr = 0, 'male+female' # sexe indiférencié if sexe == 1: sexestr = 'male' if sexe == 2: sexestr = 'female' listplaces = [] listnames = [] if liste[0] == 'FRANCE': FrDatabase = True liste = liste[1:] for el in liste: l, n = getPlace(el) if el == 'MetropoleR+': for l1, n1 in zip(l, n): listplaces.extend(l1) listnames.extend([n1]) else: listplaces.extend(l) listnames.extend(n) else: listplaces = liste[:] FrDatabase = False if verbose > 0: print(' Full command line : ' + sysargv[0] + ' ' + str(nbperiodes) + ' ' + str(decalage) + ' ' + str(UKF_filt) + ' ' + str(verbose) + ' ' + str(plot), flush=True) # Data reading to get first and last date available in the data set ######################################################@ if FrDatabase == True: pd_exerpt, HeadData, N, readStartDateStr, readStopDateStr, _ = readDataFrance( ['D69'], readStartDateStr, readStopDateStr, fileLocalCopy, sexe, verbose=0) else: pd_exerpt, HeadData, N, readStartDateStr, readStopDateStr, _ = readDataEurope( France, readStartDateStr, readStopDateStr, fileLocalCopy, verbose=0) dataLength = pd_exerpt.shape[0] readStartDate = datetime.strptime(readStartDateStr, strDate) if readStartDate < pd_exerpt.index[0]: readStartDate = pd_exerpt.index[0] readStartDateStr = pd_exerpt.index[0].strftime(strDate) readStopDate = datetime.strptime(readStopDateStr, strDate) if readStopDate < pd_exerpt.index[-1]: readStopDate = pd_exerpt.index[-1] readStopDateStr = pd_exerpt.index[-1].strftime(strDate) dataLength = pd_exerpt.shape[0] if verbose > 1: print('readStartDateStr=', readStartDateStr, ', readStopDateStr=', readStopDateStr) print('readStartDate =', readStartDate, ', readStopDate =', readStopDate) print('dataLength =', dataLength) #input('pause') # Collections of data return by this function modelSEIR1R2D = np.zeros(shape=(len(listplaces), dataLength, 6)) data_deriv = np.zeros(shape=(len(listplaces), dataLength, 2)) modelR1_deriv = np.zeros(shape=(len(listplaces), dataLength, 2)) data_all = np.zeros(shape=(len(listplaces), dataLength, 2)) modelR1_all = np.zeros(shape=(len(listplaces), dataLength, 2)) Listepd = [] ListetabParamModel = [] # data observed data = np.zeros(shape=(dataLength, 2)) # Paramètres sous forme de chaines de caractères ListeTextParam = [] ListeDateI0 = [] # Loop on the places to process for indexplace, place in enumerate(listplaces): # Get the full name of the place to process, and the special dates corresponding to the place if FrDatabase == True: placefull = 'France-' + listnames[indexplace][0] DatesString = readDates(France, verbose) else: placefull = place DatesString = readDates(place, verbose) print('PROCESSING of', placefull, 'in', listnames) # data reading of the observations ############################################################################# if FrDatabase == True: pd_exerpt, HeadData, N, readStartDateStr, readStopDateStr, dateFirstNonZeroStr = readDataFrance( place, readStartDateStr, readStopDateStr, fileLocalCopy, sexe, verbose=0) else: pd_exerpt, HeadData, N, readStartDateStr, readStopDateStr, dateFirstNonZeroStr = readDataEurope( place, readStartDateStr, readStopDateStr, fileLocalCopy, verbose=0) shift_0value = getNbDaysBetweenDateFromString(readStartDateStr, dateFirstNonZeroStr) # UKF Filtering ? if UKF_filt == True: data2Filt = pd_exerpt[[HeadData[0], HeadData[1]]].to_numpy(copy=True) sigmas = MerweScaledSigmaPoints(n=2, alpha=.5, beta=2., kappa=1.) #1-3.) ukf = UKF(dim_x=2, dim_z=2, fx=fR1D, hx=hR1D, dt=dt, points=sigmas) # Filter init ukf.x[:] = data2Filt[0, :] ukf.Q = np.diag([30., 15.]) ukf.R = np.diag([170., 100.]) if verbose > 1: print('ukf.x[:]=', ukf.x[:]) print('ukf.R =', ukf.R) print('ukf.Q =', ukf.Q) # UKF filtering and smoothing, batch mode R1Ffilt, _ = ukf.batch_filter(data2Filt) HeadData[0] += ' filt' HeadData[1] += ' filt' pd_exerpt[HeadData[0]] = R1Ffilt[:, 0] pd_exerpt[HeadData[1]] = R1Ffilt[:, 1] # Get the list of dates to process ListDates, ListDatesStr = GetPairListDates(readStartDate, readStopDate, DatesString, decalage, nbperiodes, recouvrement) if verbose > 1: #print('ListDates =', ListDates) print('ListDatesStr=', ListDatesStr) #input('pause') # Solveur edo solveur = SolveEDO_SEIR1R2D(N, dt, verbose) indexdata = solveur.indexdata E0, I0, R10, R20, D0 = 0, 1, 0, 0, 0 # Repertoire des figures if plot == True: repertoire = getRepertoire( UKF_filt, './figures/SEIR1R2D_UKFilt/' + placefull + '/sexe_' + str(sexe) + '_delay_' + str(decalage), './figures/SEIR1R2D/' + placefull + '/sexe_' + str(sexe) + '_delay_' + str(decalage)) prefFig = repertoire + '/Process_' # Remise à 0 des données data.fill(0.) # Boucle pour traiter successivement les différentes fenêtres ############################################################### ListeTextParamPlace = [] ListetabParamModelPlace = [] ListeEQM = [] DEGENERATE_CASE = False for i in range(len(ListDatesStr)): # dates of the current period fitStartDate, fitStopDate = ListDates[i] fitStartDateStr, fitStopDateStr = ListDatesStr[i] # Est-on dans un CAS degénéré? # print(getNbDaysBetweenDateFromString(dateFirstNonZeroStr, fitStopDateStr)) if getNbDaysBetweenDateFromString( dateFirstNonZeroStr, fitStopDateStr ) < 5: # Il faut au moins 5 données pour fitter DEGENERATE_CASE = True if i > 0: DatesString.addOtherDates(fitStartDateStr) # Récupérations des données observées dataLengthPeriod = 0 indMinPeriod = (fitStartDate - readStartDate).days for j, z in enumerate(pd_exerpt.loc[ fitStartDateStr:addDaystoStrDate(fitStopDateStr, -1), HeadData[0]]): data[indMinPeriod + j, 0] = z dataLengthPeriod += 1 for j, z in enumerate(pd_exerpt.loc[ fitStartDateStr:addDaystoStrDate(fitStopDateStr, -1), HeadData[1]]): data[indMinPeriod + j, 1] = z slicedata = slice(indMinPeriod, indMinPeriod + dataLengthPeriod) slicedataderiv = slice(slicedata.start + 1, slicedata.stop) if verbose > 0: print(' dataLength =', dataLength) print(' indMinPeriod =', indMinPeriod) print(' dataLengthPeriod=', dataLengthPeriod) print(' fitStartDateStr =', fitStartDateStr) print(' fitStopDateStr =', fitStopDateStr) #input('attente') # Set initialisation data for the solveur ############################################################################ # paramètres initiaux à optimiser if i == 0: datelegend = fitStartDateStr # ts=getNbDaysBetweenDateFromString(DatesString.listFirstCaseDates[0], readStartDateStr) # En premiere approximation, on prend la date du premier cas estimé pour le pays (même si c'est faux pour les régions et dpts) ts = getNbDaysBetweenDateFromString( DatesString.listFirstCaseDates[0], dateFirstNonZeroStr) if ts < 0: continue # On passe au pays suivant if nbperiodes != 1: # pour plusieurs périodes #l, b0, c0, f0 = 0.255, 1./5.2, 1./12, 0.08 #a0 = (l+c0)*(1.+l/b0) #a0, b0, c0, f0, mu0, xi0 = 0.55, 0.34, 0.12, 0.25, 0.0005, 0.0001 a0, b0, c0, f0, mu0, xi0 = 0.60, 0.55, 0.30, 0.50, 0.0005, 0.0001 T = 150 else: # pour une période #a0, b0, c0, f0, mu0, xi0 = 0.10, 0.29, 0.10, 0.0022, 0.00004, 0. a0, b0, c0, f0, mu0, xi0 = 0.70, 0.25, 0.05, 0.003, 0.0005, 0.0001 T = 350 if i == 1 or i == 2: datelegend = None _, a0, b0, c0, f0, mu0, xi0 = solveur.modele.getParam() R10 = int(data[indMinPeriod, 0]) # on corrige R1 à la valeur numérique F0 = int(data[indMinPeriod, 1]) # on corrige F à la valeur numérique if i == 1: a0 /= 4. # le confinement réduit drastiquement (pour aider l'optimisation) T = 120 ts = 0 time = np.linspace(0, T - 1, T) solveur.modele.setParam(N=N, a=a0, b=b0, c=c0, f=f0, mu=mu0, xi=xi0) solveur.setParamInit(N=N, E0=E0, I0=I0, R10=R10, R20=R20, D0=D0) # Before optimization ############################### # Solve ode avant optimization sol_ode = solveur.solveEDO(time) # calcul time shift initial (ts) with respect to data if i == 0: ts = solveur.compute_tsfromEQM(data[slicedata, :], T, indexdata) else: solveur.TS = ts = 0 sliceedo = slice(ts, min(ts + dataLengthPeriod, T)) if verbose > 0: print(solveur) print(' ts=' + str(ts)) # plot if plot == True and DEGENERATE_CASE == False: commontitre = placefull + '- Period ' + str(i) + '\\' + str( len(ListDatesStr) - 1 ) + ' - [' + fitStartDateStr + '\u2192' + addDaystoStrDate( fitStopDateStr, -1) if sewe == 0: titre = commontitre + '] (Delay (delta)=' + str( decalage) + ')' else: titre = commontitre + '] (Sex=', +sexestr + ', Delay (delta)=' + str( decalage) + ')' listePlot = indexdata filename = prefFig + str(decalage) + '_Period' + str( i) + '_' + ''.join(map(str, listePlot)) + 'Init.png' solveur.plotEDO(filename, titre, sliceedo, slicedata, plot=listePlot, data=data, text=solveur.getTextParam(datelegend, Period=i)) # Parameters optimization ############################################################################ solveur.paramOptimization( data[slicedata, :], time) # version lorsque ts est calculé automatiquement #solveur.paramOptimization(data[slicedata, :], time, ts) # version lorsque l'on veut fixer ts _, a1, b1, c1, f1, mu1, xi1 = solveur.modele.getParam() R0 = solveur.modele.getR0() if verbose > 0: print('Solver' 's state after optimization=', solveur) print(' Reproductivité après: ', R0) # After optimization ############################### # Solve ode avant optimization sol_ode = solveur.solveEDO(time) # calcul time shift with respect to data if i == 0: ts = solveur.compute_tsfromEQM(data[slicedata, :], T, indexdata) else: solveur.TS = ts = 0 sliceedo = slice(ts, min(ts + dataLengthPeriod, T)) sliceedoderiv = slice(sliceedo.start + 1, sliceedo.stop) if verbose > 0: print(solveur) print(' ts=' + str(ts)) if i == 0: # on se souvient de la date du premier infesté dateI0 = addDaystoStrDate(fitStartDateStr, -ts + shift_0value) if verbose > 2: print('dateI0=', dateI0) input('attente') # sauvegarde des param (tableau et texte) seuil = (data[slicedata.stop - 1, 0] - data[slicedata.start, 0] ) / getNbDaysBetweenDateFromString( fitStartDateStr, fitStopDateStr) / N #print('seuil=', seuil) #print('DEGENERATE_CASE=', DEGENERATE_CASE) if DEGENERATE_CASE == True: ROsignificatif = False ListetabParamModelPlace.append( [-1., -1., -1., -1., -1., -1., -1.]) else: if seuil < thresholdSignif: ROsignificatif = False ListetabParamModelPlace.append( [a1, b1, c1, f1, mu1, xi1, -1.]) else: ROsignificatif = True ListetabParamModelPlace.append( [a1, b1, c1, f1, mu1, xi1, R0]) # print('seuil=', seuil) # print('ROsignificatif=', ROsignificatif) # print('R0=', R0) # input('pause') ListeTextParamPlace.append( solveur.getTextParamWeak(datelegend, ROsignificatif, Period=i)) data_deriv_period = ( data[slicedataderiv, :] - data[slicedataderiv.start - 1:slicedataderiv.stop - 1, :]) / dt modelR1_deriv_period = ( sol_ode[sliceedoderiv, indexdata] - sol_ode[sliceedoderiv.start - 1:sliceedoderiv.stop - 1, indexdata]) / dt data_all_period = data[slicedataderiv, :] modelR1_all_period = sol_ode[sliceedoderiv, indexdata] if plot == True and DEGENERATE_CASE == False: commontitre = placefull + '- Period ' + str(i) + '\\' + str( len(ListDatesStr) - 1 ) + ' - [' + fitStartDateStr + '\u2192' + addDaystoStrDate( fitStopDateStr, -1) if sexe == 0: titre = commontitre + '] (Delay (delta)=' + str( decalage) + ')' else: titre = commontitre + '] (Sex=', +sexestr + ', Delay (delta)=' + str( decalage) + ')' # listePlot = [0,1,2,3,4,5] # filename = prefFig + str(decalage) + '_Period' + str(i) + '_' + ''.join(map(str, listePlot)) + '.png' # solveur.plotEDO(filename, titre, sliceedo, slicedata, plot=listePlot, data=data, text=solveur.getTextParam(datelegend, ROsignificatif, Period=i)) listePlot = [1, 2, 3, 5] filename = prefFig + str(decalage) + '_Period' + str( i) + '_' + ''.join(map(str, listePlot)) + 'Final.png' solveur.plotEDO(filename, titre, sliceedo, slicedata, plot=listePlot, data=data, text=solveur.getTextParam(datelegend, ROsignificatif, Period=i)) listePlot = indexdata filename = prefFig + str(decalage) + '_Period' + str( i) + '_' + ''.join(map(str, listePlot)) + 'Final.png' solveur.plotEDO(filename, titre, sliceedo, slicedata, plot=listePlot, data=data, text=solveur.getTextParam(datelegend, ROsignificatif, Period=i)) # dérivée numérique de R1 et F filename = prefFig + str(decalage) + '_Period' + str( i) + '_' + ''.join(map(str, listePlot)) + 'Deriv.png' solveur.plotEDO_deriv(filename, titre, sliceedoderiv, slicedataderiv, data_deriv_period, indexdata, text=solveur.getTextParam(datelegend, ROsignificatif, Period=i)) # sol_ode_withSwitch = solveur.solveEDO_withSwitch(T, timeswitch=ts+dataLengthPeriod) # ajout des données dérivées data_all[indexplace, slicedataderiv, :] = data_all_period modelR1_all[indexplace, slicedataderiv, :] = modelR1_all_period data_deriv[indexplace, slicedataderiv, :] = data_deriv_period modelR1_deriv[indexplace, slicedataderiv, :] = modelR1_deriv_period # ajout des SEIR1R2D modelSEIR1R2D[indexplace, slicedata.start:slicedata.stop, :] = sol_ode[ ts:ts + sliceedo.stop - sliceedo.start, :] # preparation for next iteration _, E0, I0, R10, R20, D0 = map( int, sol_ode[ts + dataLengthPeriod + recouvrement, :]) #print('A LA FIN : E0, I0, R10, R20, D0=', E0, I0, R10, R20, D0) if verbose > 1: input('next step') Listepd.append(pd_exerpt) ListeDateI0.append(dateI0) # calcul de l'EQM sur les données (et non sur les dérivées des données) #EQM = mean_squared_error(data_deriv[indexplace, :], modelR1_deriv[indexplace, :]) EQM = mean_squared_error(data_all[indexplace, :], modelR1_all[indexplace, :]) ListeEQM.append(EQM) # udpate des listes pour transmission ListeTextParam.append(ListeTextParamPlace) ListetabParamModel.append(ListetabParamModelPlace) return modelSEIR1R2D, ListeTextParam, Listepd, data_deriv, modelR1_deriv, ListetabParamModel, ListeEQM, ListeDateI0
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, 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([0, 0, 0]) ukf.P = np.diag([.1, .1, .05]) ukf.R = np.array([sigma_range**2, sigma_bearing**2]) ukf.Q = np.eye(3) * 0.0001 sim_pos = np.array([0, 0, 0.5]) # 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 = [] n = randint(4) for lmark in landmarks[0:n + 1]: 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]) print z ukf.update( z, hx_args=landmarks[0:n + 1], ) 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 run_sim(measurements, controlIn, truth): points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0, subtract=residual_x) dt = 0.1 #0.1s timestep ukf = UKF(dim_x=3, dim_z=2, fx=state_trans, hx=measurement_trans, 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([0., -0.3, 0.]) ukf.P = np.diag([.01, .01, .01]) #ukf.R = np.diag([0.1**2, 0.1**2, 0**2]) ukf.R = np.diag( [0.001**2, 0.001**2] ) #measurement noise = [linkeOdometrie, rechteOdometrie, distanzInCm, angleInRadians] ukf.Q = np.diag([ 0.00001**2, 0.00001**2, 0.001**2 ]) #process noise = [xRobo, yRobo, Orientierung, xHindernis, yHindernis] plt.plot(ukf.x[0], ukf.x[1], 'go', alpha=0.3) for x in range(np.size(measurements, 0)): u = controlIn[x] z = measurements[x][2:3] #z = measurements[x] ukf.predict(u=u) #R hoch stzen bei sensor output 1 #if(z[2] >= 1): # badR = np.diag([0.1**2, 0.1**2, 10000000**2]) # ukf.update(z, badR, u = u, dt = dt) #else: ukf.update(z, u=u, dt=dt) try: _ = np.linalg.cholesky(ukf.P) except np.linalg.LinAlgError: ukf.P = ps.nearestPD(ukf.P) angle = -np.pi / 2 rotation = np.array([[cos(angle), -sin(angle)], [sin(angle), cos(angle)]]) if x < 10000: #roboX = ukf.x[0] * rotation[0][0] + ukf.x[1] * rotation[0][1] #roboY = ukf.x[0] * rotation[1][0] + ukf.x[1] * rotation[1][1] #obsX = ukf.x[3] * rotation[0][0] + ukf.x[4] * rotation[0][1] #obsY = ukf.x[3] * rotation[1][0] + ukf.x[4] * rotation[1][1] #plt.plot(roboX, roboY, 'go', alpha=0.3) #plt.plot(obsX, obsY, 'bo', alpha=0.3) plt.plot(ukf.x[0], ukf.x[1], 'go', alpha=0.3) #plt.plot(ukf.x[3], ukf.x[4], 'bo', alpha=0.3) #print(ukf.x[2]) #echte position roboter plt.plot(truth[x][0], truth[x][1], 'ko', alpha=0.3) #plot_covariance((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=.1, facecolor='g', alpha=0.3) print(ukf.x) #echte position hindernis plt.plot(0, 0.25, 'ro', alpha=0.3) plt.show()
py = y + np.random.randn() * r if n < 1: x0 = x y0 = y zs.append([x, y]) else: zs.append([px, py]) if setv: setv = False vx0 = (x - x0) / dt vy0 = (y - y0) / dt print("x0: %6.2f y0: %6.2f vx0: %5.2f vy0: %5.2f" % (x0, y0, vx0, vy0)) # build a ca filter for comparation points_ca = MerweScaledSigmaPoints(n=6, alpha=.1, beta=2., kappa=0.1) ukf_ca = UnscentedKalmanFilter(dim_x=6, dim_z=2, fx=f_ca, hx=h_ca, dt=dt * 1.1, points=points_ca) # x vx ax y vy ay vstart_ca = [x0, vx0, 0., y0, vy0, -60.] ukf_ca.x = np.array(vstart_ca) ukf_ca.R = np.diag([r * r / 2., r * r / 2.]) ukf_ca.Q[0:3, 0:3] = Q_discrete_white_noise(3, dt=dt, var=0.001) ukf_ca.Q[3:6, 3:6] = Q_discrete_white_noise(3, dt=dt, var=0.001) ukf_ca.P *= 2.
flat_inertia0 = array([0, 0, 1, 0, 1]) state0 = hstack([eulers, angular_velocity0, flat_inertia0]) elif case == 3: DIM_X = 11 stf = state_transition_function_eigens_and_eulers flat_inertia0 = array([1, 1]) euler_offsets = array([0, 0, 0]) state0 = hstack( [eulers, angular_velocity0, flat_inertia0, euler_offsets]) elif case == 4: DIM_X = 6 stf = state_transition_function_const_omega state0 = hstack([eulers, angular_velocity0]) DIM_Z = 1 points = MerweScaledSigmaPoints(DIM_X, alpha=.3, beta=2, kappa=.1) lightcurve = load('true_lightcurve.npy') reverse_lightcurve = flip(lightcurve.copy()) time = load('time.npy') kf = UnscentedKalmanFilter(dim_x=DIM_X, dim_z=DIM_Z, dt=DT, fx=stf, hx=measurement_function, points=points) kf.x = state0 kf.P = 1 z_std = MEASUREMENT_VARIANCE R = z_std**2
def reset(self): """ Reset our SIRD model. """ # Reset β, γ and μ to the values mentioned on Wikipedia (see https://bit.ly/2VMvb6h). Model.__DATA = None 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 ]) self.__ukf.P *= 15 # 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_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.append(np.mean(landmarks, axis=0), (np.mean(cmds, axis=0)[1])) # set initial guess to mean 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) print('Sim position:', sim_pos) track.append(sim_pos) if i % step == 0: ukf.predict(u=u, wheelbase=wheelbase) if i % ellipse_step == 0: plot_covariance((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 = math.sqrt(dx**2 + dy**2) + np.random.randn() * sigma_range bearing = math.atan2(lmark[1] - y, lmark[0] - x) a = (normalize_angle(bearing - sim_pos[2] + np.random.randn() * sigma_bearing)) z.extend([d, a]) ukf.update(z, landmarks=landmarks) if i % ellipse_step == 0: plot_covariance((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.title("UKF Robot localization") plt.xlim(0, 4.2) plt.ylim(0, 6.05) plt.savefig('kalman_filter/%d.png' % len(os.listdir('kalman_filter/'))) plt.close() return ukf
def show_sigma_selections(): ax = plt.gca() ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) x = np.array([2, 5]) P = np.array([[3, 1.1], [1.1, 4]]) points = MerweScaledSigmaPoints(2, .09, 2., 1.) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') x = np.array([5, 5]) points = MerweScaledSigmaPoints(2, .15, 1., .15) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') x = np.array([8, 5]) points = MerweScaledSigmaPoints(2, .2, 3., 10) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') plt.axis('equal') plt.xlim(0, 10) plt.ylim(0, 10) plt.show()
return [ measurement.latitude, measurement.longitude, measurement.elevation, measurement.heading, measurement.velocity, measurement.acceleration, ] dim_x = 6 dim_z = 6 dt = 1 # points=[[0,0,1,1]] points = MerweScaledSigmaPoints(6, alpha=.1, beta=2., kappa=-1) kalaman_filter = UnscentedKalmanFilter(dim_x, dim_z, dt, hx, fx, points, x_mean_fn=state_mean, residual_x=residual) gpx = load_gpx_file('data/track_2018-03-14_134847.gpx') measurements = list(extract_gps_measurements(gpx)) kalaman_filter.x = numpy.array(format_measurement( measurements[0])) # initial state kalaman_filter.P *= 0.1 # initial uncertainty
def pendulum(self): messagebox.showinfo('Message title', 'End simulation by clicking Esc button') # visulaization parameters height = 600 width = 600 # simulation center = None # Pendulum parameters and variables # initial conditions theta = np.pi / 4 # the angle omega = 0 # the angular velocity # parametrs g = 9.8 # m/s^2 L = .2 # m m = 0.05 # kg b = 0.001 # friction constant # Numerical integration parameters framerate = 60.0 # in frames per second dt = 1.0 / framerate # Set dt to match the framerate of the webcam or animation t = time.clock() # Drawing parametres thickness = 3 # Noise parameters Sigma = 30 * np.array([[1, 0], [0, 1]]) #Kalman inferred state variables theta_kf = theta omega_kf = omega #theta_kf_old = theta_kf #Keep looping # Create background image frame = np.zeros((height, width, 3), np.uint8) center_old = (300, 300) center_noisy_old = (300, 300) center_kf_old = (300, 300) L_kf = 200 # Create background image frame = np.zeros((height, width, 3), np.uint8) cv2.circle(frame, (300, 300), 10, (0, 255, 255), -1) ################## # ukf functions # ################## #function to return the nonlinear state transition vatiables (theta, omega) def fx(X, dt): theta = X[0] omega = X[1] theta = theta + omega * dt omega = omega - dt * g / L_kf * np.sin(theta) - dt * b / ( m * L_kf * L_kf) * omega return np.c_[theta, omega] # The update step converts the sigmas into measurement space via the h(x) ,return theta and omega function[https://share.cocalc.com/share/7557a5ac1c870f1ec8f01271959b16b49df9d087/Kalman-and-Bayesian-Filters-in-Python/10-Unscented-Kalman-Filter.ipynb?viewer=share] def hx(X): return X points = MerweScaledSigmaPoints(2, alpha=1e-3, beta=2., kappa=4) #points = JulierSigmaPoints(n=2, kappa=1) kf = UKF(dim_x=2, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([theta_kf, omega_kf]) # initial state kf.R = Sigma # a measurement noise matrix kf.Q = np.diag( [4, 4]) # process noise the smae shape as the state variables 2X2 ###################### # end ukf functions # ###################### global readings_noisy, readings_after_ukf, theta_theoritical readings_noisy = [] readings_after_ukf = [] theta_theoritical = [] while True: cv2.circle(frame, (300, 300), 10, (0, 255, 255), -1) # == Simulation model == # Update state theta = theta + dt * omega theta_theoritical.append(theta) omega = omega - dt * g / L * np.sin(theta) - dt * b / (m * L * L) * omega # Map the state to a nearby pixel location center = np.array((int(300 + 200 * np.sin(theta)), int(300 + 200 * np.cos(theta)))) center_noisy = tuple( center + np.matmul(Sigma, np.random.randn(2)).astype(int)) # Draw the pendulum cv2.circle(frame, tuple(center_old), 10, (0, 0, 0), -1) cv2.circle(frame, center_noisy_old, 10, (0, 0, 0), -1) cv2.circle(frame, tuple(center), 10, (0, 255, 255), -1) cv2.circle(frame, center_noisy, 10, (0, 0, 255), -1) center_old = center center_noisy_old = center_noisy #################################################################### #### here starts the unscented kalman filter implementation # #################################################################### center_noisy_kf = center_noisy readings_noisy.append( np.arctan( (center_noisy_kf[0] - 300) / (center_noisy_kf[1] - 300))) print('theoritical theta and omega', theta, omega) #unscented kalman filter pridection kf.predict() #print('predicted theta and omega',kf.x[0],kf.x[1]) theta_kf = kf.x[0] omega_kf = kf.x[1] print('predicted theta and omega', theta_kf, omega_kf) #center noisy kf update # unscented kalman filter updating the state variables kf.update([(np.arctan( (center_noisy_kf[0] - 300) / (center_noisy_kf[1] - 300))), 0]) #print("after updtae",theta_kf) #maping the updated theta to the nearest pixels center_kf = np.array((int(300 + L_kf * np.sin(kf.x[0])), int(300 + L_kf * np.cos(kf.x[0])))) readings_after_ukf.append( np.arctan((center_kf[0] - 300) / (center_kf[1] - 300))) #################################################################### #### here finishes the unscented kalman filter implementation # #################################################################### # Map the state to a nearby pixel location cv2.circle(frame, tuple(center_kf_old), 10, (0, 0, 0), -1) center_kf_old = center_kf cv2.circle(frame, tuple(center_kf), 10, (255, 0, 255), -1) # show the frame to our screen cv2.imshow("Frame", frame) key = cv2.waitKey(int(dt * 400)) & 0xFF #if the 'Esc' key is pressed, stop the loop if key == 27: break # Wait with calculating next animation step to match the intended framerate t_ready = time.clock() d_t_animation = t + dt - t_ready t += dt if d_t_animation > 0: time.sleep(d_t_animation) # close all windows cv2.destroyAllWindows()
[0 , 1]] H (measurement function): [c, (1 - a - b)]^T Returns: UKF.UnscentedKalmanFilter: 2D UKF """ assert all(np.isin(["a", "b", "c"], [k for k in params.keys()])) a, b, c = params["a"], params["b"], params["c"] # generate sigma points - van der Merwe method # n = number of dimensions; alpha = how spread out; # beta = prior knowledge about distribution, 2 == gaussian; # kappa = scaling parameter, either 0 or 3-n; points = MerweScaledSigmaPoints(n=2, alpha=alpha, beta=beta, kappa=kappa) def fx2d(x, dt): a, b, c = params["a"], params["b"], params["c"] F = np.array([[1 - c, a], [0.0, 1.0]]) x = np.dot(F, x) return x def hx2d(prior_sigma, P_matrix=False): a, b, c = params["a"], params["b"], params["c"] H = np.array([[c, (1 - a - b)], [0, 1]]) if P_matrix: z_sigma = (H @ prior_sigma) @ np.transpose( H ) # np.dot(np.dot(H, prior_sigma), np.transpose(H)) else: # default
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 = .00000001 # 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 = UKF(dim_x=2, dim_z=1, dt=dt, hx=o_func, fx=t_func, points=points) ukf.x = np.array([0., 1.]) ukf.R = oc[:] ukf.Q = 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 = oc[:] kf.Q = tc[:] kf.H = H[:] kf.F = 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
def __init__(self, x, P, Q, R, mode="ore", enable_emp=True, enable_bias=True, output_debug=False): self.n_total = len(x) # total number of dimensions self.n_bias = len(np.diag(R)) # total number of biases to estimate self.n_sensor = len(np.diag(R)) # total number of sensor measurements if not enable_bias: self.n_bias = 0 self.emp = enable_emp self.bias = enable_bias self.output_debug = output_debug # Set up filterpy UKF self.ukf = UnscentedKalmanFilter( self.n_total, self.n_sensor, 120, lambda x: self.hx(x), lambda x, dt: self.fx(x, dt), MerweScaledSigmaPoints(self.n_total, 1e-3, 2, 0), scipy.linalg.cholesky, None, None, None, None) # set up debug publisher if self.output_debug: self.debug_pub = rospy.Publisher("state", FilterState, queue_size=10) # set initial filter states self.ukf.x = x self.ukf.P = P self.ukf.R = R self.ukf.Q = np.diag( np.concatenate([np.diag(Q), np.zeros(self.n_bias + 3)])) # set timestamp of initial filter state self.t_ukf = 0.0 self.integration_step = 100.0 self.mode = mode self.stm_matrix = RelativeOrbitalSTM() # time constant of empirical accelerations model self.tau_emp = 400 self.t_oe_c = 0.0 # variables to store noisy chaser state # and target states (target is only used for evaluation) self.O_c = rsl.KepOrbElem() self.osc_O_c = rsl.OscKepOrbElem() self.osc_O_t = rsl.OscKepOrbElem() # variables to store exact chaser state self.O_c_exact = rsl.KepOrbElem() self.osc_O_c_exact = rsl.OscKepOrbElem() self.R_J2K_CB = None # constant term for J2 self.J2_term = (3.0 / 4.0) * (rsl.Constants.J_2 * (rsl.Constants.R_earth**2) * np.sqrt(rsl.Constants.mu_earth)) self.update_lock = Lock() if self.output_debug: print "SETUP DONE"
mean = (0, 0) p = np.array([[32, 15], [15., 40.]]) # Compute linearized mean mean_fx = f_nonlinear_xy(*mean) #generate random points xs, ys = multivariate_normal(mean=mean, cov=p, size=10000).T #initial mean and covariance mean = (0, 0) p = np.array([[32., 15], [15., 40.]]) # create sigma points - we will learn about this later points = SigmaPoints(n=2, alpha=.1, beta=2., kappa=1.) Wm, Wc = points.weights() sigmas = points.sigma_points(mean, p) ### pass through nonlinear function sigmas_f = np.empty((5, 2)) for i in range(5): sigmas_f[i] = f_nonlinear_xy(sigmas[i, 0], sigmas[i ,1]) ### use unscented transform to get new mean and covariance ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc) #generate random points np.random.seed(100) xs, ys = multivariate_normal(mean=mean, cov=p, size=5000).T
from kf_book.ukf_internal import plot_radar, plot_altitude dt = 3. # 12 seconds between readings range_std = 5 # meters elevation_angle_std = math.radians(0.5) ac_pos = (0., 1000.) radar_pos = (0, 0) ac_vel = (100., 0.) h_radar.radar_pos = radar_pos np.random.seed(200) radar = RadarStation(radar_pos, range_std, elevation_angle_std) ac = ACSim(ac_pos, (100, 0), 0.02) time = np.arange(0, 360 + dt, dt) points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2., kappa=0.) P = np.zeros((4, 4)) # 4 element in state Q = np.zeros((4, 4)) # 4 element in state # shape: 4*4 Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1) Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=dt, var=0.1) P = np.diag([300**2, 3**2, 150**2, 3**2]) R_std = [range_std**2, elevation_angle_std**2] R = np.diag(R_std) # shape: 2*2 def myUKF(fx, hx, P, Q, R): points = MerweScaledSigmaPoints(n=4, alpha=.1, beta=2., kappa=-1.) kf = UKF(4, 2, dt, fx=fx, hx=hx, points=points) #(x_dimm, z_dimm,dt, hx, fx, sigmaPoints) kf.P = P
def __init__(self, modelType, deltaT, measurementNoiseStd): """ Initialises a tracker using initial bounding box. """ self.measurementNoiseStd = measurementNoiseStd self.modelType = modelType if (modelType == 0): #use contant linear velocity model # x = [x, y, ax, ay] self.updatedPredictions = [] self.kf = KalmanFilter(dim_x=4, dim_z=2) self.kf.F = np.array([[1, 0, deltaT, 0], [0, 1, 0, deltaT], [0, 0, 1, 0], [0, 0, 0, 1]]) self.kf.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) self.kf.x = np.array([0.01, 0.01, 0.01, 0.01]) self.kf.P *= measurementNoiseStd**2 self.kf.Q *= 0.005 self.kf.R *= measurementNoiseStd**2 elif (modelType == 1): """ Constant turn rate model """ points1 = MerweScaledSigmaPoints(5, alpha=0.001, beta=2., kappa=0) self.updatedPredictions = [] self.kf = UnscentedKalmanFilter(dim_x=5, dim_z=2, dt=deltaT, fx=f_unscented_turnRateModel, hx=h_unscented_turnRateModel, points=points1) self.kf.x = np.array([1e-3, 1e-3, 1e-3, 1e-5, 1e-10]) self.kf.P = np.eye(5) * (measurementNoiseStd**2) / 2.0 self.kf.R = np.eye(2) * (measurementNoiseStd**2) self.kf.Q = np.diag([1e-24, 1e-24, 1e-3, 4e-3, 1e-10]) elif (modelType == 2): """ Constant linear velocity model """ points1 = MerweScaledSigmaPoints(5, alpha=0.001, beta=2., kappa=0) self.updatedPredictions = [] self.kf = [] self.kf = UnscentedKalmanFilter(dim_x=5, dim_z=2, dt=deltaT, fx=f_unscented_linearModel, hx=h_unscented_linearModel, points=points1) self.kf.x = np.array([1e-3, 1e-3, 1e-3, 1e-5, 0]) self.kf.P = np.eye(5) * (measurementNoiseStd**2) / 2.0 self.kf.R = np.eye(2) * (measurementNoiseStd**2) self.kf.Q = np.diag([0.003, 0.003, 6e-4, 0.004, 0]) elif (modelType == 3): """ Random Motion Model """ points1 = MerweScaledSigmaPoints(5, alpha=0.001, beta=2., kappa=0) self.updatedPredictions = [] self.kf = [] self.kf = UnscentedKalmanFilter(dim_x=5, dim_z=2, dt=deltaT, fx=f_unscented_randomModel, hx=h_unscented_randomModel, points=points1) self.kf.x = np.array([1e-3, 1e-3, 1e-3, 1e-5, 0]) self.kf.P = np.eye(5) * (measurementNoiseStd**2) / 2.0 self.kf.R = np.eye(2) * (measurementNoiseStd**2) self.kf.Q = np.diag([1, 1, 1e-24, 1e-24, 1e-24])
plt.scatter(fxs, fys, marker='.', alpha=0.01, color='k') plt.scatter(mean_fx[0], mean_fx[1], marker='o', s=100, c='r', label='UT_mean') plt.scatter(computed_mean_x, computed_mean_y, marker='*',s=120, c='b', label='mean') plt.ylim([-10, 200]) plt.xlim([-100, 100]) plt.legend(loc='best', scatterpoints=1) print ('Difference in mean x={:.3f}, y={:.3f}'.format( computed_mean_x-mean_fx[0], computed_mean_y-mean_fx[1])) # ------------------------------------------------------------------------------------------- mean = [0, 0] # Mean of the N-dimensional distribution. cov = [[32, 15], [15, 40]] # Covariance matrix of the distribution. # create sigma points(2n+1个sigma点) # uses 3 parameters to control how the sigma points are distributed and weighted points = SigmaPoints(n=2, alpha=.1, beta=2., kappa=1.) # Wm, Wc = points.weights() Wm, Wc = points.Wm, points.Wc sigmas = points.sigma_points(mean, cov) # pass through nonlinear function sigmas_f = np.empty((5, 2)) for i in range(5): sigmas_f[i] = f_nonlinear_xy(sigmas[i, 0], sigmas[i ,1]) # use unscented transform to get new mean and covariance ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc) # generate random points xs, ys = multivariate_normal(mean, cov, size=1000).T # 从二维随机变量的正态分布中产生1000个数据点 plot2(xs, ys, f_nonlinear_xy, ukf_mean)
def init_filter( S0: float = 5.74, s_variance: float = 1, Q00_s_noise: float = 0.01, R: float = 1, h: Callable = hx, f: Callable = abc, params: Dict = parameters, alpha: float = 1e-3, beta: float = 2, kappa: float = 1, ) -> UKF.UnscentedKalmanFilter: """Init the ABC model kalman filter X (state mean): [S0]^T P (state uncertainty): [s_variance] ABC Model F (process transition matrix): H (measurement function): Args: S0 (float, optional): [description]. Defaults to 5.74. s_variance (float, optional): P_t=0. Defaults to 1. Q00_s_noise (float, optional): [description]. Defaults to 0.01. R (float, optional): [description]. Defaults to 1. h (Callable, optional): [description]. Defaults to hx. f (Callable, optional): [description]. Defaults to abc. params (Dict, optional): [description]. Defaults to parameters. alpha (float, optional): [description]. Defaults to 1e-3. beta (float, optional): [description]. Defaults to 2. kappa (float, optional): [description]. Defaults to 1. Returns: [UKF.UnscentedKalmanFilter]: Initialised Unscented Kalman Filter """ assert all(np.isin(["a", "b", "c"], [k for k in params.keys()])) a, b, c = params["a"], params["b"], params["c"] # generate sigma points - van der Merwe method # n = number of dimensions; alpha = how spread out; # beta = prior knowledge about distribution, 2 == gaussian; # kappa = scaling parameter, either 0 or 3-n; points = MerweScaledSigmaPoints(n=1, alpha=alpha, beta=beta, kappa=kappa) ## TODO: dt = 86400s (1day) (??) abc_filter = UKF.UnscentedKalmanFilter( dim_x=1, dim_z=1, dt=1, hx=h, fx=f, points=points ) # INIT FILTER # ------- Predict Variables ------- # State Vector (X): storage and rainfall # (2, 1) = column vector abc_filter.x = np.array([S0]) # State Covariance (P) initial estimate # (2, 2) = square matrix abc_filter.P[:] = np.diag([s_variance]) # Process noise (Q) # (2, 2) = square matrix abc_filter.Q = np.diag([Q00_s_noise]) # ------- Update Variables ------- # measurement uncertainty # (2, 2) = square matrix OR is it just uncertainty on discharge (q) abc_filter.R *= R # Control inputs (defaults) abc_filter.B = None # np.ndarray([a]) abc_filter.dim_u = 0 return abc_filter
m = array([[5., 10], [10, 5], [15, 15], [20., 16], [0, 30], [50, 30], [40, 10]]) #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])
def hx(x): # measurement function - convert state into a measurement # where measurements are [x_pos, y_pos] # x = [pos_x, pos_y, vel_x, vel_y] # x[0] = pos_x # x[1] = pos_y # x[2] = vel_x # x[3] = vel_y return np.array([x[0], x[1]]) # Observation x,y position covariance dt = 0.1 # create sigma points to use in the filter. This is standard for Gaussian processes points = MerweScaledSigmaPoints(4, alpha=.001, beta=2., kappa=0) kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([-1., 1., -1., 1]) # initial state kf.P *= 0.2 # 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) gt = [[i, i] for ia in range(50)] # ground truth zs = [[i+np.random.randn()*2.0, i+np.random.randn()*0.2] for i in range(50)] # measurements for g, z in zip(gt, zs): kf.predict() kf.update(z) print(g, z, kf.x, 'log-likelihood', kf.log_likelihood)
def register_and_filter_once(x, filt_times): start_frame = 48 end_frame = 12 skip = 1 filepath = "./20210312_3axis/" pose_data = np.genfromtxt(f'{filepath}point_cloud_pose.txt', delimiter=',') timestamp_data = np.genfromtxt(f'{filepath}timestamp.txt', delimiter=',') pose_data = pose_data[start_frame:-end_frame:skip, :] measurement_data = np.genfromtxt(f'{filepath}registration_measurement.csv', delimiter=',') measurement_data = measurement_data[::skip, :] pose_data[:, 6:10] = measurement_data[::skip, 3:7] timestamp_data = timestamp_data[start_frame:-end_frame:skip, :] pose_data = continous_quat(pose_data) # filepath = "./test_pointcloud/" # pose_data = np.genfromtxt(f'{filepath}blensor_data_csv.txt', delimiter=',') # pose_data = np.concatenate((np.zeros((pose_data.shape[0],6)), pose_data), axis=1) # arange_data = np.arange(pose_data.shape[0]).reshape((pose_data.shape[0],1)) # timestamp_data = np.concatenate((arange_data, arange_data*0.05), axis=1) # pose_data = pose_data[::skip,:] # timestamp_data = timestamp_data[::skip,:] quat0_inv = quat_inv(pose_data[0, 6:10]) for i in range(pose_data.shape[0]): pose_data[i, 6:10] = quaternion_mul_num(pose_data[i, 6:10], quat0_inv) # In reverse Order if filt_times % 2 == 0: pose_data = pose_data[::-1, :] timestamp_data = timestamp_data[::-1, :] measurement_data[:, 0:3] = -measurement_data[::-1, 0:3] source_quat_zero_raw = pose_data[0, 6:10] R = 1e-6 * np.diag(np.asarray([1, 1, 1])) P = 1e-4 * np.diag( np.asarray([0.001, 0.001, 0.001, 1, 1, 1, 10, 10, 1, 1, 1])) Q = 1e-8 * np.diag( np.asarray([0.001, 0.001, 0.001, 1, 1, 1, 10, 10, 1, 1, 1])) # shrink_list = [3, 5, 7] # shrink_index = bisect(shrink_list, filt_times) # P[7:9,:] = P[7:9,:]/(10**shrink_index) # Q[7:9,:] = Q[7:9,:]/(10**shrink_index) # P[0:6]*=skip**2 # Q[0:6]*=skip**2 P[0:3] *= 1000 Q[0:3] *= 1000 x_log = np.zeros((pose_data.shape[0], 19)) x_log[0, :] = x #UKF setting points = MerweScaledSigmaPoints(11, alpha=.1, beta=2., kappa=-1) ukf = UKF(dim_x=11, dim_z=3, dt=0.1, fx=ukf_f, hx=ukf_h, points=points) ukf.x = x[0:11] # initial state ukf.P = P ukf.Q = Q ukf.R = R x_global = x[11:19] # w_body = np.zeros((pose_data.shape[0]-1, 3)) # for i in range(w_body.shape[0]): # w_body[i, :] = get_w_in_body_frame(pose_data[i,6:10], pose_data[i+1,6:10], timestamp_data[i+1, 1] - timestamp_data[i, 1]) source_s_root = 1 now_quat = source_quat_zero_raw register_log = np.zeros((pose_data.shape[0], 4)) dq_real_log = np.zeros((pose_data.shape[0] - 1, 4)) dq_correct_log = np.zeros((pose_data.shape[0] - 1, 4)) register_log[0, :] = now_quat loop_start_R = Rotation.from_quat(to_scalar_last(now_quat)) loop_angle_log = np.zeros((pose_data.shape[0], )) for i in range(pose_data.shape[0] - 1): source_quat_raw = pose_data[i, 6:10] target_quat_raw = pose_data[i + 1, 6:10] source_quat = np.zeros((4, )) target_quat = np.zeros((4, )) source_quat[3] = source_quat_raw[0] source_quat[0:3] = source_quat_raw[1:4] target_quat[3] = target_quat_raw[0] target_quat[0:3] = target_quat_raw[1:4] sourceR = Rotation.from_quat(source_quat) targetR = Rotation.from_quat(target_quat) nowR_from_loop_start = targetR * loop_start_R.inv() nowR_from_loop_start_angle = np.linalg.norm( nowR_from_loop_start.as_rotvec()) loop_angle_log[i] = nowR_from_loop_start_angle dR_real = targetR * sourceR.inv() dq_real = to_scalar_first(dR_real.as_quat()) dq_real_log[i, :] = dq_real dtime = abs(timestamp_data[i + 1, 1] - timestamp_data[i, 1]) # x_predict, P = ekf_predict(x, P, Q, dtime) x_predict, P = mekf_predict(x, P, Q, dtime) # ukf.predict(dt=dtime) # point to plane ICP dq_estimate = dR_real.as_quat() if dq_estimate[3] > 0.5: dq_estimate = dq_estimate else: dq_estimate = -dq_estimate z_measure = np.zeros((7, )) z_measure[0:3] = dq_estimate[0:3] # z_measure[3:7] = quaternion_mul_num(to_scalar_first(dq_estimate), now_quat) z_measure[3:7] = target_quat_raw # x_correct, P, z_correct = ekf_update(x, x_predict, P, z_measure, R, dtime) x_correct, P, z_correct = mekf_update(x_predict, P, measurement_data[i + 1, 0:3], R, dtime) # ukf, x_global, _ = ukf_update(ukf, z_measure[3:7], x_global) # dq_correct = np.asarray([z_correct[0],z_correct[1],z_correct[2],1]) # dR_correct = Rotation.from_quat(dq_correct/np.linalg.norm(dq_correct)) x_correct = mekf_normalize_state(x_correct) x_log[i + 1, :] = x_correct x = x_correct # x_global[0:4] = x_global[0:4]/np.linalg.norm(x_global[0:4]) # x_global[4:8] = x_global[4:8]/np.linalg.norm(x_global[4:8]) # x_log[i+1,0:11] = ukf.x # x_log[i+1,11:19] = x_global # now_quat = quaternion_mul_num(to_scalar_first(dq_correct), now_quat) # now_quat = z_correct[3:7] # register_log[i+1, :] = now_quat # dq_correct_log[i,:] = to_scalar_first(dq_correct) np.savetxt(f"{filepath}mekf_log_ground_truth_{filt_times}.csv", x_log, delimiter=',') # x[0:11] = ukf.x # x[11:19] = x_global return x
def ukf_filter_without_register(x, filt_times, filepath, measurement_raw, timestamp_data): measurement = copy.deepcopy(measurement_raw) # In reverse Order if filt_times % 2 == 0: measurement = measurement[::-1, :] timestamp_data = timestamp_data[::-1, :] for i in range(1, measurement.shape[0]): measurement[measurement.shape[0] - i, 0:3] = -measurement[measurement.shape[0] - i - 1, 0:3] # measurement[i, 0:3] = -measurement[i, 0:3] dt = 0.1 points = MerweScaledSigmaPoints(11, alpha=.1, beta=2., kappa=-1) ukf = UKF(dim_x=11, dim_z=3, dt=dt, fx=ukf_f, hx=ukf_h, points=points) ukf.x = x[0:11] # initial state x_global = x[11:19] ukf.P *= 0.2 # initial uncertainty ukf.R = 1e-5 * np.diag(np.asarray([1, 1, 1])) if filt_times == 1: ukf.P = 1e-4 * np.diag( np.asarray([0.0001, 0.0001, 1, 1, 1, 1, 10, 10, 1, 1, 1])) ukf.Q = 1e-8 * np.diag( np.asarray([0.0001, 0.0001, 1, 1, 1, 1, 10, 10, 1, 1, 1])) else: ukf.P = 1e-4 * np.diag( np.asarray([0.001, 0.001, 0.001, 1, 1, 1, 10, 10, 1, 1, 1])) ukf.Q = 1e-8 * np.diag( np.asarray([0.001, 0.001, 0.001, 1, 1, 1, 10, 10, 1, 1, 1])) # shrink_list = [3, 5, 7] # shrink_index = bisect(shrink_list, filt_times) # ukf.P[6:8,:] /= (10**shrink_index) # ukf.Q[6:8,:] /= (10**shrink_index) # P[8:11,:] = P[8:11,:]/(10**shrink_index) # Q[8:11,:] = Q[8:11,:]/(10**shrink_index) # P[3:7] = P[3:7]/(10**shrink_index) # Q[3:7] = Q[3:7]/(10**shrink_index) x_log = np.zeros((measurement.shape[0], 19)) x_log[0, 0:11] = ukf.x x_log[0, 11:19] = x_global for i in range(measurement.shape[0] - 1): dtime = abs(timestamp_data[i + 1, 1] - timestamp_data[i, 1]) ukf.predict(dt=dtime) # z_measure[3:7] = target_quat_raw z_measure = measurement[i + 1, 3:7] ukf, x_global, _ = ukf_update(ukf, z_measure, x_global) x_global[0:4] = x_global[0:4] / np.linalg.norm(x_global[0:4]) x_global[4:8] = x_global[4:8] / np.linalg.norm(x_global[4:8]) x_log[i + 1, 0:11] = ukf.x x_log[i + 1, 11:19] = x_global np.savetxt(f"{filepath}ukf_log_all_{filt_times}.csv", x_log, delimiter=',') x[0:11] = ukf.x x[11:19] = x_global return x
R = np.divide(temp, dim_x) def transition(state, time_step=time_step): output = np.zeros_like(state) cursor = state[:6] output[:6] = F @ cursor output[6:] = state[6:] return output def observation(state): return H @ state[:6] points = MerweScaledSigmaPoints(np.size(states, 0), alpha=1., beta=2., kappa=0) myukf = ukf(dim_x=dim_x + dim_z, dim_z=dim_z, dt=time_step, fx=transition, hx=observation, points=points) initial_state = states[:, 0] myukf.x = initial_state myukf.R = R myukf.Q = np.eye(dim_x + dim_z) myukf.Q[:np.size(Q, 0), :np.size(Q, 1)] = Q # Estimation loop rng = np.size(states, 1) - training_size ukf_predict = np.zeros((dim_x + dim_z, rng))