def __init__(self, current_temp, dt): """Unscented kalman filter""" self._interval = 0 self._last_update = time.time() # sigmas = JulierSigmaPoints(n=2, kappa=0) sigmas = MerweScaledSigmaPoints(n=2, alpha=0.001, beta=2, kappa=0) self._kf_temp = UnscentedKalmanFilter( dim_x=2, dim_z=1, dt=dt, hx=hx, fx=fx, points=sigmas ) self._kf_temp.x = np.array([float(current_temp), 0.0]) self._kf_temp.P *= 0.2 # initial uncertainty self.interval = dt
def test_vhartman(): """ Code provided by vhartman on github #172 https://github.com/rlabbe/filterpy/issues/172 """ def fx(x, dt): # state transition function - predict next state based # on constant velocity model x = vt + x_0 F = np.array([[1.]], dtype=np.float32) 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]]) dt = 1.0 # create sigma points to use in the filter. This is standard for Gaussian processes points = MerweScaledSigmaPoints(1, alpha=1, beta=2., kappa=0.1) kf = UnscentedKalmanFilter(dim_x=1, dim_z=1, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([0.]) # initial state kf.P = np.array([[1]]) # initial uncertainty kf.R = np.diag([1]) # 1 standard kf.Q = np.diag([1]) # 1 standard ekf = ExtendedKalmanFilter(dim_x=1, dim_z=1) ekf.F = np.array([[1]]) ekf.x = np.array([0.]) # initial state ekf.P = np.array([[1]]) # initial uncertainty ekf.R = np.diag([1]) # 1 standard ekf.Q = np.diag([1]) # 1 standard np.random.seed(0) zs = [[np.random.randn()] for i in range(50)] # measurements for z in zs: kf.predict() ekf.predict() assert np.allclose(ekf.P, kf.P) assert np.allclose(ekf.x, kf.x) kf.update(z) ekf.update(z, lambda x: np.array([[1]]), hx) assert np.allclose(ekf.P, kf.P) assert np.allclose(ekf.x, kf.x)
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_linear_2d_simplex(): """ 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 = SimplexSigmaPoints(n=4) kf = UnscentedKalmanFilter(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 mss = MerweScaledSigmaPoints(4, .1, 2., -1) ukf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=mss) ukf.x = np.array([-1., 1., -1., 1]) ukf.P *= 1 x = kf.x zs = [] for i in range(20): x = fx(x, dt) z = np.array([x[0] + randn() * 0.1, x[2] + randn() * 0.1]) zs.append(z) Ms, Ps = kf.batch_filter(zs) smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dts=dt) if DO_PLOT: zs = np.asarray(zs) plt.plot(Ms[:, 0]) plt.plot(smooth_x[:, 0], smooth_x[:, 2]) print(smooth_x)
def __init__(self, dt=0.25, w=1.0): self.fx_filter_vel = 0.0 self.fy_filter_vel = 0.0 self.fz_filter_vel = 0.0 self.fsigma_filter_vel = 0.0 self.fpsi_filter_vel = 0.0 self.fphi_filter_vel = 0.0 # Q Process Noise Matrix self.Q = np.diag([0.5**2, 0.5**2, 0.5**2, 0.1**2, 0.1**2, 0.05**2]) # [x, y, z, sigma, psi, phi] # R Measurement Noise Matrix self.R = np.diag([8.5**2, 8.5**2, 8.5**2, 1.8**2, 8.5**2, 1.8**2]) # [x, y, z, sigma, psi, phi] # Sigma points self.sigma = [self.alpha, self.beta, self.kappa] = [0.9, 0.5, -2.0] self.w = w self.dt = dt self.t = 0 self.number_state_variables = 6 self.points = MerweScaledSigmaPoints(n=self.number_state_variables, alpha=self.alpha, beta=self.beta, kappa=self.kappa) self.kf = UKF(dim_x=self.number_state_variables, dim_z=self.number_state_variables, dt=self.dt, fx=self.f_bicycle, hx=self.H_bicycle, points=self.points) # Q Process Noise Matrix self.kf.Q = self.Q # R Measurement Noise Matrix self.kf.R = self.R # H identity self.H = np.eye(6) self.K = np.zeros((6, 6)) # Kalman gain self.y = np.zeros((6, 1)) # residual self.kf.x = np.zeros((1, self.number_state_variables)) # Initial state self.kf.P = np.eye( self.number_state_variables) * 10 # Covariance matrix self.P = self.kf.P
def __init__(self, bbox): """ Initialises a tracker using initial bounding box. """ def fx(x, dt): # state transition function - predict next state based on constant velocity model x = x_0 + vt F = np.array([[1, 0, 0, 0, dt, 0, 0], [0, 1, 0, 0, 0, dt, 0], [0, 0, 1, 0, 0, 0, dt], [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]], dtype=float) return np.dot(F, x) def hx(x): # measurement function - convert state into a measurement where measurements are [u, v, s, r] return np.array([x[0], x[1], x[2], x[3]]) dt = 0.1 # create sigma points to use in the filter. This is standard for Gaussian processes points = MerweScaledSigmaPoints(7, alpha=.1, beta=2., kappa=-1) #define constant velocity model self.kf = UnscentedKalmanFilter(dim_x=7, dim_z=4, dt=dt, fx=fx, hx=hx, points=points) # Assign the initial value of the state self.kf.x[:4] = convert_bbox_to_z(bbox) # Covariance matrix (dim_x, dim_x), default I self.kf.P[ 4:, 4:] *= 1000. # Give high uncertainty to the unobservable initial velocities self.kf.P *= 0.2 # Measuerment uncertainty (dim_z, dim_z), default I self.kf.R *= 0.01 # Process noise (dim_x, dim_x), default I self.kf.Q[-1, -1] *= 0.0001 self.kf.Q[4:, 4:] *= 0.0001 self.time_since_update = 0 self.id = KalmanBoxTracker.count KalmanBoxTracker.count += 1 self.history = [] self.hits = 0 self.hit_streak = 0 self.age = 0
def __init__(self, dt=1.0, w=1.0): self.dim_x = 6 self.dim_z = 2 self._w = w sigma_points = MerweScaledSigmaPoints(n=self.dim_x, alpha=.3, beta=2., kappa=-3) super(ukfFilter, self).__init__(dim_x=self.dim_x, dim_z=self.dim_z, fx=self.f_ct, hx=self.h_ca, dt=dt, points=sigma_points)
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 = 7 # 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], # [yaw]] # Number of measurement variables that the drone receives self.measurement_vector_dim = 6 # The measurement variables consist of the following vector: # [[slant_range], # [x], # [y], # [x_vel], # [y_vel], # [yaw]] # 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 __init__( self, filter_settings: RobotFilterSettings, dt: float = 1 / 60, points: Optional[Any] = None, ): self._log = structlog.get_logger().bind(dt=dt, points=points) self._settings = filter_settings super().__init__( dim_x=6, dim_z=3, dt=dt, hx=self._hx, fx=self._fx, points=points or MerweScaledSigmaPoints(4, alpha=0.1, beta=2.0, kappa=-1), x_mean_fn=self._x_mean_fn, z_mean_fn=self._z_mean_fn, residual_x=self._residual_x, residual_z=self._residual_z, ) # set initial state to all zeros, and initialize the state # covariances self.set_state(np.zeros((6, 1))) # TODO(dschwab): taken from robot tracker. I'm not sure this # is 100% correct or at least 100% the best it can be. Seems # to be assuming that given a perfect velocity, our process # will give us a perfect position. At the least this does not # account for bad friction models, pushing, slippage, etc # where velocity integration over the timestep is not 100% # correct. self.Q = np.diag([ 0, 0, 0, self._settings.velocity_variance, self._settings.velocity_variance, self._settings.angvel_variance, ]) # TODO(dschwab): taken from robot tracker. I'm not sure this # is 100% correct or at least 100% the best it can be. self.R = np.diag([ self._settings.position_variance, self._settings.position_variance, self._settings.theta_variance, ])
def UKFinit(): global ukf ukf_fuse = [] p_std_x, p_std_y = 0.2, 0.2 v_std_x, v_std_y = 0.01, 0.01 a_std_x, a_std_y = 0.01, 0.01 dt = 0.0125 #80HZ sigmas = MerweScaledSigmaPoints(6, alpha=.1, beta=2., kappa=-1.0) ukf = UKF(dim_x=6, dim_z=6, fx=f_cv, hx=h_cv, dt=dt, points=sigmas) ukf.x = np.array([0., 0., 0., 0., 0., 0.]) ukf.R = np.diag([p_std_x, v_std_x, a_std_x, p_std_y, v_std_y, a_std_y]) ukf.Q[0:3, 0:3] = Q_discrete_white_noise(3, dt=dt, var=0.2) ukf.Q[3:6, 3:6] = Q_discrete_white_noise(3, dt=dt, var=0.2) ukf.P = np.diag([8, 2, 2, 5, 2, 2])
def UKFinit(): global ukf ukf_fuse = [] std_y, std_z = 0.01, 0.01 vstd_y = 0.1 vstd_z = 0.1 dt = 0.005 sigmas = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1.0) ukf = UKF(dim_x=4, dim_z=4, fx=f_cv, hx=h_cv, dt=dt, points=sigmas) ukf.x = np.array([0., 0., 0., 0.]) ukf.R = np.diag([std_y, vstd_y, std_z, vstd_z]) ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.2) ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=dt, var=0.2) ukf.P = np.diag([3**2, 0.5, 3**2, 0.5])
def UKFinit(): global ukf p_std_x, p_std_y = 0.1, 0.1 v_std_x, v_std_y = 0.1, 0.1 dt = 0.0125 #80HZ sigmas = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1.0) ukf = UKF(dim_x=4, dim_z=4, fx=f_cv, hx=h_cv, dt=dt, points=sigmas) ukf.x = np.array([0., 0., 0., 0.]) ukf.R = np.diag([p_std_x, v_std_x, p_std_y, v_std_y]) ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.2) ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=dt, var=0.2) ukf.P = np.diag([8, 1.5 ,5, 1.5])
def UKFinit(): global ukf ukf_fuse = [] p_std_x = rospy.get_param('~p_std_x',0.005) v_std_x = rospy.get_param('~v_std_x',0.05) p_std_y = rospy.get_param('~p_std_y',0.005) v_std_y = rospy.get_param('~v_std_y',0.05) dt = rospy.get_param('~dt',0.01) #100HZ sigmas = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1.0) ukf = UKF(dim_x=4, dim_z=4, fx=f_cv, hx=h_cv, dt=dt, points=sigmas) ukf.x = np.array([0., 0., 0., 0.,]) ukf.R = np.diag([p_std_x, v_std_x, p_std_y, v_std_y]) ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=4.0) ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=dt, var=4.0) ukf.P = np.diag([3, 1, 3, 1])
def ukf_process(x, P, sigma_range, sigma_bearing, dt=1.0): points = MerweScaledSigmaPoints(n=3, alpha=0.001, beta=2, kappa=0, subtract=residual) # build the UKF based on previous functions ukf = UKF(dim_x=3, dim_z=3, fx=move, hx=Hx, dt=dt, points=points, x_mean_fn=state_mean, z_mean_fn=z_mean, residual_x=residual, residual_z=residual) # assign the parameters of ukf ukf.x = np.array(x) ukf.P = P ukf.R = np.diag([sigma_range**2, sigma_range**2, sigma_bearing**2]) ukf.Q = np.eye(3)*0.0001 return ukf
def run_ukf(zs, dt=1.0): sigmas = MerweScaledSigmaPoints(4, alpha=0.1, beta=2.0, kappa=1.0) ukf = UKF(dim_x=4, dim_z=2, fx=f, hx=h, dt=dt, points=sigmas) ukf.x = np.array([0.0, 0.0, 0.0, 0.0]) ukf.R = np.diag([0.09, 0.09]) ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.02) ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.02) uxs = [] for z in zs: ukf.predict() ukf.update(z) uxs.append(ukf.x.copy()) uxs = np.array(uxs) return uxs
def Unscentedfilter(zs): # Filter function points = MerweScaledSigmaPoints(2, alpha=.1, beta=2., kappa=1) ukf = UnscentedKalmanFilter(dim_x=2, dim_z=1, fx=fx, hx=hx, points=points, dt=dt) ukf.Q = array(([50, 0], [0, 50])) ukf.R = 100 ukf.P = eye(2) * 2 mu, cov = ukf.batch_filter(zs) x, _, _ = ukf.rts_smoother(mu, cov) return x[:, 0]
def UKFinit(): global ukf ukf_fuse = [] p_std_yaw = 0.004 v_std_yaw = 0.008 dt = 0.0125 #80HZ sigmas = MerweScaledSigmaPoints(2, alpha=.1, beta=2., kappa=-1.0) ukf = UKF(dim_x=2, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, points=sigmas) ukf.x = np.array([ 0., 0., ]) ukf.R = np.diag([p_std_yaw, v_std_yaw]) ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.2) ukf.P = np.diag([6.3, 1])
def _test_log_likelihood(): from filterpy.common import Saver 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.Q = Q_discrete_white_noise(dim=2, dt=dt, var=1.1**2, block_size=2) kf.x = np.array([-1., 1., -1., 1]) kf.P *= 1. zs = [[i + randn() * z_std, i + randn() * z_std] for i in range(40)] s = Saver(kf) for z in zs: kf.predict() kf.update(z) print(kf.x, kf.log_likelihood, kf.P.diagonal()) 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() if DO_PLOT: plt.plot(s.x[:, 0], s.x[:, 2])
def __init__(self, dt=1.0, w=1.0): self.dim_x = 4 self.dim_z = 2 self._w = w self._dt = dt self.Ft = np.eye(self.dim_x) self._T = 0. sigma_points = MerweScaledSigmaPoints(n=self.dim_x, alpha=.1, beta=2., kappa=-1.) super(ukfFilterct2, self).__init__(dim_x=self.dim_x, dim_z=self.dim_z, fx=self.f_ct, hx=self.h_ca, dt=dt, points=sigma_points)
def unscented_kf(self, number=number): global Time P0 = np.diag([ 3e-2, 3e-2, 3e-2, 3e-6, 3e-6, 3e-6, 3e-2, 3e-2, 3e-2, 3e-6, 3e-6, 3e-6 ]) error = np.random.multivariate_normal(mean=np.zeros(12), cov=P0) X0 = np.hstack( (HPOP_1[0], HPOP_2[0]) ) + error points = MerweScaledSigmaPoints(n=12, alpha=0.001, beta=2.0, kappa=-9) ukf = UKF(dim_x=12, dim_z=4, fx=self.state_equation, hx=self.measure_equation, dt=STEP, points=points) ukf.x = X0; ukf.P = P0; ukf.R = Rk; ukf.Q = Qk; XF, XP = [X0], [X0] print(error, "\n", Qk[0][0], "\n", Rk[0][0]) for i in range(1, number+1): ukf.predict() Z = nav.measure_stk(i) ukf.update(Z) X_Up = ukf.x.copy(); XF.append(X_Up) Time = Time+STEP XF = np.array(XF) return XF
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 test_ukf_ekf_comparison(): def fx(x, dt): # state transition function - predict next state based # on constant velocity model x = vt + x_0 F = np.array([[1.]], dtype=np.float32) 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]]) dt = 1.0 # create sigma points to use in the filter. This is standard for Gaussian processes points = MerweScaledSigmaPoints(1, alpha=1, beta=2., kappa=0.1) ukf = UnscentedKalmanFilter(dim_x=1, dim_z=1, dt=dt, fx=fx, hx=hx, points=points) ukf.x = np.array([0.]) # initial state ukf.P = np.array([[1]]) # initial uncertainty ukf.R = np.diag([1]) # 1 standard ukf.Q = np.diag([1]) # 1 standard ekf = ExtendedKalmanFilter(dim_x=1, dim_z=1) ekf.F = np.array([[1]]) ekf.x = np.array([0.]) # initial state ekf.P = np.array([[1]]) # initial uncertainty ekf.R = np.diag([1]) # 1 standard ekf.Q = np.diag([1]) # 1 standard np.random.seed(0) zs = [[np.random.randn()] for i in range(50)] # measurements for z in zs: ukf.predict() ekf.predict() assert np.allclose(ekf.P, ukf.P), 'ekf and ukf differ after prediction' ukf.update(z) ekf.update(z, lambda x: np.array([[1]]), hx) assert np.allclose(ekf.P, ukf.P), 'ekf and ukf differ after update'
def __init__(self, number_of_points, size, arena_origin_position): self.tracker = ukf.MultiUnscentedKalmanFilter(np.array( [0, 0, 0, 0, 0, 0, 2, 0]), np.eye(8) * 500, create_Q, Q_generator_args=(0.1, )) self.tracker.filters["pose_array"] = ukf.UnscentedKalmanFilter( 8, 4, 0, h_pose, f, MerweScaledSigmaPoints(8, 1, 3, -5)) stride = size / (number_of_points - 1.0) self.number_of_features = number_of_points * number_of_points self.features_positions = np.empty((self.number_of_features, 2)) for x in xrange(number_of_points): for y in xrange(number_of_points): self.features_positions[x + y * number_of_points] = np.array( [x * stride, y * stride]) - arena_origin_position self.lastMesuredStatus = None
def __init__(self, dt): self.dt = dt H = np.array([[0, 1, 0, 1], [0, 0, 1, 0]]) def h(x): return np.dot(H, x) sigmas = MerweScaledSigmaPoints(4, alpha=self.alpha, beta=self.beta, kappa=self.kappa) UKF.__init__(self, dim_x=4, dim_z=2, fx=self.f_xu, hx=h, dt=dt, points=sigmas)
def iniciar_ukf(list_z): 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([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) zs = list_z x_predichas = [] y_predichas = [] x_estimadas = [] y_estimadas = [] for z in zs: # Predicción kf.predict() xp = kf.x[0] yp = kf.x[1] x_predichas.append(xp) y_predichas.append(yp) print("PREDICCION: x:", xp, "y:", yp) # Actualización kf.update(z) xe = kf.x[0] ye = kf.x[1] x_estimadas.append(xe) y_estimadas.append(ye) print("ESTIMADO: x:", xe, "y:", ye) print("--------------------------------------") plt.plot(x_predichas, y_predichas, linestyle="-", color='orange') plt.plot(x_estimadas, y_estimadas, linestyle="-", color='b') plt.show()
def test_propagate(self): dt = 1 bike_lr, bike_lf, car_lr, car_lf, sigma_phi, sigma_v, sigma_d = (1., 1., 1., 1., 1., 1., 1.) model = BicycleModel(dt, bike_lr, bike_lf, car_lr, car_lf, sigma_phi, sigma_v, sigma_d) dim_x = 5 dim_z = 2 points = MerweScaledSigmaPoints(dim_x, alpha=1, beta=2., kappa=2) self.assertAlmostEqual(sum(points.Wm), 1) print() print(points.Wm) dt = 1 measurement_model = np.array([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0]]) measurement_noise = 1 * np.eye(2) state = np.array([1., 1., pi / 2, 1., 0]) variance = np.eye(dim_x) * 0.2 # initial uncertainty\ kf = UKF(dim_x=dim_x, dim_z=dim_z, dt=dt, points=points, motion_model=model, measurement_model=measurement_model) z_std = 0.1 kf.R = np.diag([z_std**2, z_std**2]) # 1 standard kf.Q = np.eye(dim_x) zs = [[i + np.random.randn() * z_std, i + np.random.randn() * z_std] for i in range(50)] # measurements _state, _var = kf.predict(state, variance, model, motion_noise=kf.Q, object_class='Car') print("State: {}".format(_state)) print("Var: {}".format(_var))
def setup(): # set up sensor simulator H = np.array([[1, 0, 0], [0, 1, 0]]) def h(x): return np.dot(H, x) x0 = np.array([[0], [-1], [0]]) sim = Sensor(x0, f, h) # set up kalman filter sigmas = MerweScaledSigmaPoints(3, alpha=.1, beta=2., kappa=1) tracker = UKF(dim_x=3, dim_z=2, fx=f, hx=h, dt=dt, points=sigmas) tracker.Q = np.diag((var_pos, var_pos, var_heading)) tracker.R = R_proto * measurement_var * filter_misestimation_factor tracker.x = np.array([0, 0, 0]) tracker.P = np.eye(3) * 500 return sim, tracker
def __init__(self, g_params, dim_z=3): self.sigmas = MerweScaledSigmaPoints(n=2, alpha=.1, beta=10., kappa=0.) self.ukf = UnscentedKalmanFilter(dim_x=2, dim_z=dim_z, dt=1., hx=self.hx, fx=self.fx, points=self.sigmas) self.s1params_a = g_params[0] self.s1params_b = g_params[1] self.s1params_c = g_params[2] self.s2params_a = g_params[3] self.s2params_b = g_params[4] self.s2params_c = g_params[5] self.s3params_a = g_params[6] self.s3params_b = g_params[7]
def __init__(self, initial_x, initial_y, initial_vx, initial_vy): self.dt = 0.05 # create sigma points to use in the filter. This is standard for Gaussian processes self.points = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1) self.kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=self.dt, fx=fx, hx=hx, points=self.points) self.kf.x = np.array([initial_x, 0, initial_y, 0]) # initial state self.kf.P *= 0.1 # initial uncertainty self.z_std = 0.1 self.kf.R = np.diag([self.z_std**2, self.z_std**2]) # 1 standard self.kf.Q = Q_discrete_white_noise(dim=2, dt=self.dt, var=0.01**2, block_size=2)
def smooth(data: ndarray, dt: float): points = MerweScaledSigmaPoints(3, alpha=1e-3, beta=2.0, kappa=0) noisy_kalman = UnscentedKalmanFilter( dim_x=3, dim_z=1, dt=dt, hx=state_to_measurement, fx=state_transition, points=points, ) noisy_kalman.x = array([0, data[1], data[1] - data[0]], dtype="float32") noisy_kalman.R *= 20**2 # sensor variance noisy_kalman.P = diag([5**2, 5**2, 1**2]) # variance of the system noisy_kalman.Q = Q_discrete_white_noise(3, dt=dt, var=0.05) means, covariances = noisy_kalman.batch_filter(data) means[:, 1][means[:, 1] < 0] = 0 # clip velocity return means[:, 1]