def test_saver_kf(): kf = kinematic_kf(3, 2, dt=0.1, dim_z=3) s = Saver(kf) for i in range(10): kf.predict() kf.update([i, i, i]) s.save() # this will assert if the KalmanFilter did not properly assert s.to_array() assert len(s.x) == 10 assert len(s.y) == 10 assert len(s.K) == 10 assert (len(s) == len(s.x)) # Force an exception to occur my malforming K kf = kinematic_kf(3, 2, dt=0.1, dim_z=3) kf.K = 0. s2 = Saver(kf) for i in range(10): kf.predict() kf.update([i, i, i]) s2.save() # this should raise an exception because K is malformed try: s2.to_array() assert False, "Should not have been able to convert s.K into an array" except: pass
def test_kinematic_filter(): global kf # make sure the default matrices are shaped correctly for dim_x in range(1, 4): for order in range(0, 3): kf = kinematic_kf(dim=dim_x, order=order) kf.predict() kf.update(np.zeros((dim_x, 1))) # H is tricky, make sure it is shaped and assigned correctly kf = kinematic_kf(dim=2, order=2) assert kf.x.shape == (6, 1) assert kf.F.shape == (6, 6) assert kf.P.shape == (6, 6) assert kf.Q.shape == (6, 6) assert kf.R.shape == (2, 2) assert kf.H.shape == (2, 6) H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]], dtype=float) assert np.array_equal(H, kf.H) kf = kinematic_kf(dim=3, order=2, order_by_dim=False) H = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0]], dtype=float) assert np.array_equal(H, kf.H)
def test_kinematic_filter(): global kf # make sure the default matrices are shaped correctly for dim_x in range(1,4): for order in range (0, 3): kf = kinematic_kf(dim=dim_x, order=order) kf.predict() kf.update(np.zeros((dim_x, 1))) # H is tricky, make sure it is shaped and assigned correctly kf = kinematic_kf(dim=2, order=2) assert kf.x.shape == (6, 1) assert kf.F.shape == (6, 6) assert kf.P.shape == (6, 6) assert kf.Q.shape == (6, 6) assert kf.R.shape == (2, 2) assert kf.H.shape == (2, 6) H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]], dtype=float) assert np.array_equal(H, kf.H) kf = kinematic_kf(dim=3, order=2, order_by_dim=False) H = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0]], dtype=float) assert np.array_equal(H, kf.H)
def test_saver_kf(): kf = kinematic_kf(3, 2, dt=0.1, dim_z=3) s = Saver(kf) for i in range(10): kf.predict() kf.update([i, i, i]) s.save() # this will assert if the KalmanFilter did not properly assert s.to_array() assert len(s.x) == 10 assert len(s.y) == 10 assert len(s.K) == 10 assert (len(s) == len(s.x)) # Force an exception to occur my malforming K kf = kinematic_kf(3, 2, dt=0.1, dim_z=3) kf.K = 0. s2 = Saver(kf) for i in range(10): kf.predict() kf.update([i, i, i]) s2.save() # this should raise an exception because K is malformed try: s2.to_array() assert False, "Should not have been able to convert s.K into an array" except: pass
def setUp(self): self.kinematic_test_kf = kinematic_kf(dim=1,order=1,dt=1) x = [0.,2.] self.zs = [x[0]] self.pos = [x[0]] noise_std = 1. # Noisy measurements generation for 30 samples for _ in range(30): last_pos = x[0] last_vel = x[1] new_vel = last_vel new_pos = last_pos + last_vel x = [new_pos, new_vel] z = new_pos + (randn()*noise_std) self.zs.append(z) # Outlier generation self.zs[5] += 10. self.zs[10] += 10. self.zs[15] += 10. self.zs[20] += 10. self.zs[25] += 10. self.detector = MahalanobisDetector()
def __init__(self, dim, dim_z): self.kf = kinematic_kf(dim, 1, dim_z=dim_z, order_by_dim=False,) self.id = self.__class__.n_trackers self.__class__.n_trackers += 1 self.time_since_update = 0 self.age = 0 self.hits = 0 self.hit_streak = 0
def test_kinematic_filter(): # make sure the default matrices are shaped correctly for dim_z in range(1, 4): for dim_x in range(1, 4): for order in range(0, 3): kf = kinematic_kf(dim=dim_x, order=order, dim_z=dim_z) kf.predict() kf.update(np.zeros((dim_z, 1))) kf = kinematic_kf(dim=dim_x, order=order, dim_z=dim_z, order_by_dim=False) kf.predict() kf.update(np.zeros((dim_z, 1))) # H is tricky, make sure it is shaped and assigned correctly kf = kinematic_kf(dim=2, order=2) assert kf.x.shape == (6, 1) assert kf.F.shape == (6, 6) assert kf.P.shape == (6, 6) assert kf.Q.shape == (6, 6) assert kf.R.shape == (1, 1) assert kf.H.shape == (1, 6) H = np.array([[1., 0, 0, 1, 0, 0]]) assert np.array_equal(H, kf.H), "H is {}, should be {}".format(kf.H, H) kf = kinematic_kf(dim=2, order=2, order_by_dim=False) assert kf.x.shape == (6, 1) assert kf.F.shape == (6, 6) assert kf.P.shape == (6, 6) assert kf.Q.shape == (6, 6) assert kf.R.shape == (1, 1) assert kf.H.shape == (1, 6) H = np.array([[1., 1, 0, 0, 0, 0]]) assert np.array_equal(H, kf.H), "H is\n{}\nshould be\n{}".format(kf.H, H) kf = kinematic_kf(dim=1, order=2, dim_z=3, order_by_dim=False) H = np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0.]]) assert np.array_equal(H, kf.H), "H is\n{}\nshould be\n{}".format(kf.H, H)
def __init__(self, n_bodyparts): # TODO Try particle filter (since we already have the keypoints) self.kf = kinematic_kf( n_bodyparts * 2, order=1, dim_z=n_bodyparts, order_by_dim=False ) self.kf.Q[self.kf.dim_z :, self.kf.dim_z :] *= 10 self.kf.R[self.kf.dim_z :, self.kf.dim_z :] *= 0.01 self.kf.P[self.kf.dim_z :, self.kf.dim_z :] *= 1000 self.id = SkeletonTracker.n_trackers SkeletonTracker.n_trackers += 1 self.time_since_update = 0 self.age = 0 self.hits = 0 self.hit_streak = 0
def test_steadystate(): dim = 7 cv = kinematic_kf(dim=dim, order=5) cv.x[1] = 1.0 for i in range(100): cv.predict() cv.update([i] * dim) for i in range(100): cv.predict_steadystate() cv.update_steadystate([i] * dim)
def __init__(self, params): self.kf = kinematic_kf(5, order=1, dim_z=5, order_by_dim=False) self.kf.R[2:, 2:] *= 10.0 self.kf.P[ 5:, 5: ] *= 1000.0 # High uncertainty to the unobservable initial velocities self.kf.P *= 10.0 self.kf.Q[5:, 5:] *= 0.01 self.state = params self.time_since_update = 0 self.id = EllipseTracker.n_trackers EllipseTracker.n_trackers += 1 self.hits = 0 self.hit_streak = 0 self.age = 0
def __init__(self, trackId): super(BeeTrack, self).__init__() ##! Name that gets shown on the screen for that bee self._name = "" ## Last detected bee position self._last_dectect = None ## The tracks ID self.trackId = trackId # Initialize the klaman filter self.dt = 1 self.KF = kinematic_kf(dim=2, order=2, dt=self.dt, dim_z=1, order_by_dim=True) self.KF.R *= 2 self.KF.Q = np.array( [[self.dt**4 / 4, self.dt**3 / 2, self.dt**4 / 2, 0, 0, 0], [self.dt**3 / 2, self.dt**2, self.dt**4, 0, 0, 0], [self.dt**3 / 1, self.dt**1, self.dt**1 / 2, 0, 0, 0], [0, 0, 0, self.dt**4 / 4, self.dt**3 / 2, self.dt**4 / 2], [0, 0, 0, self.dt**3 / 2, self.dt**2, self.dt**4], [0, 0, 0, self.dt**3 / 1, self.dt**1, self.dt**1 / 2]]) # Keep track of the self.trace = deque(maxlen=get_config("MAX_BEE_TRACE_LENGTH")) # Amount of missed detetions self.skipped_frames = 0 # Amount of frames processed with this track self.processed_frames = 0 # A set if determined track/bee characteristics self.tags = set() self.reported_tags = set() # The first detection which created this track self.first_position = None # Whether the track is underneath of a group of bees self.in_group = False self.__tagCnts = {}
def test_steadystate(): dim = 7 cv = kinematic_kf(dim=dim, order=5) cv.x[1] = 1.0 for i in range(100): cv.predict() cv.update([i]*dim) for i in range(100): cv.predict_steadystate() cv.update_steadystate([i]*dim) # test mahalanobis a = np.zeros(cv.y.shape) maha = scipy_mahalanobis(a, cv.y, cv.SI) assert cv.mahalanobis == approx(maha)
def rtsmooth(measurements, dt=0.02, order=2): """ Args: measurements (np.array): (time, measurements_dim) Returns: data (np.array): (time, measurements_dim) """ measure_dim = measurements.shape[1] kf = kinematic_kf(dim=measure_dim, order=order, dt=dt) # print(kf.F[:3, :3]) # State transition # kf.P is ordered with [2, 2] or [3, 3] blocks for each dimension # (2 if 1st order - constant velocity, 3 if 2nd order - constant acceleration) kf.P[::order + 1, ::order + 1] *= 1 kf.P *= 10 kf.Q[::order + 1, ::order + 1] *= 1 mu, cov, _, _ = kf.batch_filter(npt.numpify(measurements)) smoothed, _, _, _ = kf.rts_smoother(mu, cov) print(smoothed.shape) return smoothed[:, ::order + 1, 0]
def test_steadystate(): dim = 7 cv = kinematic_kf(dim=dim, order=5) cv.x[1] = 1.0 for i in range(100): cv.predict() cv.update([i] * dim) for i in range(100): cv.predict_steadystate() cv.update_steadystate([i] * dim) # test mahalanobis a = np.zeros(cv.y.shape) maha = scipy_mahalanobis(a, cv.y, cv.SI) assert cv.mahalanobis == approx(maha)
def test_kinematic(): kf = kinematic_kf(1,1)
import numpy as np from filterpy.common import kinematic_kf kf1 = kinematic_kf(2, 2) kf2 = kinematic_kf(2, 2) # do some settings of x, R, P etc. here, I'll just use the defaults kf2.Q *= 0 # no prediction error in second filter filters = [kf1, kf2] mu = [0.5, 0.5] # each filter is equally likely at the start trans = np.array([[0.97, 0.03], [0.03, 0.97]]) imm = IMMEstimator(filters, mu, trans) for i in range(100): # make some noisy data x = i + np.random.randn() * np.sqrt(kf1.R[0, 0]) y = i + np.random.randn() * np.sqrt(kf1.R[1, 1]) z = np.array([[x], [y]]) # perform predict/update cycle imm.predict() imm.update(z) print(imm.x.T)
accel_magnitudes = np.ndarray.tolist((process.get_column(name='accel_mag'))) timesteps = np.ndarray.tolist((process.get_column(index=0))) data = np.array([process.get_column(index=0), process.get_column(index=11), process.get_column(name='accel_mag')]) data = data.T print(data) #print(process.get_column(index=0)) # todo use numpy alt_list = [] vel_list = [] kf = kinematic_kf(dim=1, order=3, dt=.04) transition_matrix = kf.F print(kf.F) # init altitude kf.x[0] = data[0][1] print('init', data[0][1]) kst = kinematic_state_transition(3, dt=.04) kinematics = np.empty(np.append(0, kf.x).shape) #kinematics.reshape(4, 1) print(kinematics) counter = 0 for time, alt, accel_mag in data:
def test_kinematic(): kf = kinematic_kf(1, 1)
#!/usr/bin/env python import time import math import rospy from geometry_msgs.msg import PoseStamped, TwistStamped import numpy as np from filterpy.common import Q_discrete_white_noise, kinematic_kf launchTime = rospy.get_param("/launch_time") launchTime += 5.0 freq = 20.0 latestSVGS = PoseStamped() f = kinematic_kf(3, 1, dt=(1.0 / freq), order_by_dim=False) def kalmanFilter(msg): global latestSVGS, latestVel, latestAcc, t0, t1, f latestSVGS = msg x = latestSVGS.pose.position.y y = latestSVGS.pose.position.z z = latestSVGS.pose.position.x z = np.array([x, y, z]) f.predict() f.update(z) # print (f.x) positionEstimate = PoseStamped() velocityEstimate = TwistStamped()
from filterpy.common import kinematic_kf from filterpy.kalman import ExtendedKalmanFilter import time import numpy as np import filterpy as fp from filterpy.kalman import IMMEstimator np.set_printoptions(suppress=True) order = 1 kf1 = kinematic_kf(dim=2, order=order) kf2 = kinematic_kf(dim=2, order=order) kf2.Q *= 0 # no prediction error in second filter filters = [kf1, kf2] mu = [0.5, 0.5] # filter probability trans = np.array([[0.97, 0.03], [0.03, 0.97]]) imm = IMMEstimator(filters, mu, trans) def aim(bounding_boxes): # TODO: Needs testing # TODO: Finalize on input contract if len(bounding_boxes) == 0: return imm.x_prior[0], imm.x_prior[2] # return last result if isinstance(bounding_boxes[0], list): x, y, w, h = bounding_boxes[0] else: x, y, w, h = bounding_boxes xc = x + int(w / 2) yc = y + int(h / 2)
Parameters ---------- measurement: float numpy array Measurement coming from a radar and needed to be tested. self.filter: KalmanFilter object The Kalman filter the detector is attached to. ''' test_quantity = sqrt(filter.y.T @ filter.y) return test_quantity if __name__ == "__main__": # Example Kalman filter for a kinematic model kinematic_test_kf = kinematic_kf(dim=1, order=1, dt=1) x = [0., 2.] zs = [x[0]] pos = [x[0]] noise_std = 1. # Noisy position measurements generation for 30 samples for _ in range(30): last_pos = x[0] last_vel = x[1] new_vel = last_vel new_pos = last_pos + last_vel x = [new_pos, new_vel] z = new_pos + (randn() * noise_std) zs.append(z) pos.append(new_pos)