def test_sigma_points_1D(): """ tests passing 1D data into sigma_points""" kappa = 0. sp = JulierSigmaPoints(1, kappa) #ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa) Wm, Wc = sp.weights() assert np.allclose(Wm, Wc, 1e-12) assert len(Wm) == 3 mean = 5 cov = 9 Xi = sp.sigma_points (mean, cov) xm, ucov = unscented_transform(Xi,Wm, Wc, 0) # sum of weights*sigma points should be the original mean m = 0.0 for x, w in zip(Xi, Wm): 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)
def test_sigma_points_1D(): """ tests passing 1D data into sigma_points""" kappa = 0. sp = JulierSigmaPoints(1, kappa) #ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa) Wm, Wc = sp.weights() assert np.allclose(Wm, Wc, 1e-12) assert len(Wm) == 3 mean = 5 cov = 9 Xi = sp.sigma_points(mean, cov) xm, ucov = unscented_transform(Xi, Wm, Wc, 0) # sum of weights*sigma points should be the original mean m = 0.0 for x, w in zip(Xi, Wm): 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)
def test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together sp0 = JulierSigmaPoints(n=2, kappa=kappa) sp1 = JulierSigmaPoints(n=2, kappa=kappa * 1000) w0, _ = sp0.weights() w1, _ = sp1.weights() Xi0 = sp0.sigma_points(x, P) Xi1 = sp1.sigma_points(x, P) assert max(Xi1[:, 0]) > max(Xi0[:, 0]) assert max(Xi1[:, 1]) > max(Xi0[:, 1]) if DO_PLOT: plt.figure() for i in range(Xi0.shape[0]): plt.scatter((Xi0[i, 0] - x[0, 0]) * w0[i] + x[0, 0], (Xi0[i, 1] - x[0, 1]) * w0[i] + x[0, 1], color='blue') for i in range(Xi1.shape[0]): plt.scatter((Xi1[i, 0] - x[0, 0]) * w1[i] + x[0, 0], (Xi1[i, 1] - x[0, 1]) * w1[i] + x[0, 1], color='green') stats.plot_covariance_ellipse([1, 2], P)
def test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together sp0 = JulierSigmaPoints(n=2, kappa=kappa) sp1 = JulierSigmaPoints(n=2, kappa=kappa * 1000) sp2 = MerweScaledSigmaPoints(n=2, kappa=0, beta=2, alpha=1e-3) sp3 = SimplexSigmaPoints(n=2) # test __repr__ doesn't crash str(sp0) str(sp1) str(sp2) str(sp3) w0, _ = sp0.weights() w1, _ = sp1.weights() w2, _ = sp2.weights() w3, _ = sp3.weights() Xi0 = sp0.sigma_points(x, P) Xi1 = sp1.sigma_points(x, P) Xi2 = sp2.sigma_points(x, P) Xi3 = sp3.sigma_points(x, P) assert max(Xi1[:, 0]) > max(Xi0[:, 0]) assert max(Xi1[:, 1]) > max(Xi0[:, 1]) if DO_PLOT: plt.figure() for i in range(Xi0.shape[0]): plt.scatter((Xi0[i, 0] - x[0, 0]) * w0[i] + x[0, 0], (Xi0[i, 1] - x[0, 1]) * w0[i] + x[0, 1], color='blue', label='Julier low $\kappa$') for i in range(Xi1.shape[0]): plt.scatter((Xi1[i, 0] - x[0, 0]) * w1[i] + x[0, 0], (Xi1[i, 1] - x[0, 1]) * w1[i] + x[0, 1], color='green', label='Julier high $\kappa$') # for i in range(Xi2.shape[0]): # plt.scatter((Xi2[i, 0] - x[0, 0]) * w2[i] + x[0, 0], # (Xi2[i, 1] - x[0, 1]) * w2[i] + x[0, 1], # color='red') for i in range(Xi3.shape[0]): plt.scatter((Xi3[i, 0] - x[0, 0]) * w3[i] + x[0, 0], (Xi3[i, 1] - x[0, 1]) * w3[i] + x[0, 1], color='black', label='Simplex') stats.plot_covariance_ellipse([1, 2], P)
def build_ukf(x0=None, P0=None, Q = None, R = None ): # build ukf if x0 is None: x0 = np.zeros(6) if P0 is None: P0 = np.diag([1e-6,1e-6,1e-6, 1e-1, 1e-1, 1e-1]) if Q is None: Q = np.diag([1e-4, 1e-4, 1e-2, 1e-1, 1e-1, 1e-1]) #xyhvw if R is None: R = np.diag([1e-1, 1e-1, 1e-1]) # xyh #spts = MerweScaledSigmaPoints(6, 1e-3, 2, 3-6, subtract=ukf_residual) spts = JulierSigmaPoints(6, 6-2, sqrt_method=np.linalg.cholesky, subtract=ukf_residual) ukf = UKF(6, 3, (1.0 / 30.), # dt guess ukf_hx, ukf_fx, spts, x_mean_fn=ukf_mean, z_mean_fn=ukf_mean, residual_x=ukf_residual, residual_z=ukf_residual) ukf.x = x0.copy() ukf.P = P0.copy() ukf.Q = Q ukf.R = R return ukf
def __init__(self, dt, state=np.array([0.0, 0.0, -2.0, 0.0, 0.0, 0.0])): M = len(state) points = JulierSigmaPoints(M) def project(state, world_positions): return project_plane_markers(world_positions, state.reshape(2, 3)).reshape(-1) self.kf = kf = UnscentedKalmanFilter( dt=dt, dim_x=M, dim_z=1, points=points, #fx=lambda X, dt: predict_state(X.reshape(4, -1), dt).reshape(-1), fx=lambda x, dt: x, hx=project, ) z_dim = 2 # This changes according to the measurement kf.P = np.eye(M) * 0.3 kf.x = state.reshape(-1).copy() # Initial state guess self.z_var = 0.05**2 #kf.R = z_var # Observation variance dpos_var = 0.01**2 * dt drot_var = np.radians(1.0)**2 * dt #kf.Q = np.diag([0]*3 + [0]*3 + [dpos_var]*3 + [drot_var]*3) kf.Q = np.diag([dpos_var] * 3 + [drot_var] * 3)
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 = 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) 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 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 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 = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp) assert np.allclose(kf.x, kf.x_prior) assert np.allclose(kf.P, kf.P_prior) # test __repr__ doesn't crash str(kf) 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) # test mahalanobis a = np.zeros(kf.y.shape) maha = scipy_mahalanobis(a, kf.y, kf.SI) assert kf.mahalanobis == approx(maha) 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 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.) # sp = SimplexSigmaPoints(n=3) kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp) assert np.allclose(kf.x, kf.x_prior) assert np.allclose(kf.P, kf.P_prior) # test __repr__ doesn't crash str(kf) 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, env, dim_x, dim_y, X_0, P, Q, R, mn): # self.sigmas = JulierSigmaPoints(dim_x, alpha=.1, beta=2., kappa=1.) self.sigmas = JulierSigmaPoints(dim_x, kappa=0.1) self.env = env self.measure_nums = mn self.ukf = UnscentedKalmanFilter(dim_x=dim_x, dim_z=dim_y, dt=env.tau, hx=self.measurement, fx=UKF_model, points=self.sigmas) self.ukf.x = X_0[:, 0] # print(self.ukf.x.shape) self.ukf.P = np.copy(P) self.ukf.R = np.copy(R) self.ukf.Q = np.copy(Q)
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 test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together sp0 = JulierSigmaPoints(n=2, kappa=kappa) sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000) w0, _ = sp0.weights() w1, _ = sp1.weights() Xi0 = sp0.sigma_points (x, P) Xi1 = sp1.sigma_points (x, P) assert max(Xi1[:,0]) > max(Xi0[:,0]) assert max(Xi1[:,1]) > max(Xi0[:,1])
def test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together sp0 = JulierSigmaPoints(n=2, kappa=kappa) sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000) w0, _ = sp0.weights() w1, _ = sp1.weights() Xi0 = sp0.sigma_points (x, P) Xi1 = sp1.sigma_points (x, P) assert max(Xi1[:,0]) > max(Xi0[:,0]) assert max(Xi1[:,1]) > max(Xi0[:,1]) if DO_PLOT: plt.figure() for i in range(Xi0.shape[0]): plt.scatter((Xi0[i,0]-x[0, 0])*w0[i] + x[0, 0], (Xi0[i,1]-x[0, 1])*w0[i] + x[0, 1], color='blue') for i in range(Xi1.shape[0]): plt.scatter((Xi1[i, 0]-x[0, 0]) * w1[i] + x[0,0], (Xi1[i, 1]-x[0, 1]) * w1[i] + x[0,1], color='green') stats.plot_covariance_ellipse([1, 2], P)
def track_head(camera_matrix, markerss, world_markers): M = camera_matrix f_x, c_x = M[0, 0], M[0, -1] f_y, c_y = M[1, 1], M[1, -1] h = 1 w = 16 / 9 dt = 1 / 30 # TODO: Use the actual timestamps pos = np.array([0.0, 0.0, -2.0]) rot = np.array([0.0, 0.0, 0.0]) dpos = np.array([0.0, 0.0, 0.0]) drot = np.array([0.0, 0.0, 0.0]) state = np.array([ pos, rot, #dpos, drot ]) state_shape = state.shape M = np.prod(state_shape) points = JulierSigmaPoints(M) def project(state, world_positions): return project_plane_markers(world_positions, state.reshape(state_shape)).reshape(-1) kf = UnscentedKalmanFilter( dt=dt, dim_x=M, dim_z=len(world_markers) * 2, points=points, #fx=lambda X, dt: predict_state(X.reshape(4, -1), dt).reshape(-1), fx=lambda x, dt: x, hx=project, ) z_dim = 2 # This changes according to the measurement kf.P = np.eye(M) * 0.3 kf.x = state.reshape(-1).copy() # Initial state guess z_var = 0.05**2 kf.R = z_var # Observation variance dpos_var = 0.01**2 * dt drot_var = np.radians(1.0)**2 * dt #kf.Q = np.diag([0]*3 + [0]*3 + [dpos_var]*3 + [drot_var]*3) kf.Q = np.diag([dpos_var] * 3 + [drot_var] * 3) all_world_positions = [] for w in world_markers.values(): for v in w: all_world_positions.append(v) all_world_positions = np.array(all_world_positions) for row in markerss: markers = row['markers'] world_positions = [] screen_positions = [] for m in markers: try: world = world_markers[str(m['id'])] except KeyError: continue if m['id_confidence'] < 0.7: continue for w, s in zip(world, m['verts']): world_positions.append(w) screen_positions.append(s) world_positions = np.array(world_positions).reshape(-1, 2) screen_positions = np.array(screen_positions).reshape(-1, 2) screen_positions[:, 0] -= c_x screen_positions[:, 0] /= f_x screen_positions[:, 1] -= c_y screen_positions[:, 1] /= -f_y kf.predict() if len(world_positions) >= 0: measurement = screen_positions.reshape(-1) kf.update(measurement, R=np.diag([z_var] * len(measurement)), world_positions=world_positions) yield kf.x.copy(), kf.P.copy()
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 = UKF(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) 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')
####Net.Control_Input(Directory+'5_pipes_driving_transient.csv') ####Net.MOC_Run(maxTime) ##pp.show() from filterpy.kalman import unscented_transform, MerweScaledSigmaPoints, JulierSigmaPoints, JulierSigmaPoints import scipy.stats as stats #InitP = np.load(Directory+'UKFInitP.npy') #P[:2*Net.CPs,:2*Net.CPs] = InitP[:2*Net.CPs,:2*Net.CPs] ukf_mean = State ukf_cov = nearestPD(P) Iterations = 3000 points = MerweScaledSigmaPoints(n=State.size, alpha=1e-3, beta=2., kappa=3-State.size) points = JulierSigmaPoints(n=State.size,kappa = 0) sigmas = points.sigma_points(ukf_mean,ukf_cov) sigmas_f = np.empty((Iterations,sigmas.shape[0],sigmas.shape[1])) sigmas_f[0,:,:] = sigmas for i in range(sigmas.shape[0]): print i #print s ret,reservoir = epa.ENgetnodeindex(str(Net.nodes[-1].Name)) ret,demand = epa.ENgetnodeindex(str(Net.nodes[1].Name)) Net.nodes[1].demand = sigmas_f[0,i,-1] ret,downstream = epa.ENgetnodeindex(str(Net.nodes[-2].Name)) Net.nodes[-2].demand = sigmas_f[0,i,-2] #demand_generator(Directory+'5_pipes_driving_transient.csv',6,maxTime,dt,sigmas_f[0,i,-2]*1000.,sigmas_f[0,i,-2]*1000.+0.5,2,30) #Net.Control_Input(Directory+'5_pipes_driving_transient.csv')
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_distance_polar): r_pred, alpha_pred = measure_func(y[:2], agent_state[:2], agent_state[2]) 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 Attitude_Filter(lightcurve, obsvecs, sunvecs, time, noise, Geometry, Inertia_Matrix, exposure_time, est_inertia=False): dts = diff(time) ave_dt = average(dts) if est_inertia: DIM_X = 8 DIM_Z = 1 points = JulierSigmaPoints(DIM_X) initial_rate = array([.01, .01, .01]) initial_mrps = array([.01, .01, .01]) initial_inertia = array([1.0, 1.0]) initial_state = hstack([initial_mrps, initial_rate, initial_inertia]) kf = UnscentedKalmanFilter(dim_x=DIM_X, dim_z=DIM_Z, dt=ave_dt, fx=modified_rodrigues_prop, hx=mrp_measurement_function, points=points) kf.x = initial_state kf.P = diag(ones(DIM_X)) * 100 kf.R = noise**2 kf.Q = diag( [1, 1, 1, 1e3, 1e3, 1e3, 1e3, 1e3] ) * 1e-12 #I suspect that the last two values here are not tuned super great. Those in particular correspond to the model uncertainty of the inertia values else: DIM_X = 6 DIM_Z = 1 points = JulierSigmaPoints(DIM_X) initial_rate = array([.01, .01, .01]) initial_mrps = array([.01, .01, .01]) initial_state = hstack([initial_mrps, initial_rate]) kf = UnscentedKalmanFilter(dim_x=DIM_X, dim_z=DIM_Z, dt=ave_dt, fx=modified_rodrigues_prop, hx=mrp_measurement_function, points=points) kf.x = initial_state kf.P = diag(ones(DIM_X)) * 100 kf.R = noise**2 kf.Q = diag([1, 1, 1, 1e3, 1e3, 1e3]) * 1e-12 #print(kf.Q) # plt.plot(noisy_lightcurve) # plt.plot(lightcurve) # plt.show() means = zeros((len(lightcurve), DIM_X)) covariances = zeros((len(lightcurve), DIM_X, DIM_X)) residuals = zeros(len(lightcurve)) filtered_lightcurve = zeros(len(lightcurve)) for i, (z, obsvec, sunvec, dt) in enumerate(zip(lightcurve, obsvecs, sunvecs, dts)): kf.update(z, obsvec=obsvec, sunvec=sunvec, exposure_time=exposure_time, Geometry=Geometry) kf.predict(dt=dt, inertia=Inertia_Matrix, est_inertia=est_inertia) #print('[{0:+.4f}, {1:+.4f}, {2:+.4f}] [{3:+.4f}, {4:+.4f}, {5:+.4f}] {6:<10}'.format(*kf.x, i), end = '\r') #switch from mrps to shadow parameters if norm(kf.x[0:3]) > 1: mrps = kf.x[0:3] kf.x[0:3] = -mrps / norm(mrps)**2 #keep angular velocity from exploding #limits maximum estimate to be 1 radian/s if norm(kf.x[3:6]) > 1: kf.x[3:6] = kf.x[3:6] / norm(kf.x[3:6]) #keep inertia from exploding #limits maximum inertia estimate to be 5 if est_inertia: if abs(kf.x[6]) > 5: kf.x[6] = 5 if abs(kf.x[7]) > 5: kf.x[7] = 5 #print(kf.sigmas_f) #print(kf.y) loading_bar(i / len(lightcurve), 'Filtering Data') means[i, :] = kf.x covariances[i, :, :] = kf.P residuals[i] = kf.y filtered_lightcurve[i] = kf.zp return means, covariances, residuals, filtered_lightcurve
obstacle_threshold = 15 voltage_threshold = 6.7 uv_counter = 0 orientation = rpi_orientation() yaw_gyro,yaw_mag,yaw_mag_compensated =0,0,0 ts, yaws_gyro, yaws_mag, yaws_mag_compensated = [], [], [], [] rolls, pitchs = [], [] sigmas = JulierSigmaPoints(n=2, kappa=1) def fx(x, dt): xout = np.empty_like(x) xout[0] = x[1] * dt + x[0] xout[1] = x[1] return xout def hx(x): return x[:1] # return position [x] ukf = UnscentedKalmanFilter(dim_x=2, dim_z=1, dt=1., hx=hx, fx=fx, points=sigmas) ukf.P *= 10 ukf.R *= .5 ukf.Q = Q_discrete_white_noise(2, dt=1., var=0.03) while(1):
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 = 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 *= 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')
Ns = 12 ukf_test = ukf_uav.UnscentedKalmanFilter(Ns, Ns, 0.01) q = 1 r = 1 ukf_test.J = J ukf_test.e3 = e3 Q = q**2*np.eye(Ns) R = r**2 x = np.zeros(Ns) P = np.eye(Ns) x_ukf = [] x_sensor = [] sp = JulierSigmaPoints(Ns,5) def state_tran(x, dt): A = np.eye(12) for i in range(3): A[i,i+3] = dt A[i+6,i+9] = dt return np.dot(A,x) def obs_state(x): A = np.zeros((6,12)) for i in range(3): A[i,i+3] = 1 A[i+3, i + 9] = 1 return np.dot(A,x) ukf_filter = UKF.UnscentedKalmanFilter(dim_x=Ns,dim_z=6, dt=0.01, hx = obs_state, fx = state_tran, points = sp) ukf_filter.Q = Q ukf_filter.R = R