def _KF_init(self): # para: Center of box used for prediction KF = KalmanFilter(4,2) KF.x = self.KF_center + [0,0] # can be improved for accuracy e.g. from which edge KF.F = np.array([ [1,0,1,0], [0,1,0,1], [0,0,1,0], [0,0,0,1]]) KF.H = np.array([ [1,0,0,0], [0,1,0,0]]) KF.P *= 100 KF.R *= 100 # KF.Q *= 2 # KF.predict() return KF
def do_filter_kalman_1D(X, noise_level=1, Q=0.001): #dim_x = X.shape[1]*2 dim_x = 2 fk = KalmanFilter(dim_x=dim_x, dim_z=1) fk.x = numpy.array([0., 1.]) # state (x and dx) fk.F = numpy.array([[1., 1.], [0., 1.]]) fk.H = numpy.array([[1., 0.]]) # Measurement function fk.P = 10. # covariance matrix fk.R = noise_level # state uncertainty fk.Q = Q # process uncertainty X_fildered, cov, _, _ = fk.batch_filter(X) return X_fildered[:, 0]
def create_kf_and_assign_predict_update(dim_z, X, P, A, Q, dt, R, H, B, U): ''' Function creates a Kalman Filter object and assigns all the instance variables :return: Kalman Filter ''' kf = KalmanFilter(dim_x=X.shape[0], dim_z=dim_z) kf.x = X kf.P = P kf.F = A print('=======================') kf.Q = Q kf.B = B kf.U = U kf.R = R kf.H = H return kf
def __init__(self, x0): self.kf = KalmanFilter(dim_x=2, dim_z=1) # Initial state estimate self.kf.x = np.array([x0, 0]) # Initial Covariance matrix self.kf.P = np.eye(2) * 2**10 # State transition function self.kf.F = np.array([[1., 1], [0., 1.]]) # Process noise self.kf.Q = Q_discrete_white_noise(dim=2, dt=1, var=1) # Measurement noise self.kf.R = np.array([[50]]) # Measurement function self.kf.H = np.array([[1., 0.]])
def tracker1(): tracker = KalmanFilter(dim_x=2, dim_z=2) dt = 0.002 tracker.F = np.array([[1, -1 * dt], [0, 1]]) tracker.u = 0.0 tracker.H = np.array([[1, 0]]) tracker.R = 0.05 q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01) tracker.Q = q tracker.x = np.array([[0, 0]]).T tracker.P = np.eye(2) * 5 return tracker
def setup_filter(self, detections: np.ndarray): """ Initialize Kalman Filter Args: detections: Points for initializing Kalman Filter """ # Validate shape of detected points detections = validate_points_shape(detections) # Calculates dimension of 'x' (Filter State Estimate) # Position x Velocity x Number of Points dim_x = 2 * 2 * self.points_count # Calculates dimension of 'z' (Last Measurement) dim_z = 2 * self.points_count self.dim_z = dim_z # Initialize the Kalman Filter self.filter = KalmanFilter(dim_x=dim_x, dim_z=dim_z) # Define the F (State Transition Matrix) self.filter.F = np.eye(dim_x) # Update dt = 1 # At each step we update pos with v * dt for p in range(dim_z): self.filter.F[p, p + dim_z] = dt # Define the H (Measurement Function) self.filter.H = np.eye( dim_z, dim_x, ) # Define the R (Measurement Uncertainty) self.filter.R *= 4.0 # Define the Q (Process Uncertainty/Noise) self.filter.Q[dim_z:, dim_z:] /= 10 # Initial state: numpy.array(dim_x, 1) self.filter.x[:dim_z] = np.expand_dims(detections.flatten(), 0).T # Estimation uncertainty: numpy.array(dim_x, dim_x) self.filter.P[dim_z:, dim_z:] *= 10.0
def kalman(bbox, gt, stats, Q = 0): #define kalman filter nx,nz = 4,4 Kal = KalmanFilter(dim_x = nx, dim_z = nz) Kal.R = np.eye(nz) * 500 Kal.R[0,0] = stats[0] Kal.R[1,1] = stats[1] Kal.R[2,2] = stats[2] Kal.R[3,3] = stats[3] Kal.Q = np.eye(nx) * float(Q) Kal.P *= 0 Kal.F = np.eye(nx) #Kal.F[2,4] = 1 Kal.H = np.eye(nz,nx,0) init = np.zeros(nx) init[:4] = gt[0,:] #init[4] = 1 Kal.x = init end = h.lastindex(gt) mem = ma.array(np.zeros((bbox.shape[0],nx)), mask = True) mem[0,:] = init memk = np.zeros((end,nx)) memp = np.zeros((end,nx)) for i in range(1, end): z = bbox[i, :] if bbox.mask[i, 0] == True: z = None Kal.predict() Kal.update(z) x = Kal.x mem[i,:] = x[:] for j in range(nx): memk[i,j] = Kal.K[j,j] for j in range(nx): memp[i,j] = Kal.P[j,j] return mem, memk,memp
def __init__(self, kf_obs, param): """ Initialises a tracker using initial position. KF Instance variables: x : ndarray (dim_x, 1), default = [0,0,0…0] filter state estimate P : ndarray (dim_x, dim_x), default eye(dim_x) covariance matrix Q : ndarray (dim_x, dim_x), default eye(dim_x) Process uncertainty/noise R : ndarray (dim_z, dim_z), default eye(dim_x) measurement uncertainty/noise H : ndarray (dim_z, dim_x) measurement function F : ndarray (dim_x, dim_x) state transistion matrix B : ndarray (dim_x, dim_u), default 0 control transition matrix """ #define constant velocity model self.kf = KalmanFilter(dim_x=5, dim_z=3) # x = [x,y,vx,vy,va] self.kf.x[:3] = kf_obs.reshape((3, 1)) #self.kf.P[3:,3:] *= 1000. #state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix self.kf.P = np.eye(5) * 50. self.kf.R = np.eye(3) * 50 #self.kf.R = np.eye(3) * parameter self.kf.P *= 10. self.kf.Q[3:,3:] *= 0.01 self.kf.F = np.array([[1,0,1,0,0], # state transition matrix [0,1,0,1,0], [0,0,1,0,0], [0,0,0,1,0], [0,0,0,0,1]]) self.kf.H = np.array([[0,0,1,0,0], # measurement function, [0,0,0,1,0], [0,0,0,0,1]]) # self.kf.R[0:,0:] *= 10. # measurement uncertainty self.time_since_update = 0 self.history = [] self.hits = 1 # number of total hits including the first detection self.hit_streak = 1 # number of continuing hit considering the first detection self.first_continuing_hit = 1 self.still_first = True self.age = 0
def test_noisy_11d(): f = KalmanFilter(dim_x=2, dim_z=1) f.x = np.array([2., 0]) # initial state (location and velocity) f.F = np.array([[1., 1.], [0., 1.]]) # state transition matrix f.H = np.array([[1., 0.]]) # Measurement function f.P *= 1000. # covariance matrix f.R = 5 # state uncertainty f.Q = 0.0001 # process uncertainty measurements = [] results = [] zs = [] for t in range(100): # create measurement = t plus white noise z = t + random.randn() * 20 zs.append(z) # perform kalman filtering f.update(z) f.predict() # save data results.append(f.x[0]) measurements.append(z) # now do a batch run with the stored z values so we can test that # it is working the same as the recursive implementation. # give slightly different P so result is slightly different f.x = np.array([[2., 0]]).T f.P = np.eye(2) * 100. m, c, _, _ = f.batch_filter(zs, update_first=False) # plot data if DO_PLOT: p1, = plt.plot(measurements, 'r', alpha=0.5) p2, = plt.plot(results, 'b') p4, = plt.plot(m[:, 0], 'm') p3, = plt.plot([0, 100], [0, 100], 'g') # perfect result plt.legend([p1, p2, p3, p4], ["noisy measurement", "KF output", "ideal", "batch"], loc=4) plt.show()
def __init__(self, bbox, label): """ Initialises a tracker using initial bounding box. """ # define constant velocity model self.kf = KalmanFilter(dim_x=7, dim_z=4) self.kf.F = np.array([ [1, 0, 0, 0, 1, 0, 0], [0, 1, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0, 1], [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], ]) self.kf.H = np.array([ [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], ]) self.kf.R[2:, 2:] *= 10.0 self.kf.P[ 4:, 4:] *= 1000.0 # give high uncertainty to the unobservable initial velocities self.kf.P *= 10.0 self.kf.Q[-1, -1] *= 0.01 self.kf.Q[4:, 4:] *= 0.01 self.kf.x[:4] = convert_bbox_to_z(bbox) self.time_since_update = 0 self.id = KalmanBoxTracker.count KalmanBoxTracker.count += 1 self.history = [] self.hits = 0 self.hit_streak = 0 self.age = 0 self.labels = [label] self.counted = False self.locations = [] self.creation_time = int(datetime.now().timestamp() * 1000) self.color = ( random.randint(0, 256), random.randint(0, 256), 0, )
def __init__(self, position, parameter): """ Initialises a tracker using initial position. KF Instance variables: x : ndarray (dim_x, 1), default = [0,0,0…0] filter state estimate P : ndarray (dim_x, dim_x), default eye(dim_x) covariance matrix Q : ndarray (dim_x, dim_x), default eye(dim_x) Process uncertainty/noise R : ndarray (dim_z, dim_z), default eye(dim_x) measurement uncertainty/noise H : ndarray (dim_z, dim_x) measurement function F : ndarray (dim_x, dim_x) state transistion matrix B : ndarray (dim_x, dim_u), default 0 control transition matrix """ #define constant velocity model self.kf = KalmanFilter(dim_x=4, dim_z=2) # x = [x,y,vx,vy] self.kf.x[:2] = position.reshape((2, 1)) correct = 1000 self.kf.R = np.array([ [1000, 0], # Measurement uncertainty [0, 1000] ]) self.kf.P *= 80 self.kf.Q = np.eye(4) * 100 self.kf.F = np.array([ [1, 0, 1, 0], # state transition matrix [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1] ]) self.kf.H = np.array([ [1, 0, 0, 0], # measurement function, [0, 1, 0, 0] ]) self.time_since_update = 0 self.history = [] self.hits = 1 # number of total hits including the first detection self.hit_streak = 1 # number of continuing hit considering the first detection self.first_continuing_hit = 1 self.still_first = True self.age = 0
def KFtest(): dt = 0.001 my_filter = KalmanFilter(dim_x=2, dim_z=1) my_filter.x = np.array([[0.0], [0.0]]) # initial state (location and velocity) my_filter.F = np.array([[1.0, dt], [0.0, 1.0]]) # state transition matrix my_filter.H = np.array([[1.0, 0.0]]) # Measurement function # my_filter.P = 1000. # covariance matrix my_filter.R = 0.1 # state uncertainty my_filter.Q = Q_discrete_white_noise(2, dt, 1e5) # process uncertainty x_list = list() k = static_var() tmp = k.get_some_measurement() for i in tmp: my_filter.predict() # print ( i ) my_filter.update(i[0]) # do something with the output # x_list = my_filter.x # print( my_filter.x) x_list.append(my_filter.x[0][0].tolist()) # plt.figure(1) # plt.plot( tmp ) a = tmp[:, 0] b = np.asarray(x_list) c = np.vstack((a, b)).T plt.figure(2) # tmp = np.asarray(tmp).T plt.plot(c) # plt.plot( np.hstack((x_list, tmp)) ) plt.show() input()
def __init__(self, bbox, ids, logger, cfg): self.abnormal1 = 0 self.abnormal2 = 0 self.Vsv = cfg["Vs"] self.Vxyv = cfg["Vxy"] self.logger = logger self.kf = KalmanFilter(dim_x=7, dim_z=4) self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0], [0, 1, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0, 1], [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]]) self.kf.H = np.array([[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]]) self.kf.R[2:, 2:] *= 10 self.kf.Q[-1, -1] *= 0.01 self.kf.Q = np.array([[0.01, 0, 0, 0, 0.01, 0, 0], [0, 0.01, 0, 0, 0, 0.01, 0], [0, 0, 0.01, 0, 0, 0, 0.01], [0, 0, 0, 0.01, 0, 0, 0], [0.01, 0, 0, 0, 0.1, 0, 0], [0, 0.01, 0, 0, 0, 0.1, 0], [0, 0, 0.01, 0, 0, 0, 0.1]]) #self.kf.Q[6, 6] = 1 #self.kf.P *= 10 self.kf.x[:4] = Tracker.get_xysr_rect(bbox) self.ids = ids self.state = STATE.INITED self.trackHistory = [] self.timeSinceUpdate = 0 self.hits = 1 self.age = 0 self.countRect = np.array([0, 0, 0, 0]) self.VsFlag = False self.VxyFlag = False
def __init__(self, face): """Initializes a tracker using an initial detected face. Parameters ---------- face : dict Face to initialize tracker to, as returned by a `Detection` instance. """ # 4 measurements plus 3 velocities. We don't keep a velocity for the # ratio of the bounding box. self.kf = KalmanFilter(dim_x=7, dim_z=4) self.kf.F = np.array([ [1, 0, 0, 0, 1, 0, 0], [0, 1, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0, 1], [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], ]) self.kf.H = np.array([ [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], ]) self.kf.R[2:, 2:] *= 10.0 # Give high uncertainty to the unobservable initial velocities. self.kf.P[4:, 4:] *= 1000.0 self.kf.P *= 10.0 self.kf.Q[-1, -1] *= 0.01 self.kf.Q[4:, 4:] *= 0.01 self.kf.x[:4] = corners_to_center(face['bbox']) self.hits = 0 self.time_since_update = 0 self.id = KalmanTracker.count KalmanTracker.count += 1
def compute_ground_truth(observations, initial_mean, initial_var, transition_factor, transition_noise_var, observation_factor, observation_noise_var): fk = KalmanFilter(dim_x=len(initial_mean), dim_z=observations.shape[1]) fk.x = initial_mean fk.P = initial_var fk.F = transition_factor fk.Q = transition_noise_var fk.H = observation_factor fk.R = observation_noise_var mu, cov, _, _ = fk.batch_filter(observations) means, covs, _, _ = fk.rts_smoother(mu, cov) return means, covs
def create_kf_and_assign_predict_update(dim_z, X, P, A, Q, dt, R, H, B, U): ''' :param configs tuple: all the values to define the kalman filter :return: Kalman Filter ''' kf = KalmanFilter(dim_x=X.shape[0], dim_z=dim_z) kf.x = X kf.P = P kf.F = A print('=======================') kf.Q = Q kf.B = B kf.U = U kf.R = R kf.H = H return kf
def run_standard_kf(zs, dt=1.0, std_x=0.3, std_y=0.3): kf = KalmanFilter(4, 2) kf.x = np.array([0., 0., 0., 0.]) kf.R = np.diag([std_x**2, std_y**2]) kf.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) kf.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) kf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.02) kf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.02) xs, *_ = kf.batch_filter(zs) return xs
def build_kf(self, bbox): kf = KalmanFilter(dim_x=7, dim_z=4) kf.F = np.array([[1, 0, 0, 0, 1, 0, 0], [0, 1, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0, 1], [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]]) kf.H = np.array([[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]]) kf.R[2:, 2:] *= 10. kf.P[ 4:, 4:] *= 1000. #give high uncertainty to the unobservable initial velocities kf.P *= 10. kf.Q[-1, -1] *= 0.01 kf.Q[4:, 4:] *= 0.01 kf.x[:4] = self.convert_bbox_to_z(bbox) return kf
def filter(self): "Filtra la corsa attraverso un filtro di Kalman" # Crea il filtro di Kalman f = KalmanFilter(dim_x=2, dim_z=2) # Inizializza il filtro f.x = np.array(self.points[0]) index = 0 while index < len(self.points) - 1: f.F = np.array([[1, 0], [0, 1]]) # state transition matrix f.H = np.array([[1, 0], [0, 1]]) # Measurement function f.P *= 1.5 # covariance matrix f.R = np.array([[1, 0], [0, 1]]) # state uncertainty f.Q = Q_discrete_white_noise(2, 1., 1.) # process uncertainty f.predict() f.update(self.points[index + 1]) self.points[index + 1] = f.x index += 1
def __init__(self, dim_state, dim_control, dim_measurement, initial_state_mean, initial_state_covariance, matrix_F, matrix_B, process_noise_Q, matrix_H, measurement_noise_R): filter = KalmanFilter(dim_x=dim_state, dim_u=dim_control, dim_z=dim_measurement) filter.x = initial_state_mean filter.P = initial_state_covariance filter.Q = process_noise_Q filter.F = matrix_F filter.B = matrix_B filter.H = matrix_H filter.R = measurement_noise_R # covariance matrix super().__init__(filter)
def make_filter(start): f = KalmanFilter(dim_x=6, dim_z=3) f.x = start f.F = np.eye(6) f.F[:3, 3:6] = np.eye(3) * dt f.B = np.array([0, dt**2 / 2, 0, 0, dt, 0]) g = -9.81 f.H = np.zeros((3, 6)) f.H[:, :3] = np.eye(3) f.Q *= 0.5 f.R *= 2 f.P *= 1 return f
def setup(): # set up sensor simulator dt = 0.1 F = np.array([[1, dt], [0, 1]]) H = np.array([[1, 0]]) x0 = np.array([[0], [0.1]]) sim = LinearSensor(x0, F, H) # set up kalman filter tracker = KalmanFilter(dim_x=2, dim_z=1) tracker.F = F q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01) tracker.Q = q tracker.H = H tracker.R = measurement_var * filter_misestimation_factor tracker.x = np.array([[0, 0]]).T tracker.P = np.eye(2) * 500 return sim, tracker
def __init__(self, coors: Tuple, state_noise: float = 1.0, r_scale: float = 1.0, q_var: float = 1.0): self.filter = KalmanFilter(dim_x=4, dim_z=2) self.filter.x = np.array([coors[0], 0, coors[1], 0]) self.dt = 1.0 self.filter.F = np.array([[1, self.dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, self.dt], [0, 0, 0, 1]]) self.filter.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) self.filter.P *= state_noise self.filter.R = np.diag(np.ones(2)) * state_noise * r_scale self.filter.Q = Q_discrete_white_noise(dim=2, dt=self.dt, var=q_var, block_size=2)
def test_procedure_form(): dt = 1. std_z = 10.1 x = np.array([[0.], [0.]]) F = np.array([[1., dt], [0., 1.]]) H = np.array([[1., 0.]]) P = np.eye(2) R = np.eye(1) * std_z**2 Q = Q_discrete_white_noise(2, dt, 5.1) kf = KalmanFilter(2, 1) kf.x = x.copy() kf.F = F.copy() kf.H = H.copy() kf.P = P.copy() kf.R = R.copy() kf.Q = Q.copy() measurements = [] xest = [] pos = 0. for t in range(2000): z = pos + random.randn() * std_z pos += 100 # perform kalman filtering x, P = predict(x, P, F, Q) kf.predict() assert norm(x - kf.x) < 1.e-12 x, P, _, _, _, _ = update(x, P, z, R, H, True) kf.update(z) assert norm(x - kf.x) < 1.e-12 # save data xest.append(x.copy()) measurements.append(z) xest = np.asarray(xest) measurements = np.asarray(measurements) # plot data if DO_PLOT: plt.plot(xest[:, 0]) plt.plot(xest[:, 1]) plt.plot(measurements)
def Kalman1(self): pos = self.pos dt = 0.01 rk = KalmanFilter(dim_x=4, dim_z=2) #radar = RadarSim(dt, pos=0., vel=100., alt=1000.) # make an imperfect starting guess rk.x = np.array( [pos[0] - 10, self.vel[0] + 10, pos[1] + 10, self.vel[1] - 10]).T rk.F = np.eye(4) + np.array([[0, 1, 0, 0], [0, 0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 0]]) * dt range_std = 5 # 5 pixels rk.R = np.eye(2) * 200**2 q = Q_discrete_white_noise(dim=2, dt=dt, var=0.1**2) rk.Q = block_diag(q, q) rk.P *= 1000 rk.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) return rk
def test_1d(): f = KalmanFilter(dim_x=2, dim_z=1) inf = InformationFilter(dim_x=2, dim_z=1) f.x = np.array([[2.], [0.]]) # initial state (location and velocity) inf.x = f.x.copy() f.F = (np.array([[1., 1.], [0., 1.]])) # state transition matrix inf.F = f.F.copy() f.H = np.array([[1., 0.]]) # Measurement function inf.H = np.array([[1., 0.]]) # Measurement function f.R *= 5 # state uncertainty inf.R_inv *= 1. / 5 # state uncertainty f.Q *= 0.0001 # process uncertainty inf.Q *= 0.0001 m = [] r = [] r2 = [] zs = [] for t in range(100): # create measurement = t plus white noise z = t + random.randn() * 20 zs.append(z) # perform kalman filtering f.update(z) f.predict() inf.update(z) inf.predict() # save data r.append(f.x[0, 0]) r2.append(inf.x[0, 0]) m.append(z) assert abs(f.x[0, 0] - inf.x[0, 0]) < 1.e-12 if DO_PLOT: plt.plot(m) plt.plot(r) plt.plot(r2)
def __init__(self, pose, tracked_points_num): """ Initialises a tracker using initial bounding box. """ dim_x = 2 * 2 * tracked_points_num # We need to accomodate for velocities dim_z = 2 * tracked_points_num # We need to accomodate for velocities self.dim_z = dim_z # Define constant velocity model self.kf = KalmanFilter(dim_x=dim_x, dim_z=dim_z) # State transition matrix (models physics): numpy.array() self.kf.F = np.eye(dim_x) dt = 1 # At each step we update pos with v * dt # Add position vs velocity update rules for p in range(dim_z): self.kf.F[p, p + dim_z] = dt # Process uncertainty: numpy.array(dim_x, dim_x) self.kf.Q[-1,-1] *= 0.01 # TODO check self.kf.Q[dim_z:, dim_z:] *= 0.01 # Measurement function: numpy.array(dim_z, dim_x) self.kf.H = np.eye(dim_z, dim_x,) # Measurement uncertainty (sensor noise): numpy.array(dim_z, dim_z) # NOTE: Reduced from 10 to 1 as it made our predictions lag behind our detections too much self.kf.R *= 1. # Initial state: numpy.array(dim_x, 1) self.kf.x[:dim_z] = self.convert_to_kf_x_format(pose) # Initial state covariance matrix: numpy.array(dim_x, dim_x) self.kf.P[dim_z:, dim_z:] *= 1000. # Give high uncertainty to the unobservable initial velocities self.kf.P *= 10. # TODO we get 10 * 1000 in velocities, is this correct? self.time_since_update = 0 self.id = KalmanPoseTracker.count KalmanPoseTracker.count += 1 self.history = [] self.hits = 0 self.hit_streak = 0 self.age = 0 self.debug_dict = {} # # NOTE: I am overriding our predictions by setting this huge K because when a limb dissapears # # the kalman filter interpolates between its position and (0, 0) and f***s my code up. # self.kf.K += 10000 self.last_detection = pose
def __init__(self, x=0.0, y=0.0, z=0.0, is_active=False, is_dead=False, youth=0, strikes=0, dt=0.16): #dt = 0.1 bcoz we getting data at 10Hz ''' x and yare coordinates of the the obstacles in the plane they are moving. is_active determines if a marker is to be published for this object. Youth variable is used to track the number of frames a new object appears, this is to filter out transient noise. Strikes calculate how long the object was not tracked. ''' global color_pallete self.id = str(rospy.Time.now()) self.x = x self.y = y self.z = z self.is_active = is_active self.is_dead = is_dead self.youth = youth self.strikes = strikes self.color = color_pallete[random.randint(0, 8)] # kalman filter object. x = [x, x_dot, y, y_dot] and z = [x_measured, y_measured] self.kf = KalmanFilter(dim_x=4, dim_z=2) # state transition matrix self.kf.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) # process noise matrix q = Q_discrete_white_noise(dim=2, dt=dt, var=2) # orignal value 200 self.kf.Q = block_diag(q, q) # measurement funtion self.kf.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) # measurement noise matrix self.kf.R = np.array([[0.05, 0.], [0., 0.05]]) #orignal values .2 self.kf.x = np.array([[self.x, 0, self.y, 0]]).T ######it is z, not y for the time being # self.kf.P = np.eye(4) * 100. self.kf.P = np.array([ [1, 0, 0, 0], #######original value 2 [0, 4, 0, 0], [0, 0, 1, 0], [0, 0, 0, 4] ])
def __init__(self, vid_settings, maze_settings) -> None: # read frame size from config file # self.settings = read_yaml(conf) self.settings = vid_settings self.height = self.settings['frame_height'][1] - self.settings[ 'frame_height'][0] self.width = self.settings['frame_width'][1] - self.settings[ 'frame_width'][0] print(f'frame width: {self.width} frame height: {self.height}') # setup fast corner detection self.fast = cv.FastFeatureDetector_create( threshold=maze_settings['fast_thresh']) self.fast.setNonmaxSuppression(0) # set initial points to use as a corner estimation self.prev_tl = (0, 0) self.prev_tr = (0, self.width - 1) self.prev_bl = (self.height - 1, 0) self.prev_br = (self.height - 1, self.width - 1) # corner filtering self.med_offset = self.settings['median_offset'] self.offsets = self.settings['offsets'] # search area when ball already known self.area_size = self.settings['search_area'] # annotated text locations self.text_tl = (self.settings['text_tl'][0], self.settings['text_tl'][1]) self.text_tr = (self.settings['text_tr'][0], self.settings['text_tr'][1]) self.text_spacing = self.settings['text_spacing'] self.text_size = self.settings['text_size'] # initialize Kalman Filter self.kf = KalmanFilter(dim_x=4, dim_z=2) self.kf.x = np.array([0., 0., 0., 0.]) self.kf.F = np.array([[1., 0., 1., 0.], [0., 1., 0., 1.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) self.kf.H = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.]]) self.kf.P *= 100. self.kf.R = 5.
def __init__(self, bbox3D, info, R, Q, P_0, delta_t): """ Initialises a tracker using initial bounding box. """ #define constant velocity model self.kf = KalmanFilter(dim_x=STATE_SIZE, dim_z=MEAS_SIZE) if MOTION_MODEL == "CV": self.kf.F = get_CV_F(delta_t) elif MOTION_MODEL == "CA": self.kf.F = get_CA_F(delta_t) elif MOTION_MODEL == "CYRA": self.kf.F = get_CYRA_F(delta_t) else: print("unknown motion model", MOTION_MODEL) raise ValueError # x y z theta l w h self.kf.H = np.zeros((MEAS_SIZE, STATE_SIZE)) for i in range(min(MEAS_SIZE, STATE_SIZE)): self.kf.H[i, i] = 1. self.kf.R[0:, 0:] = R # measurement uncertainty # initialisation cov # self.kf.P[7:,7:] *= 1000. #state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix # self.kf.P *= 10. # innov cov from pixor stats self.kf.P = P_0 # self.kf.Q[-1,-1] *= 0.01 # self.kf.Q[7:,7:] *= 0.01 # process uncertainty self.kf.Q = Q self.kf.x[:7] = bbox3D.reshape((7, 1)) self.time_since_update = 0 self.id = KalmanBoxTracker.count KalmanBoxTracker.count += 1 self.history = [] self.hits = 1 # number of total hits including the first detection self.hit_streak = 1 # number of continuing hit considering the first detection self.first_continuing_hit = 1 self.still_first = True self.age = 0 self.info = info # other info