def get_rk(): global rk rk = ExtendedKalmanFilter(dim_x=9, dim_z=3) dt = 0.01 rk.F = asarray([[1., dt, dt * dt / 2, 0., 0., 0., 0., 0., 0.], [0., 1., dt, 0., 0., 0., 0., 0., 0.], [0., 0., 1., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 1., dt, dt * dt / 2, 0., 0., 0.], [0., 0., 0., 0., 1., dt, 0., 0., 0.], [0., 0., 0., 0., 0., 1., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 1., dt, dt * dt / 2], [0., 0., 0., 0., 0., 0., 0., 1.0, dt], [0., 0., 0., 0., 0., 0., 0., 0., 1.0]]) # 初始位置 先设置为0 # rk.x = array([0., 0., 1., 0., -0.473, 1.0, 0.6, 0., 1.]).T rk.x = array([0., 0., 1.0, 0., 0, 1.0, 0.9, 0.0, 0.0]).T # 测量误差 rk.R *= 0.001 rk.P *= 1 rk.P[6, 6] = 0.0 rk.P[7, 7] = 0.0 rk.P[8, 8] = 0.00 rk.Q *= 0.001 rk.Q[6, 6] = 0.0 rk.Q[7, 7] = 0.0 rk.Q[8, 8] = 0.0 return rk
def __init__(self): self.wx = 100 self.wy = 0 self.wz = 0 self.q1 = 0 self.q2 = 1 self.q3 = 0 self.q4 = 0 self.rk = ExtendedKalmanFilter(dim_x=7, dim_z=2)
def test_initialization_wrong_gamma(self): with self.assertRaises(ValueError): filter = ExtendedKalmanFilter(dim_x = 9, dim_z = 6) gamma = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) attacker = PeriodAttacker(filter = self.filter, radar = self.radar, radar_pos = self.radar_pos, gamma = gamma,mag_vector = self.mag_vector, t0 = self.t0, time = self.time)
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 get_rk(self): self.rk = ExtendedKalmanFilter(dim_x=3, dim_z=1) dt = 0.01 self.rk.F = asarray([[1., dt, dt * dt / 2], [0., 1., dt], [0., 0., 1.]]) # 初始位置 先设置为0 # self.rk.x = array([0, 0, 0.2]).T self.rk.x = self.initialState # 测量误差 self.rk.R *= 0.01 self.rk.P *= 10 self.rk.Q *= 0.001 return self.rk
def setUp(self): # Simulated filter for 2 radars (2*3 measurements) self.filter = ExtendedKalmanFilter(dim_x = 9, dim_z = 6) self.gamma = np.eye(3) self.mag_vector = np.array([[-10, -10, -10]]).T self.t0 = 10 self.time = 50 self.radar = Radar(x=10,y=10) self.radar_pos = 1 self.attacker = PeriodAttacker(filter = self.filter, radar = self.radar, radar_pos = self.radar_pos, gamma = self.gamma,mag_vector = self.mag_vector, t0 = self.t0, time = self.time)
def __init__(self, starting_value): self._filter = ExtendedKalmanFilter(dim_x=2, dim_z=1) self._filter.x = np.array([starting_value, starting_value]) self._filter.F = np.array([[1.,1.], [0.,1.]]) self._filter.H = np.array([[1.,0.]]) self._filter.P = np.array([[1000., 0.], [ 0., 1000.] ]) self._filter.R = np.array([[5.]]) self._filter.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13) self.inc = 0 self.last_val = starting_value self.first = True
def __init__(self): self.rk = ExtendedKalmanFilter(dim_x=7, dim_z=2) self.wx = 0 self.wy = 0 self.wz = 265 self.q1 = 1 self.q2 = 0 self.q3 = 0 self.q4 = 0 #range_std = 5000 self.rk.R = np.diag([300,500]) self.rk.Q = np.diag([50,50,50,50,0.1,0.1,0.1]) self.rk.P *= 50 self.rk.x = np.array([[self.q1,self.q2,self.q3,self.q4,self.wx,self.wy,self.wz]]).T
def initFilter(app): app.f = ExtendedKalmanFilter(dim_x=2, dim_z=len(ANCHORS) - 1) # The estimate app.f.x = np.array([[1.], [1.]]) # State transition matrix, just use last position for now app.f.F = np.array([[1., 0.], [0., 1.]]) # Measurement function, values from linear localization app.f.P = np.eye( 2) * 1000 # Covaraince, large number since our initial guess is (1, 1) app.f.R = np.eye( len(ANCHORS) - 1 ) * 50 # Measurement uncertainty, making it low since we want to weight sensor data app.f.Q = np.eye( 2 ) * 1 # System noise, making it big since we're just using the last location as our prediction
def get_rk(): global rk rk = ExtendedKalmanFilter(dim_x=3, dim_z=1) dt = 0.01 rk.F = asarray([[1., dt, dt * dt / 2], [0., 1., dt], [0., 0., 1.], ]) # 初始位置 先设置为0 rk.x = array([0, 0, 0]).T # 测量误差 rk.R *= 0.001 rk.P *= 0.1 rk.Q *= 0.001 return rk
def __init__(self): if (VELOCITY): self.f = ExtendedKalmanFilter(dim_x=4, dim_z=(len(ANCHORS) - 1 + 2)) # The estimate self.f.x = np.array([[1.], [1.], [0.], [0.]]) # State transition matrix, just use last position for now self.f.F = np.array([[1., 0., 0.12, 0.], [0., 1., 0., 0.12], [0., 0., 1., 0.], [0., 0., 0., 1.]]) # Measurement function, values from linear localization self.f.P = np.eye( 4 ) * P # Covaraince, large number since our initial guess is (1, 1) self.f.R = np.eye( len(ANCHORS) - 1 + 2 ) * R # Measurement uncertainty, making it high since noisy data self.f.Q = np.eye( 4 ) * Q # System noise, making it small since we're using a velocity estimation else: self.f = ExtendedKalmanFilter(dim_x=2, dim_z=len(ANCHORS) - 1) # The estimate self.f.x = np.array([[1.], [1.]]) # State transition matrix, just use last position for now self.f.F = np.array([[1., 0.], [0., 1.]]) # Measurement function, values from linear localization self.f.P = np.eye( 2 ) * P # Covaraince, large number since our initial guess is (1, 1) self.f.R = np.eye( len(ANCHORS) - 1 ) * R # Measurement uncertainty, making it low since we want to weight sensor data self.f.Q = np.eye( 2 ) * Q # System noise, making it big since we're just using the last location as our prediction
def __init__(self, path): dt = 0.05 self.rk = ExtendedKalmanFilter(dim_x=2, dim_z=1) # radar = RadarSim(dt, pos=0., vel=100., alt=1000.) #self.detector = PlanetDetector('C:\\Users\\Phuc Dang\\Desktop\\Developer\\Collaborative-Sensing\\model\\cascade.xml') self.detector = PlanetDetector(path) # make an imperfect starting guess # rk.x = array([radar.pos - 100, radar.vel + 100, radar.alt + 1000]) self.rk.x = array([0, .273]) # need better initial location self.rk.F = eye(2) + array([[0, 0], [1, 0]]) * dt range_std = 5. # meters self.rk.R = np.diag([range_std**2]) self.rk.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1) self.rk.Q[1, 1] = 0.1 self.rk.P *= 50
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 get_rk(): global rk global rotating_model_jacobian rotating_model_jacobian = Rotating_Model_Jacobian() rk = ExtendedKalmanFilter(dim_x=7, dim_z=3) dt = 0.01 rk.F = asarray([[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., 0., 0., 0.], [0., 0., 0., 0., 1., dt, dt * dt / 2], [0., 0., 0., 0., 0., 1., dt], [0., 0., 0., 0., 0., 0., 1.]]) # 初始位置 先设置为0 rk.x = array([1, 1, 1, 0.4, 0, 1, 1]).T # 测量误差 rk.R *= 0.001 rk.P *= 1 rk.Q *= 0.001 return rk
def setUp(self): # Simulated filter for 2 radars (2*3 measurements) self.filter = ExtendedKalmanFilter(dim_x=9, dim_z=6) # Attack matrix: second radar is compromised self.gamma = np.array([[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]]) self.mag_vector = np.array([[0, 0, 0, -10, -10, -10]]).T self.t0 = 10 self.time = 50 self.radar = Radar(x=10, y=10) self.attacker = Attacker(filter=self.filter, radar=self.radar, gamma=self.gamma, mag_vector=self.mag_vector, t0=self.t0, time=self.time)
def __init__(self, K, dim_x=3, dim_z=3, lamb=2, sigma=2, dt=0.05): #Init Kalman filter self.rk = ExtendedKalmanFilter(dim_x=dim_x, dim_z=dim_z) self.rk.x = np.array([1, 1, 1]) self.sigma = sigma self.lamb = lamb self.K = K self.rho = 28.0 self.sigma_lorenz = 10.0 self.beta = 8.0 / 3.0 self.dt = dt self.rk.F = self.get_F(self.rk.x) self.rk.R = np.diag([lamb**2]*3) self.rk.Q = self.get_Q(dt=self.dt) self.rk.H = np.identity(3, dtype=np.float32) self.rk.P *= 3
def test_saver_ekf(): def HJ(x): return np.array([[1, 1]]) def hx(x): return np.array([x[0]]) kf = ExtendedKalmanFilter(2, 1) s = Saver(kf) for i in range(3): kf.predict() kf.update([[i]], HJ, hx) s.save() # this will assert if the KalmanFilter did not properly assert s.to_array() assert len(s.x) == 3 assert len(s.y) == 3 assert len(s.K) == 3
def build_ekf(coeffs, z_data, linear_consts=None, nmsmt=n_msmt, dx=dimx): global n_msmt global dimx (dimx, n_msmt) = (dx, nmsmt) ekf = ExtendedKalmanFilter(dim_x=dimx, dim_z=n_msmt) #ekf.x = zeros([dimx, 1]) #ekf.__init__(dimx, n_msmt) if len(coeffs): coeffs = array(coeffs).flatten() if n_coeff == dimx * 2 + n_msmt: ekf.Q = symmetric(array(coeffs[0:dimx])) ekf.F = symmetric(array(coeffs[dimx:dimx * 2])) r = symmetric(array(coeffs[-n_msmt:])) else: ekf.Q = read2d(coeffs, dimx, 0, dimx * dimx) ekf.F = read2d(coeffs, dimx, dimx * dimx, dimx * dimx * 2) r = read2d(coeffs, n_msmt, -n_msmt * n_msmt, n_coeff) logger.info("ekf.Q=" + str(ekf.Q) + ", ekf.F = " + str(ekf.F) + ", r = " + str(r)) return update_ekf(ekf, z_data, r, linear_consts) return update_ekf(ekf, z_data)
def setup(): # set up sensor simulator F = np.array([[1, dt, 0], [0, 1, 0], [0, 0, 1]]) def f(x): return np.dot(F, x) x0 = np.array([[-5], [0.5], [1]]) sim = Sensor(x0, f, h) # set up kalman filter tracker = ExtendedKalmanFilter(dim_x=3, dim_z=2) tracker.F = F q_x = Q_discrete_white_noise(dim=2, dt=dt, var=0.001) q_y = 0.0001 * dt**2 tracker.Q = block_diag(q_x, q_y) tracker.R = R_proto * measurement_var_max * filter_misestimation_factor tracker.x = np.array([[-5, 0.5, 1]]).T tracker.P = np.eye(3) * 500 return sim, tracker
def __init__(self, planetDetector, FIFO_FILENAME, OTHER_FIFO): # Haar classifier self.detector = planetDetector # EKF model self.rk = ExtendedKalmanFilter(dim_x=2, dim_z=1) self.initialize_rk() # FIFO self.writeFiFo = os.open(FIFO_FILENAME, os.O_WRONLY) self.readFiFo = os.open(OTHER_FIFO, os.O_RDONLY) if FIFO_FILENAME == "srv_transmit": self.server = True else: self.server = False #Merge values self.omega = self.rk.x[0] self.omega_hat = self.rk.x[1] #Client sensor values self.omega_client = self.rk.x[0] self.omega_hat_client = self.rk.x[1]
def kalman_filter(y, fs): r_locs = detect_qrs(y, fs) r_vals = y[r_locs] r_val_mean = r_vals.mean() y_truncated = y[r_locs[0]:(r_locs[-1] + 1)] theta_value = 0 theta_init = np.array([-np.pi / 3, -np.pi / 12, 0, np.pi / 12, np.pi / 2]) a_init = np.array([1.2, -5.0, 30.0, -7.5, 0.75]) b1_init = np.array([0.25, 0.1, 0.1, 0.1, 0.4]) b2_init = np.array([0.25, 0.1, 0.1, 0.1, 0.4]) a_init = a_init * r_val_mean / 30 x_init = np.concatenate( [a_init, theta_init, b1_init, b2_init, np.array([y_truncated[0]])]) val_init = np.concatenate( [a_init, theta_init, b1_init, b2_init, np.array([theta_value])]) d_theta = 0.005 * 2 * np.pi rk = ExtendedKalmanFilter(dim_x=21, dim_z=1) rk.x = np.array([x_init]).T rk.F = eye(21) rk.R = np.diag([0.1]) rk.Q = np.diag([0.1 for _ in range(21)]) val_v = val_init xs = [] for i in tqdm(range(len(y_truncated))): z = y_truncated[i] for j in range(20): rk.F[20, j] = eval_var(ag_d_theta_var_d_params[j], val_v) rk.update(array([[z]]), HJacobian_at, hx) xs.append(rk.x) rk.predict() val_v[20] += d_theta if val_v[20] > np.pi: val_v[20] -= 2 * np.pi val_v[:20] = rk.x[0, :20] return np.array(xs)[:, 20, 0]
# d0 = 10 def HJacobian_at(x): angular_vel = x[1] angular_pos = x[0] return array([[0., (-1)*np.sin(angular_pos)*d0]]) def get_pixel_between_sun_and_planet(x): return d0 * cos(x[0]) dt = 0.05 rk = ExtendedKalmanFilter(dim_x=2, dim_z=1) #radar = RadarSim(dt, pos=0., vel=100., alt=1000.) detector = PlanetDetector('../old_models/model_4/cascade.xml', False, False) # make an imperfect starting guess #rk.x = array([radar.pos - 100, radar.vel + 100, radar.alt + 1000]) rk.x = array([0, 0.237]) #need better initial location rk.F = eye(2) + array([[0, 0], [1, 0]]) * dt range_std = 5. # meters bs = np.diag([range_std ** 2]) print(bs)
def __init__(self): self.ekf = ExtendedKalmanFilter(dim_x=6, dim_z=3) self.set_pose(np.zeros(6)) self.dt = 1
def eval_segpose_icp_kalman_predictions(image_ids=None, object_names=None, selected_noise=param.selected_noise): if image_ids is None: image_ids = param.selected_linemod_image_ids if object_names is None: object_names = list(param.selected_linemod_objects.keys()) model_clouds = io.load_clouds_from_selected_models(True, object_names) ds_avg = np.zeros((len(object_names), len(image_ids), 5)) ds_std = np.zeros((len(object_names), len(image_ids), 5)) images = [] depths = [] for i in image_ids: image, depth = io.load_rgb_and_d(i,selected_noise=selected_noise) images.append(image) depths.append(depth) # First compute the image to image transformations intrinsic = io.load_camera_intrinsics_open3d().intrinsic_matrix ans = optical_flow.compute_tracked_features_and_tranformation( images, depths, intrinsic) image_RTs = ans[:2] err = ans[2] def hx(x,image_RT): x_shape = x.shape x = np.ndarray.flatten(x) RT_x = np.eye(4) RT_x[:3, :3] = R.from_euler('xyz', x[3:]).as_matrix() RT_x[:3, -1] = x[:3] x_pred = image_RT @ RT_x # Convert back to euler euler_angles = R.from_matrix(x_pred[:3, :3]).as_euler('xyz') state = np.hstack([x_pred[:3, -1], euler_angles]) return np.reshape(state, x_shape) def H(x): return np.eye(6) # Set up an Extended Kalman filter. Assume we are observing the 6D pose. ekf = ExtendedKalmanFilter(6, 6) # xyz, euler prev_RT_kalman_input = None for obj_i, object_name in enumerate(object_names): for img_i, image_id in enumerate(image_ids): scene_cloud = io.load_cloud_from_selected_image_id(image_id, selected_noise=selected_noise, intrinsics=intrinsic) RT_gt = np.eye(4) RT_segpose = np.eye(4) RT_gt[:3,:] = io.load_ground_truth_pose(object_name,image_id) RT_segpose[:3,:] = io.load_segpose_prediction(object_name,image_id, selected_noise=selected_noise) # compute ICP using segpose prediction as the seed threshold = 0.1 reg_p2p = o3d.pipelines.registration.registration_icp( model_clouds[object_name], scene_cloud, threshold, RT_segpose) RT_est = reg_p2p.transformation # update Kalman filter RT_kalman_input = RT_segpose image_transformation = np.eye(4) if img_i > 0: image_transformation[:3, :3] = image_RTs[0][img_i-1] image_transformation[:3, -1] = image_RTs[1][img_i-1] if prev_RT_kalman_input is None: euler_angles = R.from_matrix(RT_kalman_input[:3, :3]).as_euler('xyz') state_observed = np.hstack([RT_kalman_input[:3, -1], euler_angles]) ekf.predict_update(np.reshape(state_observed, (-1, 1)), H, hx, hx_args=(np.eye(4),)) # Do EKF as euler angles # Compute the estimation euler_angles = R.from_matrix(RT_kalman_input[:3,:3]).as_euler('xyz') state_observed = np.hstack([RT_kalman_input[:3,-1], euler_angles]) ekf.predict_update(np.reshape(state_observed,(-1,1)), H, hx, hx_args=(image_transformation,)) state_post = np.ndarray.flatten(ekf.x_post) RT_kalman = np.eye(4) RT_kalman[:3,:3] = R.from_euler('xyz',state_post[3:]).as_matrix() RT_kalman[:3,-1] = state_post[:3] state_prior = np.ndarray.flatten(ekf.x_prior) RT_kalman_prior = np.eye(4) RT_kalman_prior[:3,:3] = R.from_euler('xyz',state_prior[3:]).as_matrix() RT_kalman_prior[:3,-1] = state_prior[:3] reg_kalman = o3d.pipelines.registration.registration_icp( model_clouds[object_name], scene_cloud, threshold, RT_kalman) RT_kalman_icp = reg_kalman.transformation # Memoize the previous RT estimation from point cloud & segpose prev_RT_kalman_input = np.copy(RT_kalman_input) model_cloud_gt = copy.copy(model_clouds[object_name]) model_cloud_gt.transform(RT_gt) model_cloud_seg = copy.copy(model_clouds[object_name]) model_cloud_seg.transform(RT_segpose) model_cloud_est = copy.copy(model_clouds[object_name]) model_cloud_est.transform(RT_est) model_cloud_kalman = copy.copy(model_clouds[object_name]) model_cloud_kalman.transform(RT_kalman) model_cloud_kalman_icp = copy.copy(model_clouds[object_name]) model_cloud_kalman_icp.transform(RT_kalman_icp) model_cloud_kalman_prior = copy.copy(model_clouds[object_name]) model_cloud_kalman_prior.transform(RT_kalman_prior) print(img_i) for cloud_i, cloud in enumerate([model_cloud_seg, model_cloud_est, model_cloud_kalman, model_cloud_kalman_icp]): d_current = np.asarray(model_cloud_gt.compute_point_cloud_distance(cloud)) ds_avg[obj_i, img_i, cloud_i] = np.average(d_current) ds_std[obj_i, img_i, cloud_i] = np.std(d_current) # visualize.draw_point_clouds([model_cloud_gt, model_cloud_seg, # model_cloud_est, model_cloud_kalman, # model_cloud_kalman_icp, # scene_cloud], # [[1,0,0],[0,1,0],[0,0,1], # [1,1,0],[1,0,1]]) #R: GT, G: Seg, B: Seg+ICP, Y:Kalman, P: Kalman+ICP return ds_avg, ds_std
return array(jacobian) def hx(x): ranges = [] est_x = x.item(0) est_y = x.item(1) for s in radar.sensors: ranges.append([sqrt((est_x - s[0])**2 + (est_y - s[1])**2)]) return ranges radar = RadarNet() ekf = ExtendedKalmanFilter(dim_x=2, dim_z=3) ekf.x = array([[1.0], [1.0]]) # ekf.x = array([[radar.pos, radar.vel+100, radar.alt+1000]]).T # ekf.x = array([[0.0, 0.0, 0.0]]).T ekf.F = eye(2) # ekf.R = radar.target[1] * 0.05 ekf.R *= 10 ekf.Q *= 0.0001 ekf.P *= 50 truth_x = [] truth_y = [] estimated_x = []
return slant_dist + err def HJacobian_at (x): horiz_dist = x[0,0] altitude = x[2,0] denom = sqrt(horiz_dist**2 + altitude**2) return array([[horiz_dist/denom, 0.0, altitude/denom]]) def hx (x): return (x[0,0]**2 + x[2,0]**2)**0.5 dt = 0.05 flight = Flight (dt, pos = 0.0, vel = 100.0, alt = 1000.0) ekf = ExtendedKalmanFilter (dim_x = 3, dim_z = 1) ekf.x = array([[flight.pos, flight.vel+100, flight.alt+1000]]).T # ekf.x = array([[flight.pos, flight.vel+100, flight.alt+1000]]).T # ekf.x = array([[0.0, 0.0, 0.0]]).T ekf.F = eye(3) + array([[0,1,0], [0,0,0], [0,0,0]])*dt ekf.R = flight.alt * 0.05 ekf.Q = array ([[0,0,0], [0,1,0], [0,0,1]]) * 0.001 ekf.P *= 50 truth_x = [] truth_y = [] estimated_x = [] estimated_y = []
# Functionality : # ################################### from scipy.io import loadmat from numpy import eye, array import numpy as np import matplotlib.pyplot as plt import pandas as pd from filterpy.kalman import ExtendedKalmanFilter noisy_data = loadmat('l10_y.mat')['y'] clean_data = loadmat('l10_x_true.mat')['x_true'] noisy_data.shape EKF = ExtendedKalmanFilter(1, 1) EKF.x = 0 dt = 0.05 EKF.F = eye(3) + array([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) * dt EKF.R def HJacobian_at(x): new_signal = (1.9999 * x[i - 1]) + (-1 * x[i - 2]) return new_signal #EKF.y = noisy_data #print(EKF.x_post) #something = EKF.update(None,'HJacobian',clean_data)
def __init__(self, cameras, keypoints, R_std, Q_var): #Variables: self.camera_number = cameras # number ob cameras self.state_dim = 6 # per keypoint self.measurement_dim = 2 * self.camera_number # per keypoint self.keypoint_number = keypoints # COCO model self.frame_number = 0 # actual interation number of the filter self.R_std = R_std**2 # measurement noise: before 1.0 self.Q_var = Q_var**2 # process/system noise: before 5.0 # state dimension: [x,y,z,x',y',z'] for 18 keypoints self.x_dimension = self.state_dim * self.keypoint_number # 108 # measurement dimension: [x0, y0, x1, y1, x2,..., x_j, y_j] screen coord. for all j cameras for 18 keypoints self.z_dimension = self.measurement_dim * self.keypoint_number # 180 self.dt = 0.04 # time steps: 1/25 FPS #Build State Vector X: [x,y,z,x',y',z'] for each keypoint X = np.zeros((self.x_dimension, 1)) #Create the actual filter now: ekf = ExtendedKalmanFilter(dim_x=self.x_dimension, dim_z=self.z_dimension) # state vector ekf.x = X # state covariance #ekf.P = np.zeros((self.x_dimension,self.x_dimension)) ekf.P = np.eye(self.x_dimension) * 20 # Process Model # Build Transition Matrix F or also called A block = np.matrix([[1., 0., 0., self.dt, 0., 0.], [0., 1., 0., 0., self.dt, 0.], [0., 0., 1., 0., 0., self.dt], [0., 0., 0., 1., 0., 0.], [0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 1.]]) matrix_list = [] for i in range(self.keypoint_number): matrix_list.append(block) F = scipy.linalg.block_diag(*matrix_list) ekf.F = F # measurement noise ekf.R = np.eye(self.z_dimension) * (self.R_std) # process noise # TODO: which noise model should be used? # q = Q_discrete_white_noise(dim=2, dt=self.dt, var=self.Q_var) # block = np.matrix([[q[0,0], 0., 0., q[0,1], 0., 0.], # [0., q[0,0], 0., 0., q[0,1], 0.], # [0., 0., q[0,0], 0., 0., q[0,1]], # [q[1,0], 0., 0., q[1,1], 0., 0.], # [0., q[1,0], 0., 0., q[1,1], 0.], # [0., 0., q[1,0], 0., 0., q[1,1]]]) # matrix_list = [] # for i in range (self.keypoint_number): # matrix_list.append(block) # ekf.Q = scipy.linalg.block_diag(*matrix_list) ekf.Q = np.eye(self.x_dimension) * (self.Q_var) self.filter = ekf
def extended_kalman_filter(): from filterpy.kalman import ExtendedKalmanFilter data_array = np.load('/media/samsumg_1tb/synthia/prepared_data_shuffle.npy') test_data_array = data_array[2] data_mean = data_array[3] data_std = data_array[4] test_data_array = (test_data_array * data_std) + data_mean print('Finish Loading. Test array shape: ' + str(test_data_array.shape)) test_pred = np.zeros(shape=(len(test_data_array), 8, 6)) test_pred[:, :, 4:] = test_data_array[:, -8:, 4:] def HJacobian_at(x): """ compute Jacobian of H matrix at x """ return np.array([[0, 1., 0, 1]]) def hx(x): """ compute measurement for slant range that would correspond to state x. """ return x for f, data in enumerate(test_data_array): if f % 100 == 0: print("process batch %d" %f) measurements = [[d[0], d[1]] for d in data[:15]] ekf = ExtendedKalmanFilter(dim_x=4, dim_z=2) ekf.x = np.array([measurements[0][0], measurements[0][1], 0, 0]) ekf.F = np.array([[1., 0., 1., 0.], [0., 1., 0., 1.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) # Define the measurement function: ekf.H = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.]]) # Define the covariance matrix. Here I take advantage of the fact that P already contains np.eye(dim_x), # and just multiply by the uncertainty: ekf.P *= 1 for t in range(1, 15): ekf.predict() ekf.update([measurements[t][0], measurements[t][1]], HJacobian_at, hx) for t in range(8): ekf.predict() next_x = ekf.x test_pred[f, t, 0] = next_x[0] test_pred[f, t, 1] = next_x[1] test_pred[f, t, 2] = test_data_array[f, 14, 2] # we set the width to be fixed test_pred[f, t, 3] = test_data_array[f, 14, 3] # we set the width to be fixed totalerrCoverage = 0 totalerrCenter = 0 Kalman_valid_errCenter = [] Kalman_valid_iou_2d = [] for i in range(len(test_pred)): res = test_pred[i] anno = test_data_array[i, -8:, :] center = [[r[0], r[1]] for r in res] centerGT = [[r[0], r[1]] for r in anno] seq_length = len(centerGT) errCenter = [ssd_2d(center[i], centerGT[i]) for i in range(len(centerGT))] Kalman_valid_errCenter.append(errCenter) iou_2d = calc_rect_int_2d(res, anno) Kalman_valid_iou_2d.append(iou_2d) for s in range(seq_length): totalerrCenter += errCenter[s] totalerrCoverage += iou_2d[s] aveErrCoverage = totalerrCoverage / (len(Kalman_valid_errCenter) * float(seq_length)) aveErrCenter = totalerrCenter / (len(Kalman_valid_errCenter) * float(seq_length)) print('aveErrCoverage: %.4f, aveErrCenter: %.2f ' %(aveErrCoverage, aveErrCenter)) # P: aveErrCoverage: 0.6174, aveErrCenter: 31.11 result_dir = '/media/samsumg_1tb/cvpr_DTA_Results/kalman_filter' np.save(os.path.join(result_dir, 'Kalman_valid_errCenter.npy'), Kalman_valid_errCenter) np.save(os.path.join(result_dir, 'Kalman_valid_iou_2d.npy'), Kalman_valid_iou_2d)
def test_ekf(): def H_of(x): """ compute Jacobian of H matrix for state x """ horiz_dist = x[0] altitude = x[2] denom = sqrt(horiz_dist**2 + altitude**2) return array([[horiz_dist / denom, 0., altitude / denom]]) def hx(x): """ takes a state variable and returns the measurement that would correspond to that state. """ return sqrt(x[0]**2 + x[2]**2) dt = 0.05 proccess_error = 0.05 rk = ExtendedKalmanFilter(dim_x=3, dim_z=1) rk.F = eye(3) + array([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) * dt def fx(x, dt): return np.dot(rk.F, x) rk.x = array([-10., 90., 1100.]) rk.R *= 10 rk.Q = array([[0, 0, 0], [0, 1, 0], [0, 0, 1]]) * 0.001 rk.P *= 50 rs = [] xs = [] radar = RadarSim(dt) ps = [] pos = [] s = Saver(rk) for i in range(int(20 / dt)): z = radar.get_range(proccess_error) pos.append(radar.pos) rk.update(asarray([z]), H_of, hx, R=hx(rk.x) * proccess_error) ps.append(rk.P) rk.predict() xs.append(rk.x) rs.append(z) s.save() # test mahalanobis a = np.zeros(rk.y.shape) maha = scipy_mahalanobis(a, rk.y, rk.SI) assert rk.mahalanobis == approx(maha) s.to_array() xs = asarray(xs) ps = asarray(ps) rs = asarray(rs) p_pos = ps[:, 0, 0] p_vel = ps[:, 1, 1] p_alt = ps[:, 2, 2] pos = asarray(pos) if DO_PLOT: plt.subplot(311) plt.plot(xs[:, 0]) plt.ylabel('position') plt.subplot(312) plt.plot(xs[:, 1]) plt.ylabel('velocity') plt.subplot(313) #plt.plot(xs[:,2]) #plt.ylabel('altitude') plt.plot(p_pos) plt.plot(-p_pos) plt.plot(xs[:, 0] - pos)