def build_ukf(x, P, std_r, std_b, dt=1.0): ''' Build UKF. x: initial state. P: initial covariance matrix. std_r: standard var. of laser measurement. std_b: standard var. of IMU measurement. dt: time interval. Plus some defined functions as parameters. returns ukf. ''' # Calculate sigma points. points = MerweScaledSigmaPoints(n=6, alpha=0.001, beta=2, kappa=-3, subtract=residual_x) ukf = UKF(dim_x=6, dim_z=4, 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_z) ukf.x = np.array(x) ukf.P = P ukf.R = np.diag([std_r ** 2, std_r ** 2, std_b ** 2, std_b ** 2]) q1 = Q_discrete_white_noise(dim=2, dt=dt, var=1.0) q2 = Q_discrete_white_noise(dim=2, dt=dt, var=1.0) q3 = Q_discrete_white_noise(dim=2, dt=dt, var=3.05 * pow(10, -4)) ukf.Q = block_diag(q1, q2, q3) # ukf.Q = np.eye(3) * 0.0001 return ukf
def create_ukf(cmds, landmarks, sigma_vel, sigma_steer, sigma_range, sigma_bearing, ellipse_step=1, step=10): points = MerweScaledSigmaPoints(n=3, alpha=0.03, beta=2., kappa=0, subtract=residual_x, sqrt_method=sqrt_func) ukf = UKF(dim_x=3, dim_z=2 * len(landmarks), fx=move, hx=Hx, dt=dt, points=points, x_mean_fn=state_mean, z_mean_fn=z_mean, residual_x=residual_x, residual_z=residual_h) ukf.x = np.array([203.0, 1549.2, 1.34]) ukf.P = np.diag([100., 100., .5]) ukf.R = np.diag([sigma_range**2, sigma_bearing**2] * len(landmarks)) ukf.Q = np.diag([10.**2, 10.**2, 0.3**2]) return ukf
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 _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 = UKF(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() s.to_array() plt.plot(s.x[:, 0], s.x[:, 2])
def main(): [t, dt, s, v, a, theta, omega, alpha] = build_real_values() zs = build_measurement_values(t, [a, omega]) u = build_control_values(t, v) [F, B, H, Q, R] = init_kalman(t, dt) sigmas = MerweScaledSigmaPoints(n=9, alpha=.1, beta=2., kappa=-1) kf = UKF(dim_x=9, dim_z=3, fx=f_bot, hx=h_bot, dt=0.2, points=sigmas) kf.x = np.array([0., 0., 0., 0., 0., 0., 0., 0., 0.]) kf.R = R kf.F = F kf.H = H kf.Q = Q xs, cov = [], [] for zk, uk in zip(zs, u): kf.predict(fx_args=uk) kf.update(z=zk) xs.append(kf.x.copy()) cov.append(kf.P) xs, cov = np.array(xs), np.array(cov) xground = construct_xground(s, v, a, theta, omega, alpha, xs.shape) nees = NEES(xground, xs, cov) print(np.mean(nees)) plot_results(t, xs, xground, zs, nees)
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 ukf_process(x, P, sigma_range, sigma_bearing, dt=1.0): """ construct Unscented Kalman Filter with the initial state x and the initial covaiance matrix P sigma_range: the std of laser range sensors sigma_bearing: the std of IMU """ # construct the sigma points 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 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 = 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]]) 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 run_localization( cmds, landmarks, sigma_vel, sigma_steer, sigma_range, sigma_bearing, ellipse_step=1, step=10): plt.figure() points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0, subtract=residual_x) ukf = UKF(dim_x=3, dim_z=2*len(landmarks), fx=fx, hx=Hx, dt=dt, points=points, x_mean_fn=state_mean, z_mean_fn=z_mean, residual_x=residual_x, residual_z=residual_h) ukf.x = np.array([2, 6, .3]) ukf.P = np.diag([.1, .1, .05]) ukf.R = np.diag([sigma_range**2, sigma_bearing**2]*len(landmarks)) ukf.Q = np.eye(3)*0.0001 sim_pos = ukf.x.copy() # plot landmarks if len(landmarks) > 0: plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60) track = [] for i, u in enumerate(cmds): sim_pos = move(sim_pos, u, dt/step, wheelbase) track.append(sim_pos) if i % step == 0: ukf.predict(fx_args=u) if i % ellipse_step == 0: plot_covariance_ellipse( (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6, facecolor='k', alpha=0.3) x, y = sim_pos[0], sim_pos[1] z = [] for lmark in landmarks: dx, dy = lmark[0] - x, lmark[1] - y d = sqrt(dx**2 + dy**2) + randn()*sigma_range bearing = atan2(lmark[1] - y, lmark[0] - x) a = (normalize_angle(bearing - sim_pos[2] + randn()*sigma_bearing)) z.extend([d, a]) ukf.update(z, hx_args=(landmarks,)) if i % ellipse_step == 0: plot_covariance_ellipse( (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6, facecolor='g', alpha=0.8) track = np.array(track) plt.plot(track[:, 0], track[:,1], color='k', lw=2) plt.axis('equal') plt.title("UKF Robot localization") plt.show() return ukf
def 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 kf.Q = Q kf.R = R kf.x = np.array([0., 90., 1100., 0.]) # initial gauss return kf
def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)): n_state = len(Q) n_meas = len(R) sigmas = SigmaPoints(n_state, alpha=.1, beta=2., kappa=1.) ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal, hx=h_kal, dt=dt, points=sigmas) ukf.Q = Q ukf.R = R self.ukf = ukf self.isFirst = True
def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)): n_state = len(Q) n_meas = len(R) sigmas = SigmaPoints(n_state, alpha=.5, beta=2., kappa=0.) ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal_accel, hx=h_kal_accel, dt=dt, points=sigmas, x_mean_fn = state_mean, residual_x=res_x, residual_z=res_x) ukf.Q = Q ukf.R = R self.ukf = ukf self.isFirst = True
def 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 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 create(self, pose, *args, **kwargs): print 'CREATE!' ukf = UKF(**self.ukf_args) ukf._Q = self.Q.copy() ukf.Q = self.Q.copy() ukf.R = self.R.copy() ukf.x = pose #np.zeros(5, dtype=np.float32) #ukf.x[:3] = pose[:3] ukf.P = self.P.copy() # TODO : fill in more info, such as color(red/green/unknown), type(target/obs/unknown) self.est[self.p_idx] = UKFEstimate(pose, *args, ukf=ukf, **kwargs) self.p_idx += 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 __init__(self, trueTrajectory, dt, route, Q=np.eye(2), R=np.eye(2)): #from filterpy.kalman import KalmanFilter as KF from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints n_state = len(Q) n_meas = len(R) sigmas = SigmaPoints(n_state, alpha=.1, beta=2., kappa=0.) ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal_v, hx=h_kal, dt=dt, points=sigmas) ukf.Q = Q ukf.R = R self.ukf = ukf self.isFirst = True self.route = route
def 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 test_1d(): def fx(x, dt): F = np.array([[1., dt], [0, 1]]) return np.dot(F, x) def hx(x): return np.array([[x[0]]]) ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx) ckf.x = np.array([[1.], [2.]]) ckf.P = np.array([[1, 1.1], [1.1, 3]]) ckf.R = np.eye(1) * .05 ckf.Q = np.array([[0., 0], [0., .001]]) dt = 0.1 points = MerweScaledSigmaPoints(2, .1, 2., -1) kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([1, 2]) kf.P = np.array([[1, 1.1], [1.1, 3]]) kf.R *= 0.05 kf.Q = np.array([[0., 0], [0., .001]]) for i in range(50): z = np.array([[i+randn()*0.1]]) #xx, pp, Sx = predict(f, x, P, Q) #x, P = update(h, z, xx, pp, R) ckf.predict() ckf.update(z) kf.predict() kf.update(z[0]) assert abs(ckf.x[0] -kf.x[0]) < 1e-10 assert abs(ckf.x[1] -kf.x[1]) < 1e-10 plt.show()
def test_1d(): def fx(x, dt): F = np.array([[1., dt], [0, 1]]) return np.dot(F, x) def hx(x): return x[0:1] ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx) ckf.x = np.array([[1.], [2.]]) ckf.P = np.array([[1, 1.1], [1.1, 3]]) ckf.R = np.eye(1) * .05 ckf.Q = np.array([[0., 0], [0., .001]]) dt = 0.1 points = MerweScaledSigmaPoints(2, .1, 2., -1) kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([1, 2]) kf.P = np.array([[1, 1.1], [1.1, 3]]) kf.R *= 0.05 kf.Q = np.array([[0., 0], [0., .001]]) s = Saver(kf) for i in range(50): z = np.array([[i+randn()*0.1]]) ckf.predict() ckf.update(z) kf.predict() kf.update(z[0]) assert abs(ckf.x[0] - kf.x[0]) < 1e-10 assert abs(ckf.x[1] - kf.x[1]) < 1e-10 s.save() # test mahalanobis a = np.zeros(kf.y.shape) maha = scipy_mahalanobis(a, kf.y, kf.SI) assert kf.mahalanobis == approx(maha) s.to_array()
def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)): n_state = len(Q) n_meas = len(R) sigmas = SigmaPoints(n_state, alpha=.5, beta=2., kappa=0.) ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal_accel, hx=h_kal_accel, dt=dt, points=sigmas, x_mean_fn=state_mean, residual_x=res_x, residual_z=res_x) ukf.Q = Q ukf.R = R self.ukf = ukf self.isFirst = True
def 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 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 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]
def test_1d(): def fx(x, dt): F = np.array([[1., dt], [0, 1]]) return np.dot(F, x) def hx(x): return x[0:1] ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx) ckf.x = np.array([[1.], [2.]]) ckf.P = np.array([[1, 1.1], [1.1, 3]]) ckf.R = np.eye(1) * .05 ckf.Q = np.array([[0., 0], [0., .001]]) dt = 0.1 points = MerweScaledSigmaPoints(2, .1, 2., -1) kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([1, 2]) kf.P = np.array([[1, 1.1], [1.1, 3]]) kf.R *= 0.05 kf.Q = np.array([[0., 0], [0., .001]]) s = Saver(kf) for i in range(50): z = np.array([[i + randn() * 0.1]]) ckf.predict() ckf.update(z) kf.predict() kf.update(z[0]) assert abs(ckf.x[0] - kf.x[0]) < 1e-10 assert abs(ckf.x[1] - kf.x[1]) < 1e-10 s.save() # test mahalanobis a = np.zeros(kf.y.shape) maha = scipy_mahalanobis(a, kf.y, kf.SI) assert kf.mahalanobis == approx(maha) s.to_array()
def test_1d(): def fx(x, dt): F = np.array([[1., dt], [0, 1]]) return np.dot(F, x) def hx(x): return x[0:1] ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx) ckf.x = np.array([[1.], [2.]]) ckf.P = np.array([[1, 1.1], [1.1, 3]]) ckf.R = np.eye(1) * .05 ckf.Q = np.array([[0., 0], [0., .001]]) dt = 0.1 points = MerweScaledSigmaPoints(2, .1, 2., -1) kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([1, 2]) kf.P = np.array([[1, 1.1], [1.1, 3]]) kf.R *= 0.05 kf.Q = np.array([[0., 0], [0., .001]]) s = Saver(kf) for i in range(50): z = np.array([[i + randn() * 0.1]]) #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 s.save() s.to_array()
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]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0]]) 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]]) 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
""" px, py = landmark dist = sqrt((px - x[0])**2 + (py - x[1])**2) angle = atan2(py - x[1], px - x[0]) return array([dist, normalize_angle(angle - x[2])]) points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0) #points = JulierSigmaPoints(n=3, kappa=3) ukf= UKF(dim_x=3, dim_z=2, fx=fx, hx=Hx, dt=dt, points=points, x_mean_fn=state_mean, z_mean_fn=z_mean, residual_x=residual_x, residual_z=residual_h) ukf.x = array([2, 6, .3]) ukf.P = np.diag([.1, .1, .2]) ukf.R = np.diag([sigma_r**2, sigma_h**2]) ukf.Q = None#np.eye(3)*.00000 u = array([1.1, 0.]) xp = ukf.x.copy() plt.cla() plt.scatter(m[:, 0], m[:, 1]) cmds = [[v, .0] for v in np.linspace(0.001, 1.1, 30)] cmds.extend([cmds[-1]]*50) v = cmds[-1][0] cmds.extend([[v, a] for a in np.linspace(0, np.radians(2), 15)])
dt = 20 # ms points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2., kappa=1.) ukf = UKF(testx.shape[0], testy.shape[0], dt, fx=f_force, hx=h_forceToNeural, points=points) #(x_dimm, z_dimm,dt, hx, fx, sigmaPoints) ukf.x = [0.05, 0, 0] # initial gauss # Optimize : P fstd = 0.5 ratestd = 0.01 yankstd = 0.001 P = np.diag([fstd**2, ratestd**2, yankstd**2]) ukf.P = P ukf.Q = Q ukf.R = R predict, fvar = [], [] for t in np.arange(target.shape[0]): ukf.predict() ukf.update(testy[:, t]) fvar.append(ukf.P[0, 0]) predict.append(ukf.x[0]) #(f,f',f'') criterion = nn.MSELoss() mse_loss = criterion(torch.from_numpy(np.asarray(predict)), torch.from_numpy(np.asarray(target))) figname = plot_dir + "ukfResult" fig, ax = plt.subplots(2, 1, figsize=(12, 6))
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): try: dt = time() - dt mb.time += dt dt = time() mb.telemetry() ts.append(round(mb.time, 3)) gyros.append(mb.gyroz) magxs.append(mb.magx) magys.append(mb.magy)
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()
platform is stationary, this is a very difficult problem because there are an infinite number of solutions. The literature is filled with this example, along with proposed solutions (usually, platform makes manuevers). """ dt = 0.1 y = 20 platform_pos = (0, 20) sf = SUKF(2, 1, dt, alpha=1.0e-4, beta=2.0, kappa=1.0) sf.Q = Q_discrete_white_noise(2, dt, 0.1) f = UKF(2, 1, dt, kappa=0.0) f.Q = Q_discrete_white_noise(2, dt, 0.1) def fx(x, dt): """ state transition function""" # pos = pos + vel # vel = vel return array([x[0] + x[1], x[1]]) def hx(x): """ measurement function - convert position to bearing""" return math.atan2(platform_pos[1], x[0] - platform_pos[0])
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()
def residual(a, b): y = a - b y[2] = normalize_angle(y[2]) return y def f(x,dt,u): """Estimate the non-linear state of the system""" #print ((u[0]/self.L)*math.tan(u[1])) return np.array([x[0]+u[0]*math.cos(x[2]), x[1]+u[0]*math.sin(x[2]), x[2]+((u[0]/1)*math.tan(u[1]))]) def h(z): return z points = MerweScaledSigmaPoints(3,.001,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,1]) filter.Q = np.diag([(0.2)**2,(0.2)**2,(1*3.14/180)**2]) filter.R = np.diag([100,100,0.25]) robot = vehicle.Vehicle(1,50) #create a robot robot.setOdometry(True) #configure its odometer robot.setOdometryVariance([0.2,1]) speed,angle = [],[] for a in xrange(4): #create a retangular path for i in xrange(400): angle.append(0) for i in xrange(9): angle.append(10) for i in xrange(len(angle)): #set the speed to a constant along the path speed.append(1) robot.sim_Path(speed,angle) #run in a rectangular path
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')
left_reader = lr.LogReader(l_logname,l_first_data_time_ms) right_reader = lr.LogReader(r_logname,r_first_data_time_ms) track = tr.TrackingReader(track_fname,right_reader,track_data_log_offset,F,30,1,vid_fname=vid_fname) pos_init = left_reader.get_ekf_loc_1d(START_TIME) vel_init = left_reader.get_ekf_vel(START_TIME) att_init = np.deg2rad(left_reader.get_ekf_att(START_TIME)) att_vel_init = np.zeros(3) state_init = np.hstack((pos_init,vel_init,att_init,att_vel_init)) ukf = UKF(dim_x=12, dim_z=15, dt=1.0/30, fx=fx, hx=hx) ukf.P = np.diag([5,5,2, 2,2,2, .017,.017,.017, .1,.1,.1]) ukf.x = state_init ukf.Q = np.diag([.5,.5,.5, .5,.5,.5, .1,.1,.1, .1,.1,.1]) T = np.array([16.878, -7.1368, 0]) #Translation vector joining two inertial frames time = np.arange(START_TIME,END_TIME,dt) print time.shape,time[0],time[-1] d = np.linspace(20,40,time.shape[0]) zs = np.zeros((time.shape[0],15)) Rs = np.zeros((time.shape[0],15,15)) means = np.zeros((time.shape[0],12)) covs = np.zeros((time.shape[0],12,12)) cam_locs = np.zeros((time.shape[0],3)) ekf_locs = np.zeros((time.shape[0],3)) for i in range(1,time.shape[0]): t = time[i]
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 test_circle(): from filterpy.kalman import KalmanFilter from math import radians def hx(x): radius = x[0] angle = x[1] x = cos(radians(angle)) * radius y = sin(radians(angle)) * radius return np.array([x, y]) def fx(x, dt): return np.array([x[0], x[1]+x[2], x[2]]) std_noise = .1 f = UKF(dim_x=3, dim_z=2, dt=.01, hx=hx, fx=fx, kappa=0) f.x = np.array([50., 90., 0]) f.P *= 100 f.R = np.eye(2)*(std_noise**2) f.Q = np.eye(3)*.001 f.Q[0,0]=0 f.Q[2,2]=0 kf = KalmanFilter(dim_x=6, dim_z=2) kf.x = np.array([50., 0., 0, 0, .0, 0.]) F = np.array([[1., 1., .5, 0., 0., 0.], [0., 1., 1., 0., 0., 0.], [0., 0., 1., 0., 0., 0.], [0., 0., 0., 1., 1., .5], [0., 0., 0., 0., 1., 1.], [0., 0., 0., 0., 0., 1.]]) kf.F = F kf.P*= 100 kf.H = np.array([[1,0,0,0,0,0], [0,0,0,1,0,0]]) kf.R = f.R kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001) kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001) measurements = [] results = [] zs = [] kfxs = [] for t in range (0,12000): a = t / 30 + 90 x = cos(radians(a)) * 50.+ randn() * std_noise y = sin(radians(a)) * 50. + randn() * std_noise # create measurement = t plus white noise z = np.array([x,y]) zs.append(z) f.predict() f.update(z) kf.predict() kf.update(z) # save data results.append (hx(f.x)) kfxs.append(kf.x) #print(f.x) results = np.asarray(results) zs = np.asarray(zs) kfxs = np.asarray(kfxs) print(results) if DO_PLOT: plt.plot(zs[:,0], zs[:,1], c='r', label='z') plt.plot(results[:,0], results[:,1], c='k', label='UKF') plt.plot(kfxs[:,0], kfxs[:,3], c='g', label='KF') plt.legend(loc='best') plt.axis('equal')
""" px = landmark[0] py = landmark[1] dist = np.sqrt((px - x[0])**2 + (py - x[1])**2) Hx = array([dist, atan2(py - x[1], px - x[0]) - x[2]]) return Hx points = MerweScaledSigmaPoints(n=3, alpha=1.e-3, beta=2, kappa=0) ukf= UKF(dim_x=3, dim_z=2, fx=fx, hx=Hx, dt=dt, points=points, x_mean_fn=state_mean, z_mean_fn=z_mean, residual_x=residual_x, residual_z=residual_h) ukf.x = array([2, 6, .3]) ukf.P = np.diag([.1, .1, .2]) ukf.R = np.diag([sigma_r**2, sigma_h**2]) ukf.Q = np.zeros((3,3)) u = array([1.1, .01]) xp = ukf.x.copy() plt.figure() plt.scatter(m[:, 0], m[:, 1]) for i in range(200): xp = move(xp, u, dt/10., wheelbase) # simulate robot plt.plot(xp[0], xp[1], ',', color='g') if i % 10 == 0: ukf.predict(fx_args=u)
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
sigma_r = .3 sigma_h = .1#radians(.5)#np.radians(1) #sigma_steer = radians(10) dt = 0.1 wheelbase = 0.5 points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0, subtract=residual_x) #points = JulierSigmaPoints(n=3, kappa=3) ukf= UKF(dim_x=3, dim_z=2*len(m), fx=fx, hx=Hx, dt=dt, points=points, x_mean_fn=state_mean, z_mean_fn=z_mean, residual_x=residual_x, residual_z=residual_h) ukf.x = array([2, 6, .3]) ukf.P = np.diag([.1, .1, .05]) ukf.R = np.diag([sigma_r**2, sigma_h**2]* len(m)) ukf.Q =np.eye(3)*.00001 u = array([1.1, 0.]) xp = ukf.x.copy() plt.cla() plt.scatter(m[:, 0], m[:, 1]) cmds = [[v, .0] for v in np.linspace(0.001, 1.1, 30)] cmds.extend([cmds[-1]]*50) v = cmds[-1][0] cmds.extend([[v, a] for a in np.linspace(0, np.radians(2), 15)])