def __init__(self, vmax): self.vmax_ = vmax # rospy.get_param('~vmax', 6.0) sigma_points = MerweScaledSigmaPoints(4, 1e-3, 2, -2) #sigma_points = JulierSigmaPoints(4, 5-2, sqrt_method=np.linalg.cholesky) self.ukf_l_ = UKF( dim_x=4, # x,y,vx,vy dim_z=2, # x,y dt=0.01, # note: dynamic hx=ukf_hx, fx=ukf_fx, points=sigma_points) self.ukf_r_ = UKF( dim_x=4, # x,y,vx,vy dim_z=2, # x,y dt=0.01, # note: dynamic hx=ukf_hx, fx=ukf_fx, points=sigma_points) self.l_lost_ = 0 self.r_lost_ = 0
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 self.U_init = [] self.w = w self.dt = dt self.t = 0 self.number_state_variables = 6 # Q Process Noise Matrix [x, y, z, sigma, psi, phi] self.Q = np.diag([0.5**2, 0.5**2, 0.5**2, 0.1**2, 0.1**2, 0.05**2]) # R Measurement Noise Matrix [xf, xr, yf, yr, zf, zr, za, delta, psi, phi] self.R = np.diag([ 3.5**2, 3.5**2, # x 3.5**2, 3.5**2, # y 1.5**2, 1.5**2, 3.5**2, # z 0.05**2, 0.05**2, 0.5**2 ]) # delta - psi - phi # Init Sigma points self.sigma = [self.alpha, self.beta, self.kappa] = [0.7, 2.0, -2.0] self.points = MerweScaledSigmaPoints(n=self.number_state_variables, alpha=self.alpha, beta=self.beta, kappa=self.kappa) # Create UKF filter based on filterpy library self.kf = UKF(dim_x=self.number_state_variables, dim_z=10, 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 self.kf.P = np.eye( self.number_state_variables) * 10 # Covariance matrix self.P = self.kf.P # only for consistency self.K = np.zeros((6, 6)) # Kalman gain self.y = np.zeros((6, 1)) # residual self.H = np.zeros((10, 6)) # 10 measurements x 6 state variables
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 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_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 = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([-1., 1., -1., 1]) kf.P *= 0.0001 zs = [] for i in range(20): z = np.array([i + randn() * 0.1, i + randn() * 0.1]) zs.append(z) Ms, Ps = kf.batch_filter(zs) smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt) if DO_PLOT: zs = np.asarray(zs) #plt.plot(zs[:,0]) plt.plot(Ms[:, 0]) plt.plot(smooth_x[:, 0], smooth_x[:, 2]) print(smooth_x)
def 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 _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 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 *= 1.1 # test __repr__ doesn't crash str(kf) zs = [[i + randn() * 0.1, i + randn() * 0.1] for i in range(20)] 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 __init__(self, param, SS_model, dim_x=2, dim_u=2, dim_y=2): super().__init__(param) self.SS_model = SS_model self.last_time = 0 self.last_commend = {} points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=2, kappa=1) self.ukf = UKF(dim_x=dim_x, dim_z=dim_y, fx=self.fx, hx=self.hx, dt=param['dt'], points=points) self.ukf.R = np.diag([ 0.0672, #Pressure transducer (DP) noise 0.0672 0.0187 ]) #Voltage noise 0.00187 self.ukf.Q = np.diag([ 0.15**2, #Pressure disturbance 0.2**2 ]) #Torque disturbance self.initialize_filter([param['q_pred'], param['w_pred']], param['P_init'])
def test_sigma_points_1D(): """ tests passing 1D data into sigma_points""" kappa = 0. ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa) points = ukf.weights(1, 0.) assert len(points) == 3 mean = 5 cov = 9 Xi = ukf.sigma_points(mean, cov, kappa) xm, ucov = unscented_transform(Xi, ukf.W, ukf.W, 0) # sum of weights*sigma points should be the original mean m = 0.0 for x, w in zip(Xi, ukf.W): m += x * w assert abs(m - mean) < 1.e-12 assert abs(xm[0] - mean) < 1.e-12 assert abs(ucov[0, 0] - cov) < 1.e-12 assert Xi.shape == (3, 1) assert len(ukf.W) == 3
def 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 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 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 filter_init(self): ## UKF filter parameters n=self.n m=self.m dt = 0.01 # self.dim_x = 20 # self.dim_z = 20 # #unscented points self.points = sigma_points.SimplexSigmaPoints(n=self.dim_x) # # initial cov Pos_cov = 10*np.eye(m*n) Vel_cov = np.eye(m*n) P_up = np.concatenate((Pos_cov, 0*np.eye(self.m*self.n)),axis=1) P_down = np.concatenate((0*np.eye(self.m*self.n), Vel_cov), axis=1) P = np.concatenate((P_up,P_down),axis=0) # initial process cov Q = np.eye(self.m*self.n*2)*(dt**2.) #meas cov GPS_cov = 10*np.eye(m*n) ACC_cov = np.eye(m*n) R_up = np.concatenate((GPS_cov, 0*np.eye(m*n)),axis=1) R_down = np.concatenate((0*np.eye(m*n), ACC_cov), axis=1) R = np.concatenate((R_up,R_down),axis=0) R = np.array(R) # self.filter = UKF(dim_x=self.dim_x, dim_z=self.dim_z, dt=dt, hx=self.hx, fx=self.fx, points=self.points) # self.filter.x = X self.filter.P = P self.filter.Q = Q self.filter.R = R
def test_batch_missing_data(): """ batch filter should accept missing data with None in the measurements """ def fx(x, dt): F = np.array( [[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = MerweScaledSigmaPoints(4, .1, 2., -1) kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([-1., 1., -1., 1]) kf.P *= 0.0001 zs = [] for i in range(20): z = np.array([i + randn() * 0.1, i + randn() * 0.1]) zs.append(z) zs[2] = None Rs = [1] * len(zs) Rs[2] = None Ms, Ps = kf.batch_filter(zs)
def test_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 creat_batch3(min_dn,max_dn,pos_noise,vel_noise,BN): batch_size = int(100*BN) my_traj, my_obser, Tran_m = trajectory_batch_generator(batch_size,data_len) Traj_r = my_traj Obser = my_obser Traj_s = np.array([[[0 for i in range(4)] for j in range(data_len)] for k in range(batch_size)],'float64') for i in range(batch_size): my_SP = SP(dim_state,kappa=0.) my_UKF = UKF(dim_x=dim_state, dim_z=dim_obser, dt=sT, hx=my_hx, fx=my_fx, points=my_SP) my_UKF.Q *= var_m my_UKF.R *= np.array([[azi_var,0],[0,dis_var]]) x0_noise = np.array([np.random.normal(0,pos_noise,2),np.random.normal(0,vel_noise,2)]) my_UKF.x = Traj_r[i,0,:] + np.reshape(x0_noise,[4,]) my_UKF.P *= 1. #tracking results of UKF xs = [] xs.append(my_UKF.x) for j in range(data_len-1): my_UKF.predict() my_UKF.update(Obser[i,j+1,:]) xs.append(my_UKF.x.copy()) Traj_s[i] = np.asarray(xs) return Traj_r, Obser, Traj_s, Traj_r-Traj_s
def __init__(self, sensor_std, state0, state): ''' 预测量:x, y, z, q0, q1, q2, q3 :param sensor_std:【float】sensor的噪声标准差[μV] :param state0: 【np.array】初始值 (7,) :param state: 【np.array】预测值 (7,) ''' self.stateNum = 7 # 预测量:x, y, z, q0, q1, q2, q3 self.dt = 0.01 # 时间间隔[s] self.points = MerweScaledSigmaPoints(n=self.stateNum, alpha=0.3, beta=2., kappa=3 - self.stateNum) self.ukf = UKF(dim_x=self.stateNum, dim_z=self.measureNum, dt=self.dt, points=self.points, fx=self.f, hx=self.h) self.ukf.x = state0.copy() # 初始值 self.x0 = state # 计算NEES的真实值 self.ukf.R *= sensor_std self.ukf.P = np.eye(self.stateNum) * 0.01 for i in range(3, 7): self.ukf.P[i, i] = 0.01 self.ukf.Q = np.eye( self.stateNum) * 0.001 * self.dt # 将速度作为过程噪声来源,Qi = [v*dt] for i in range(3, 7): self.ukf.Q[i, i] = 0.01 # 四元数的过程误差 self.pos = (round(self.ukf.x[0], 3), round(self.ukf.x[1], 3), round(self.ukf.x[2], 3)) self.m = q2R(self.ukf.x[3:7])[:, -1]
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 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 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 get_nonlinear_tracker1(): dt = IMSHOW_SLEEP_TIME / 1000 # time step sigmas = MerweScaledSigmaPoints(6, alpha=.1, beta=2., kappa=1.) ukf = UKF(dim_x=6, dim_z=2, fx=f_cv1, hx=h_cv1, dt=dt, points=sigmas) ukf.x = np.array([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) return ukf
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 create_filter(): dim_x = 5 points = MerweScaledSigmaPoints(dim_x, alpha = 1e-3, beta = 2, kappa = 0.0) ukf = UKF(dim_x = dim_x, dim_z = 2, dt = 0.1, hx = hx, fx = fx, points = points) ukf.Q *= np.identity(5) * 0.2 std_las = 0.15 ukf.R *= std_las * std_las ukf.x = np.zeros(5) ukf.P *= 0.2 return ukf
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, 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 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 __init__(self, trueTrajectory, dt=.1, noise=0.): from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints self.dt = dt sigmas = SigmaPoints(3, alpha=.1, beta=2., kappa=0.) self.KF = UKF(dim_x=3, dim_z=2, fx=f_kal_a, hx=h_kal, dt=dt, points=sigmas) self.KF.Q = np.diag([1., 0.5, 0.2]) self.KF.R = np.diag([2., 1.12]) * noise + np.diag([.05, .05]) self.first = True
def test_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 kf = UKF(3, 1, dt, fx=fx, hx=hx, kappa=0.) kf.Q *= 0.01 kf.R = 10 kf.x = np.array([0., 90., 1100.]) kf.P *= 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])