class KFMotionModel(object): def __init__(self,bb): 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.P[4:,4:] *= 1000. self.kf.P *= 10. self.kf.Q[-1,-1] *= 0.01 self.kf.Q[4:,4:] *= 0.01 self.kf.x[:4,0] = np.array(bb2z(bb)) self.history = [] self.predicted = False def update(self,bb): self.history = [] bb = np.array(bb2z(bb)) bb = np.expand_dims(bb, axis=1) self.kf.update(bb) self.predicted = False def predict(self): if not self.predicted: if((self.kf.x[6]+self.kf.x[2])<=0): self.kf.x[6] *= 0.0 self.kf.predict() self.history.append(z2bb(self.kf.x)) self.predicted=True return self.history[-1] def get_state(self): return z2bb(self.kf.x)
def create_kalman_filter(self, det): """(x, y, s(area), r(aspect ratio), x', y', s') """ model = KalmanFilter(dim_x=7, dim_z=4) model.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], ], 'float32') model.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], ], 'float32') model.R[2:,2:] *= 10. model.P[4:,4:] *= 1000. # high uncertainty of initial volocity model.P *= 10. model.Q[-1,-1] *= 0.01 model.Q[4:,4:] *= 0.01 model.x[:4] = np.array(xywh_to_xysr(*det), 'float32').reshape(4, 1) return model
def _KF_init(self): # para: Center of box used for prediction KF = KalmanFilter(4,2) # KF.x = location + [0,0,0,0] # KF.F = np.array([ # [1,0,0,0,1,0,0,0], # [0,1,0,0,0,1,0,0], # [0,0,1,0,0,0,1,0], # [0,0,0,1,0,0,0,1], # [0,0,0,0,1,0,0,0], # [0,0,0,0,0,1,0,0], # [0,0,0,0,0,0,1,0], # [0,0,0,0,0,0,0,1]]) # KF.H = np.array([ # [1,0,0,0,0,0,0,0], # [0,1,0,0,0,0,0,0], # [0,0,1,0,0,0,0,0], # [0,0,0,1,0,0,0,0]]) 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 __init__(self, dt, ID, position_x, position_y): self.p_x = KalmanFilter(dim_x=3, dim_z=1) self.p_y = KalmanFilter(dim_x=3, dim_z=1) self.p_x.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]]) self.p_y.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]]) self.p_x.H = np.array([[1, 0., 0.]]) self.p_y.H = np.array([[1, 0., 0.]]) self.R_x_std = 0.01 # update to the correct value self.Q_x_std = 7 # update to the correct value self.R_y_std = 0.01 # update to the correct value self.Q_y_std = 7 # update to the correct value self.p_y.Q = Q_discrete_white_noise(dim=3, dt=dt, var=self.Q_y_std ** 2) self.p_x.Q = Q_discrete_white_noise(dim=3, dt=dt, var=self.Q_x_std ** 2) self.p_x.R *= self.R_x_std ** 2 self.dt = dt self.ID = ID self.p_x.x = np.array([[position_x], [0.], [0.]]) self.p_y.x = np.array([[position_y], [0.], [0.]]) self.p_x.P *= 100. # can very likely be set to 100. self.p_y.P *= 100. # can very likely be set to 100. self.time_since_last_update = 0.0 self.p_y.R *= self.R_y_std ** 2
class KalmanBoxTracker(object): """ This class represents the internel state of individual tracked objects observed as bbox. """ count = 0 def __init__(self,bbox): """ 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. self.kf.P[4:,4:] *= 1000. #give high uncertainty to the unobservable initial velocities self.kf.P *= 10. 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 def update(self,bbox): """ Updates the state vector with observed bbox. """ self.time_since_update = 0 self.history = [] self.hits += 1 self.hit_streak += 1 self.kf.update(convert_bbox_to_z(bbox)) def predict(self): """ Advances the state vector and returns the predicted bounding box estimate. """ if((self.kf.x[6]+self.kf.x[2])<=0): self.kf.x[6] *= 0.0 self.kf.predict() self.age += 1 if(self.time_since_update>0): self.hit_streak = 0 self.time_since_update += 1 self.history.append(convert_x_to_bbox(self.kf.x)) return self.history[-1] def get_state(self): """ Returns the current bounding box estimate. """ return convert_x_to_bbox(self.kf.x)
def make_cv_filter(dt, noise_factor): cvfilter = KalmanFilter(dim_x = 2, dim_z=1) cvfilter.x = array([0., 0.]) cvfilter.P *= 3 cvfilter.R *= noise_factor**2 cvfilter.F = array([[1, dt], [0, 1]], dtype=float) cvfilter.H = array([[1, 0]], dtype=float) cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02) return cvfilter
def test_rts(): fk = KalmanFilter(dim_x=2, dim_z=1) fk.x = np.array([-1., 1.]) # initial state (location and velocity) fk.F = np.array([[1.,1.], [0.,1.]]) # state transition matrix fk.H = np.array([[1.,0.]]) # Measurement function fk.P = .01 # covariance matrix fk.R = 5 # state uncertainty fk.Q = 0.001 # process uncertainty zs = [t + random.randn()*4 for t in range(40)] mu, cov, _, _ = fk.batch_filter (zs) mus = [x[0] for x in mu] M, P, _, _ = fk.rts_smoother(mu, cov) if DO_PLOT: p1, = plt.plot(zs,'cyan', alpha=0.5) p2, = plt.plot(M[:,0],c='b') p3, = plt.plot(mus,c='r') p4, = plt.plot([0, len(zs)], [0, len(zs)], 'g') # perfect result plt.legend([p1, p2, p3, p4], ["measurement", "RKS", "KF output", "ideal"], loc=4) plt.show()
def make_ca_filter(dt, noise_factor): cafilter = KalmanFilter(dim_x=3, dim_z=1) cafilter.x = array([0., 0., 0.]) cafilter.P *= 3 cafilter.R *= noise_factor**2 cafilter.Q = Q_discrete_white_noise(dim=3, dt=dt, var=0.02) cafilter.F = array([[1, dt, 0.5*dt*dt], [0, 1, dt], [0, 0, 1]], dtype=float) cafilter.H = array([[1, 0, 0]], dtype=float) return cafilter
def ZeroOrderKF(R, Q, P=20): """ Create zero order Kalman filter. Specify R and Q as floats.""" kf = KalmanFilter(dim_x=1, dim_z=1) kf.x = np.array([0.]) kf.R *= R kf.Q *= Q kf.P *= P kf.F = np.eye(1) kf.H = np.eye(1) return kf
def kf_circle(): from filterpy.kalman import KalmanFilter from math import radians import math 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]]) def hx_inv(x, y): angle = math.atan2(y,x) radius = math.sqrt(x*x + y*y) return np.array([radius, angle]) std_noise = .1 kf = KalmanFilter(dim_x=3, dim_z=2) kf.x = np.array([50., 0., 0.]) F = np.array([[1., 0, 0.], [0., 1., 1.,], [0., 0., 1.,]]) kf.F = F kf.P*= 100 kf.H = np.array([[1,0,0], [0,1,0]]) kf.R = np.eye(2)*(std_noise**2) #kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001) zs = [] kfxs = [] for t in range (0,2000): a = t / 30 + 90 x = cos(radians(a)) * 50.+ randn() * std_noise y = sin(radians(a)) * 50. + randn() * std_noise z = hx_inv(x,y) zs.append(z) kf.predict() kf.update(z) # save data kfxs.append(kf.x) zs = np.asarray(zs) kfxs = np.asarray(kfxs)
def FirstOrderKF(R, Q, dt): """ Create first order Kalman filter. Specify R and Q as floats.""" kf = KalmanFilter(dim_x=2, dim_z=1) kf.x = np.zeros(2) kf.P *= np.array([[100, 0], [0, 1]]) kf.R *= R kf.Q = Q_discrete_white_noise(2, dt, Q) kf.F = np.array([[1., dt], [0., 1]]) kf.H = np.array([[1., 0]]) return kf
def test_against_kf(): inv = np.linalg.inv dt = 1.0 IM = np.eye(2) Q = np.array([[.25, 0.5], [0.5, 1]]) F = np.array([[1, dt], [0, 1]]) #QI = inv(Q) P = inv(IM) from filterpy.kalman import InformationFilter from filterpy.common import Q_discrete_white_noise #f = IF2(2, 1) r_std = .2 R = np.array([[r_std*r_std]]) RI = inv(R) '''f.F = F.copy() f.H = np.array([[1, 0.]]) f.RI = RI.copy() f.Q = Q.copy() f.IM = IM.copy()''' kf = KalmanFilter(2, 1) kf.F = F.copy() kf.H = np.array([[1, 0.]]) kf.R = R.copy() kf.Q = Q.copy() f0 = InformationFilter(2, 1) f0.F = F.copy() f0.H = np.array([[1, 0.]]) f0.R_inv = RI.copy() f0.Q = Q.copy() #f.IM = np.zeros((2,2)) for i in range(1, 50): z = i + (np.random.rand() * r_std) f0.predict() #f.predict() kf.predict() f0.update(z) #f.update(z) kf.update(z) print(f0.x.T, kf.x.T) assert np.allclose(f0.x, kf.x)
def test_univariate(): f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1) f.x = np.array([[0]]) f.P *= 50 f.H = np.array([[1.]]) f.F = np.array([[1.]]) f.B = np.array([[1.]]) f.Q = .02 f.R *= .1 for i in range(50): f.predict() f.update(i)
def single_measurement_test(): dt = 0.1 sigma = 2. kf2 = KalmanFilter(dim_x=2, dim_z=1) kf2.F = array ([[1., dt], [0., 1.]]) kf2.H = array ([[1., 0.]]) kf2.x = array ([[0.], [1.]]) kf2.Q = array ([[dt**3/3, dt**2/2], [dt**2/2, dt]]) * 0.02 kf2.P *= 100 kf2.R[0,0] = sigma**2 random.seed(SEED) xs = [] zs = [] nom = [] for i in range(1, 100): m0 = i + randn()*sigma z = array([[m0]]) kf2.predict() kf2.update(z) xs.append(kf2.x.T[0]) zs.append(z.T[0]) nom.append(i) xs = asarray(xs) zs = asarray(zs) nom = asarray(nom) res = nom-xs[:,0] std_dev = np.std(res) print('std: {:.3f}'.format (std_dev)) global DO_PLOT if DO_PLOT: plt.subplot(211) plt.plot(xs[:,0]) #plt.plot(zs[:,0]) plt.subplot(212) plt.plot(res) plt.show() return std_dev
def test_1d_0P(): 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 f.P *= 20 inf.P_inv = 0 #inf.P_inv = inv(f.P) 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.predict() f.update(z) inf.predict() inf.update(z) # 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 template(): kf = KalmanFilter(dim_x=2, dim_z=1) # x0 kf.x = np.array([[.0], [.0]]) # P - change over time kf.P = np.diag([500., 49.]) # F - state transition matrix dt = .1 kf.F = np.array([[1, dt], [0, 1]]) ## now can predict ## дисперсия растет и становится видна корреляция #plot_covariance_ellipse(kf.x, kf.P, edgecolor='r') #kf.predict() #plot_covariance_ellipse(kf.x, kf.P, edgecolor='k', ls='dashed') #show() # Control information kf.B = 0 # !!Attention!! Q! Process noise kf.Q = Q_discrete_white_noise( dim=2, dt=dt, var=2.35) # H - measurement function kf.H = np.array([[1., 0.]]) # R - measure noise matrix kf.R = np.array([[5.]])
def createLegKF(x, y): # kalman_filter = KalmanFilter(dim_x=4, dim_z=2) kalman_filter = KalmanFilter(dim_x=4, dim_z=4) dt = 0.1 KF_F = np.array([[1.0, dt, 0, 0], [0, 1.0, 0, 0], [0, 0, 1.0, dt], [0, 0, 0, 1.0]]) KF_q = 0.7 # 0.3 KF_Q = np.vstack( ( np.hstack((Q_discrete_white_noise(2, dt=0.1, var=KF_q), np.zeros((2, 2)))), np.hstack((np.zeros((2, 2)), Q_discrete_white_noise(2, dt=0.1, var=KF_q))), ) ) KF_pd = 25.0 KF_pv = 10.0 KF_P = np.diag([KF_pd, KF_pv, KF_pd, KF_pv]) KF_rd = 0.05 KF_rv = 0.2 # 0.5 # KF_R = np.diag([KF_rd,KF_rd]) KF_R = np.diag([KF_rd, KF_rd, KF_rv, KF_rv]) # KF_H = np.array([[1.,0,0,0],[0,0,1.,0]]) KF_H = np.array([[1.0, 0, 0, 0], [0, 0, 1.0, 0], [0, 1.0, 0, 0], [0, 0, 0, 1.0]]) kalman_filter.x = np.array([x, 0, y, 0]) kalman_filter.F = KF_F kalman_filter.H = KF_H kalman_filter.Q = KF_Q kalman_filter.B = 0 kalman_filter.R = KF_R kalman_filter.P = KF_P return kalman_filter
def __init__(self,bbox): """ Initialises a tracker using initial bounding box. """ #define constant velocity model self.kf = KalmanFilter(dim_x=8, dim_z=4) self.kf.F = np.array([[1,0,0,0,1,0,0.5,0],[0,1,0,0,0,1,0,0.5],[0,0,1,0,0,0,0,0],[0,0,0,1,0,0,0,0],[0,0,0,0,1,0,1,0],[0,0,0,0,0,1,0,1],[0,0,0,0,0,0,1,0],[0,0,0,0,0,0,0,1]]) self.kf.H = np.array([[1,0,0,0,0,0,0,0],[0,1,0,0,0,0,0,0],[0,0,1,0,0,0,0,0],[0,0,0,1,0,0,0,0]]) self.kf.R[:,:] *= 25.0 # set measurement uncertainty for positions self.kf.Q[:2,:2] = 0.0 # process uncertainty for positions is zero - only moves due to velocity, leave process for width height as 1 to account for turning self.kf.Q[2:4,2:4] *= 0.1 # process uncertainty for width/height for turning self.kf.Q[4:6,4:6] = 0.0 # process uncertainty for velocities is zeros - only accelerates due to accelerations self.kf.Q[6:,6:] *= 0.01 # process uncertainty for acceleration self.kf.P[4:,4:] *= 15.0 # maximum speed z=convert_bbox_to_kfx(bbox) self.kf.x[:4] = z self.time_since_update = 0 self.id = KalmanBoxTracker.count KalmanBoxTracker.count += 1 self.hits = 1 self.hit_streak = 1 self.age = 1 self.score = bbox[4]
def __init__(self, bbox): """ 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. # Give high uncertainty to the unobservable initial velocities. self.kf.P[4:, 4:] *= 1000. self.kf.P *= 10. 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
class CarTracker: def __init__(self, dt, ID, position_x, position_y): self.p_x = KalmanFilter(dim_x=3, dim_z=1) self.p_y = KalmanFilter(dim_x=3, dim_z=1) self.p_x.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]]) self.p_y.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]]) self.p_x.H = np.array([[1, 0., 0.]]) self.p_y.H = np.array([[1, 0., 0.]]) self.R_x_std = 0.01 # update to the correct value self.Q_x_std = 7 # update to the correct value self.R_y_std = 0.01 # update to the correct value self.Q_y_std = 7 # update to the correct value self.p_y.Q = Q_discrete_white_noise(dim=3, dt=dt, var=self.Q_y_std ** 2) self.p_x.Q = Q_discrete_white_noise(dim=3, dt=dt, var=self.Q_x_std ** 2) self.p_x.R *= self.R_x_std ** 2 self.dt = dt self.ID = ID self.p_x.x = np.array([[position_x], [0.], [0.]]) self.p_y.x = np.array([[position_y], [0.], [0.]]) self.p_x.P *= 100. # can very likely be set to 100. self.p_y.P *= 100. # can very likely be set to 100. self.time_since_last_update = 0.0 self.p_y.R *= self.R_y_std ** 2 def update_pose(self,position_x, position_y): self.time_since_last_update = 0.0 # reset time since last update self.p_x.update([[position_x]]) self.p_y.update([[position_y]]) def predict_pose(self): self.time_since_last_update += self.dt self.p_x.predict() self.p_y.predict() def get_last_update_time(self): return self.time_since_last_update def get_position(self): return [self.p_x.x[0], self.p_y.x[0]] def get_current_error(self): return [(self.p_x.P[0])[0], (self.p_y.P[0])[0]] def get_total_speed(self): # calculate magnitude of speed return np.sqrt(self.p_x.x[1]**2+self.p_y.x[1]**2) def get_ID(self): return self.ID
def __init__(self, bbox): """ 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 = KalmanFilter(dim_x=8, dim_z=4) self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0, 0], [0, 1, 0, 0, 0, 1, 0, 0], [0, 0, 1, 0, 0, 0, 1, 0], [0, 0, 0, 1, 0, 0, 0, 1], [0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 1]]) self.kf.H = np.array( [[1, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0]]) # self.kf.R[2:, 2:] *= 10. self.kf.R[:, :] *= FLAGS.measurement_noise # Give high uncertainty to the unobservable initial velocities. self.kf.P[4:, 4:] *= 10000. 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 # @mhsung self.bbox_idx = -1 self.class_idx = -1 # bbox: [x0, y0, x1, y2, bbox_idx, class_index, ...] # Store bounding box index and class index if given. if len(bbox) >= 5: self.bbox_idx = bbox[4] if len(bbox) >= 6: self.class_idx = bbox[5]
def test_1d(): global inf f = KalmanFilter(dim_x=2, dim_z=1) inf = InformationFilter(dim_x=2, dim_z=1) # ensure __repr__ doesn't assert str(inf) 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 = [] s = Saver(inf) 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) print(inf.y) s.save() 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 tracker1(): # # # Design 2D filter # # # 1. Choose state vars - x # 2. Design state trans. Function - F tracker = KalmanFilter(dim_x=4, dim_z=2) dt = 1. # time step 1 second tracker.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) # 3. Design Process Noise Mat - Q v = 0.05 q = Q_discrete_white_noise(dim=2, dt=dt, var=v) tracker.Q = block_diag(q, q) # 4. B # 5. Design measurement function tracker.H = np.array([[1/0.3048, 0, 0, 0], [0, 0, 1/0.3048, 0]]) # 6. Design Meas. Noise Mat - R tracker.R = np.array([[5, 0],[0, 5]]) # Init conditions tracker.x = np.array([[0, 0, 0, 0]]).T tracker.P = np.eye(4) * 500. return tracker
def rot_box_kalman_filter(initial_state, Q_std, R_std): """ Tracks a 2D rectangular object (e.g. a bounding box) whose state includes position, centroid velocity, dimensions, and rotation angle. Parameters ---------- initial_state : sequence of floats [x, vx, y, vy, w, h, phi] Q_std : float Standard deviation to use for process noise covariance matrix R_std : float Standard deviation to use for measurement noise covariance matrix Returns ------- kf : filterpy.kalman.KalmanFilter instance """ kf = KalmanFilter(dim_x=7, dim_z=5) dt = 1.0 # time step # state mean and covariance kf.x = np.array([initial_state]).T kf.P = np.eye(kf.dim_x) * 500. # no control inputs kf.u = 0. # state transition matrix kf.F = np.eye(kf.dim_x) kf.F[0, 1] = kf.F[2, 3] = dt # measurement matrix - maps from state space to observation space, so # shape is dim_z x dim_x. kf.H = np.zeros([kf.dim_z, kf.dim_x]) # z = Hx. H has nonzero coefficients for the following components of kf.x: # x y w h phi kf.H[0, 0] = kf.H[1, 2] = kf.H[2, 4] = kf.H[3, 5] = kf.H[4, 6] = 1.0 # measurement noise covariance kf.R = np.eye(kf.dim_z) * R_std**2 # process noise covariance for x-vx or y-vy pairs q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2) # diagonal process noise sub-matrix for width, height, and phi qq = Q_std**2*np.eye(3) # process noise covariance matrix for full state kf.Q = block_diag(q, q, qq) return kf
def const_vel_filter_2d(dt, x_ndim=1, P_diag=(1., 1, 1, 1), R_std=1., Q_var=0.0001): """ helper, constructs 1d, constant velocity filter""" kf = KalmanFilter(dim_x=4, dim_z=2) kf.x = np.array([[0., 0., 0., 0.]]).T kf.P *= np.diag(P_diag) 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.R *= np.eye(2) * (R_std**2) q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_var) kf.Q = block_diag(q, q) return kf
def test_batch_filter(): f = KalmanFilter(dim_x=2, dim_z=1) f.x = np.array([2.0, 0]) # initial state (location and velocity) f.F = np.array([[1.0, 1.0], [0.0, 1.0]]) # state transition matrix f.H = np.array([[1.0, 0.0]]) # Measurement function f.P *= 1000.0 # covariance matrix f.R = 5 # state uncertainty f.Q = 0.0001 # process uncertainty zs = [None, 1.0, 2.0] m, c, _, _ = f.batch_filter(zs, update_first=False) m, c, _, _ = f.batch_filter(zs, update_first=True)
def __init__(self,bb): 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.P[4:,4:] *= 1000. self.kf.P *= 10. self.kf.Q[-1,-1] *= 0.01 self.kf.Q[4:,4:] *= 0.01 self.kf.x[:4,0] = np.array(bb2z(bb)) self.history = [] self.predicted = False
def test_procedural_batch_filter(): f = KalmanFilter (dim_x=2, dim_z=1) f.x = np.array([2., 0]) f.F = np.array([[1.,1.], [0.,1.]]) f.H = np.array([[1.,0.]]) f.P = np.eye(2)*1000. f.R = np.eye(1)*5 f.Q = Q_discrete_white_noise(2, 1., 0.0001) f.test_matrix_dimensions() x = np.array([2., 0]) F = np.array([[1.,1.], [0.,1.]]) H = np.array([[1., 0.]]) P = np.eye(2)*1000. R = np.eye(1)*5 Q = Q_discrete_white_noise(2, 1., 0.0001) zs = [13., None, 1., 2.]*10 m,c,_,_ = f.batch_filter(zs,update_first=False) n = len(zs) # test both list of matrices, and single matrix forms mp, cp, _, _ = batch_filter(x, P, zs, F, Q, [H]*n, R) for x1, x2 in zip(m, mp): assert np.allclose(x1, x2) for p1, p2 in zip(c, cp): assert np.allclose(p1, p2)
def dog_tracking_filter(R,Q=0,cov=1.): f = KalmanFilter (dim_x=2, dim_z=1) f.x = np.matrix([[0], [0]]) # initial state (location and velocity) f.F = np.matrix([[1,1],[0,1]]) # state transition matrix f.H = np.matrix([[1,0]]) # Measurement function f.R = R # measurement uncertainty f.P *= cov # covariance matrix f.Q = Q return f
def createPersonKF(x, y): kalman_filter = KalmanFilter(dim_x=6, dim_z=2) # kalman_filter = KalmanFilter(dim_x=4, dim_z=4) dt = 0.1 dt2 = 0.005 # KF_F = np.array([[1., dt, 0 , 0], # [0 , 1., 0 , 0], # [0 , 0 , 1., dt], # [0 , 0 , 0 , 1.]]) KF_Fca = np.array([[1.0, dt, dt2 / 2], [0, 1.0, dt], [0, 0, 1]]) KF_F = np.vstack( (np.hstack((KF_Fca, np.zeros((3, 3)))), np.hstack((np.zeros((3, 3)), KF_Fca))) # np.zeros((3,3)))), ) # , np.zeros((3,3)))) )))#, # np.hstack((np.zeros((3,3)),np.zeros((3,3)), KF_Fca)))) KF_q = 0.7 # 0.3 # KF_Q = np.vstack((np.hstack((Q_discrete_white_noise(2, dt=0.1, var=KF_q),np.zeros((2,2)))),np.hstack((np.zeros((2,2)),Q_discrete_white_noise(2, dt=0.1, var=KF_q))))) KF_Q = np.vstack( ( np.hstack((Q_discrete_white_noise(3, dt=0.1, var=KF_q), np.zeros((3, 3)))), np.hstack((np.zeros((3, 3)), Q_discrete_white_noise(3, dt=0.1, var=KF_q))), ) ) KF_pd = 25.0 KF_pv = 10.0 KF_pa = 30.0 KF_P = np.diag([KF_pd, KF_pv, KF_pa, KF_pd, KF_pv, KF_pa]) KF_rd = 0.05 KF_rv = 0.2 # 0.5 KF_ra = 2 # 0.5 KF_R = np.diag([KF_rd, KF_rd]) # KF_R = np.diag([KF_rd,KF_rd, KF_rv, KF_rv]) # KF_R = np.diag([KF_rd,KF_rd, KF_rv, KF_rv, KF_ra, KF_ra]) # KF_H = np.array([[1.,0,0,0],[0,0,1.,0]]) # KF_H = np.array([[1.,0,0,0],[0,0,1.,0],[0,1.,0,0],[0,0,0,1.]]) KF_H = np.array([[1.0, 0, 0, 0, 0, 0], [0, 0, 0, 1.0, 0, 0]]) # kalman_filter.x = np.array([x,0,y,0]) kalman_filter.x = np.array([x, 0, 0, y, 0, 0]) kalman_filter.F = KF_F kalman_filter.H = KF_H kalman_filter.Q = KF_Q kalman_filter.B = 0 kalman_filter.R = KF_R kalman_filter.P = KF_P return kalman_filter
def __init__(self, bbox3D, info): """ Initialises a tracker using initial bounding box. """ #define constant velocity model self.kf = KalmanFilter(dim_x=10, dim_z=7) self.kf.F = np.array([ [1, 0, 0, 0, 0, 0, 0, 1, 0, 0], # state transition matrix [0, 1, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1] ]) self.kf.H = np.array([ [1, 0, 0, 0, 0, 0, 0, 0, 0, 0], # measurement function, [0, 1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0] ]) # with angular velocity # self.kf = KalmanFilter(dim_x=11, dim_z=7) # self.kf.F = np.array([[1,0,0,0,0,0,0,1,0,0,0], # state transition matrix # [0,1,0,0,0,0,0,0,1,0,0], # [0,0,1,0,0,0,0,0,0,1,0], # [0,0,0,1,0,0,0,0,0,0,1], # [0,0,0,0,1,0,0,0,0,0,0], # [0,0,0,0,0,1,0,0,0,0,0], # [0,0,0,0,0,0,1,0,0,0,0], # [0,0,0,0,0,0,0,1,0,0,0], # [0,0,0,0,0,0,0,0,1,0,0], # [0,0,0,0,0,0,0,0,0,1,0], # [0,0,0,0,0,0,0,0,0,0,1]]) # self.kf.H = np.array([[1,0,0,0,0,0,0,0,0,0,0], # measurement function, # [0,1,0,0,0,0,0,0,0,0,0], # [0,0,1,0,0,0,0,0,0,0,0], # [0,0,0,1,0,0,0,0,0,0,0], # [0,0,0,0,1,0,0,0,0,0,0], # [0,0,0,0,0,1,0,0,0,0,0], # [0,0,0,0,0,0,1,0,0,0,0]]) # self.kf.R[0:,0:] *= 10. # measurement uncertainty self.kf.P[ 7:, 7:] *= 1000. #state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix self.kf.P *= 10. # self.kf.Q[-1,-1] *= 0.01 # process uncertainty self.kf.Q[7:, 7:] *= 0.01 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
class Localization: """ localization module for Boat-Udrone system """ rospy.init_node('LocalizationUdrone', anonymous=True) last_t = rospy.Time.now() def __init__(self): rospy.Subscriber('/aruco_single/pose', PoseStamped, callback=self.aruco_pose_subscriber) self.observation = None self.listener = tf.TransformListener() # Kalman filter definition self.kf = KalmanFilter(dim_x=3, dim_z=1) self.dt = 0.01 self.kf.x = np.array([[0.5], [0.5], [0.5]]) self.kf.P = np.array([[2, 0, 0], [0, 1, 0], [0, 0, 1]]) self.kf.H = np.array([[1., 0, 0]]) self.kf.R = np.eye(1) * 1.5 self.kf.Q = np.eye(3) * 0.002 self.kf.F = np.array([[1., self.dt, -self.dt], [0., 1., 0.], [0, 0, 1.]]) # Variables to maintain kalman update only on new aruco observation self.aruco_previous_timestamp = rospy.Time.now() self.aruco_pose_timestamp = rospy.Time.now() self.is_observation = False # Plots to stop_count iterations self.plot = True self.stop_count = 100 # 5000, means 50 sec if dt is 0.01 self.count = 0 if self.plot: self.save_time = np.empty((0, 1)) # n x timestamps self.save_observation = np.empty((0, 7)) # n x 7, where 7 values are x, y, z and quaternion self.save_ground_truth = np.empty((0, 7)) # n x 7, where 7 values are x, y, z and quaternion self.save_estimate = np.empty((0, 3)) # n x 3, where 2 values are the state estimates x and xdot self.save_observation_time = np.empty((0, 1)) # n x timestamps self.run_filter() self.save_data_to_file() self.plot_fusion() def save_data_to_file(self): np.save('logs/kf_with_boat_ground_truth.npy', self.save_ground_truth) np.save('logs/kf_with_boat_observations.npy', self.save_observation) np.save('logs/kf_with_boat_observation_time.npy', self.save_observation_time) np.save('logs/kf_with_boat_estimates.npy', self.save_estimate) def aruco_pose_subscriber(self, msg): """ This subscriber publishes the timestamp of the aruco detection. This will make sure the rate of Kalman update is synchronized to image detections """ self.aruco_pose_timestamp = rospy.Time(secs=msg.header.stamp.secs, nsecs=msg.header.stamp.nsecs) def run_filter(self): rate = rospy.Rate(100.0) # print(kf.x) while not rospy.is_shutdown(): try: # (trans, rot) = listener.lookupTransform('to', 'from', rospy.Time(0)) # timestamp is used to make sure update happens only after an observation if (self.aruco_pose_timestamp - self.aruco_previous_timestamp) > rospy.Duration(0): self.is_observation = True (trans, rot) = self.listener.lookupTransform('/udrone', '/world', self.aruco_pose_timestamp) self.aruco_previous_timestamp = self.aruco_pose_timestamp print("Aruco Observation: ", trans) if self.plot: (gtrans, grot) = self.listener.lookupTransform('/ground_truth_udrone', '/ground_truth_heron', rospy.Time(0)) print("Ground Truth: ", gtrans) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as Ex: print(Ex) rate.sleep() continue self.kf.predict() print("Predicted pos x: ", self.kf.x[0][0]) # Do kalman update call only when the camera detects an aruco marker if self.is_observation: self.kf.update(np.array([trans[0]])) self.is_observation = False if self.plot: self.save_observation_time = np.append(self.save_observation_time, np.array([[self.count]]), axis=0) self.save_observation = np.append(self.save_observation, np.array([trans + rot]), axis=0) print("Updated pos x : ", self.kf.x[0][0]) if self.plot: self.count += 1 self.save_time = np.append(self.save_time, np.array([[self.count]]), axis=0) self.save_estimate = np.append(self.save_estimate, np.array([self.kf.x.T.flatten()]), axis=0) self.save_ground_truth = np.append(self.save_ground_truth, np.array([gtrans + grot]), axis=0) print(self.count) if self.count > self.stop_count: break rate.sleep() def plot_fusion(self, dim=2, axis=0): if dim == 2: # 2D plots ax = plt.subplot(111) if self.save_observation_time.shape[0] > 0: ax.scatter(self.save_observation_time, self.save_observation[:, axis], marker='o', c="green", label='Sensor Observation') # 0 is x-axis, 1 is x-axis vel # 2 is y-axis, 3 is y-axis vel # 4 is z-axis, 5 is z-axis vel ax.plot(self.save_time, self.save_estimate[:, axis*2], ls='-', label='State Estimate') ax.plot(self.save_time, self.save_ground_truth[:, axis], ls='-.', label='Ground Truth') plt.title("Sensor observation v/s State estimate v/s Ground truth") ax.set_xlabel("Time") ax.set_ylabel("Position") ax.legend() plt.legend() plt.show()
class KalmanBoxTracker(object): """ This class represents the internel state of individual tracked objects observed as bbox. """ count = 0 def __init__(self, bbox3D, info): """ Initialises a tracker using initial bounding box. """ #define constant velocity model self.kf = KalmanFilter(dim_x=10, dim_z=7) self.kf.F = np.array([ [1, 0, 0, 0, 0, 0, 0, 1, 0, 0], # state transition matrix [0, 1, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1] ]) self.kf.H = np.array([ [1, 0, 0, 0, 0, 0, 0, 0, 0, 0], # measurement function, [0, 1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0] ]) # with angular velocity # self.kf = KalmanFilter(dim_x=11, dim_z=7) # self.kf.F = np.array([[1,0,0,0,0,0,0,1,0,0,0], # state transition matrix # [0,1,0,0,0,0,0,0,1,0,0], # [0,0,1,0,0,0,0,0,0,1,0], # [0,0,0,1,0,0,0,0,0,0,1], # [0,0,0,0,1,0,0,0,0,0,0], # [0,0,0,0,0,1,0,0,0,0,0], # [0,0,0,0,0,0,1,0,0,0,0], # [0,0,0,0,0,0,0,1,0,0,0], # [0,0,0,0,0,0,0,0,1,0,0], # [0,0,0,0,0,0,0,0,0,1,0], # [0,0,0,0,0,0,0,0,0,0,1]]) # self.kf.H = np.array([[1,0,0,0,0,0,0,0,0,0,0], # measurement function, # [0,1,0,0,0,0,0,0,0,0,0], # [0,0,1,0,0,0,0,0,0,0,0], # [0,0,0,1,0,0,0,0,0,0,0], # [0,0,0,0,1,0,0,0,0,0,0], # [0,0,0,0,0,1,0,0,0,0,0], # [0,0,0,0,0,0,1,0,0,0,0]]) # self.kf.R[0:,0:] *= 10. # measurement uncertainty self.kf.P[ 7:, 7:] *= 1000. #state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix self.kf.P *= 10. # self.kf.Q[-1,-1] *= 0.01 # process uncertainty self.kf.Q[7:, 7:] *= 0.01 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 def update(self, bbox3D, info): """ Updates the state vector with observed bbox. """ self.time_since_update = 0 self.history = [] self.hits += 1 self.hit_streak += 1 # number of continuing hit if self.still_first: self.first_continuing_hit += 1 # number of continuing hit in the fist time ######################### orientation correction if self.kf.x[3] >= np.pi: self.kf.x[3] -= np.pi * 2 # make the theta still in the range if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2 new_theta = bbox3D[3] if new_theta >= np.pi: new_theta -= np.pi * 2 # make the theta still in the range if new_theta < -np.pi: new_theta += np.pi * 2 bbox3D[3] = new_theta predicted_theta = self.kf.x[3] if abs(new_theta - predicted_theta) > np.pi / 2.0 and abs( new_theta - predicted_theta ) < np.pi * 3 / 2.0: # if the angle of two theta is not acute angle self.kf.x[3] += np.pi if self.kf.x[3] > np.pi: self.kf.x[3] -= np.pi * 2 # make the theta still in the range if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2 # now the angle is acute: < 90 or > 270, convert the case of > 270 to < 90 if abs(new_theta - self.kf.x[3]) >= np.pi * 3 / 2.0: if new_theta > 0: self.kf.x[3] += np.pi * 2 else: self.kf.x[3] -= np.pi * 2 ######################### self.kf.update(bbox3D) if self.kf.x[3] >= np.pi: self.kf.x[3] -= np.pi * 2 # make the theta still in the range if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2 self.info = info def predict(self): """ Advances the state vector and returns the predicted bounding box estimate. """ self.kf.predict() if self.kf.x[3] >= np.pi: self.kf.x[3] -= np.pi * 2 if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2 self.age += 1 if (self.time_since_update > 0): self.hit_streak = 0 self.still_first = False self.time_since_update += 1 self.history.append(self.kf.x) return self.history[-1] def get_state(self): """ Returns the current bounding box estimate. """ return self.kf.x[:7].reshape((7, ))
class KalmanBoxTracker(object): """ This class represents the internel state of individual tracked objects observed as bbox. """ count = 0 def __init__(self, bbox): """ Initialises a tracker using initial bounding box. """ # define constant velocity model self.kf = KalmanFilter(dim_x=8, dim_z=4) self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0, 0], [0, 1, 0, 0, 0, 1, 0, 0], [0, 0, 1, 0, 0, 0, 1, 0], [0, 0, 0, 1, 0, 0, 0, 1], [0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 1]]) self.kf.H = np.array([[1, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0]]) self.kf.R[ 2:, 2:] *= 10. #传感器测量的不确定性,越大认为传感器测得越不准,预测距离测试值越远;越小认为传感器测得准,预测越近测量值 self.kf.P[ 4:, 4:] *= 1000. # give high uncertainty to the unobservable initial velocities self.kf.P *= 10. #关键值P,它是误差协方差初始值,表示我们对当前预测状态的信任度,它越小说明我们越相信当前预测状态; self.kf.Q[ -1, -1] *= 0.01 #Q值越大我们对于预测的信任度就越低,而对测量值的信任度就变高;如果Q值无穷大,那么我们只信任测量值; self.kf.Q[ 4:, 4:] *= 0.01 #Q值为过程噪声,越小系统越容易收敛,我们对模型预测的值信任度越高;但是太小则容易发散,如果Q为零,那么我们只相信预测值; 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 def update(self, bbox): """ Updates the state vector with observed bbox. """ self.time_since_update = 0 self.history = [] self.hits += 1 self.hit_streak += 1 self.kf.update(convert_bbox_to_z(bbox)) def predict(self): """ Advances the state vector and returns the predicted bounding box estimate. """ #print(self.kf.x) if ((self.kf.x[6] + self.kf.x[2]) <= 0): self.kf.x[6] *= 0.0 self.kf.predict() self.age += 1 if (self.time_since_update > 0): self.hit_streak = 0 self.time_since_update += 1 self.history.append(convert_x_to_bbox(self.kf.x)) return self.history[-1] def get_state(self): """ Returns the current bounding box estimate. """ # print("111",self.kf.x) # print("222",convert_x_to_bbox(self.kf.x)) #if self.kf.x[] a = convert_x_to_bbox(self.kf.x) if a[0][0] < 0: a[0][0] = 0 if a[0][1] < 0: a[0][1] = 0 return a
def setup_kalman(self): # Optical Flow velocity in the x direction self.flow_x = KalmanFilter(dim_x=cfg.flow.dim_x, dim_z=cfg.flow.dim_z) self._setup_kalman(self.flow_x, cfg.flow) # OPtical Flow velocity in the y self.flow_y = KalmanFilter(dim_x=cfg.flow.dim_x, dim_z=cfg.flow.dim_z) self._setup_kalman(self.flow_y, cfg.flow) # Velocity of left wheels from encoder self.vel_left = KalmanFilter(dim_x=cfg.vel_left.dim_x, dim_z=cfg.vel_left.dim_z) self._setup_kalman(self.vel_left, cfg.vel_left) # Velocity of the right wheels from encoder self.vel_right = KalmanFilter(dim_x=cfg.vel_right.dim_x, dim_z=cfg.vel_right.dim_z) self._setup_kalman(self.vel_right, cfg.vel_right) # Rotational velocity from gyroscope self.rotation = KalmanFilter(dim_x=cfg.rotation.dim_x, dim_z=cfg.rotation.dim_z) self._setup_kalman(self.rotation, cfg.rotation) # Overall motion with all sensors included self.odometry = KalmanFilter(dim_x=cfg.odometry.dim_x, dim_z=cfg.odometry.dim_z) self._setup_kalman(self.odometry, cfg.odometry) # Rotational Velocity self.rot_vel = KalmanFilter(dim_x=cfg.rotation.dim_x, dim_z=cfg.rotation.dim_z) self._setup_kalman(self.rot_vel, cfg.rotation)
class KalmanBoxTracker(object): """ This class represents the internel state of individual tracked objects observed as bbox. """ count = 0 def __init__(self, bbox): """ 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. self.kf.P[ 4:, 4:] *= 1000. #give high uncertainty to the unobservable initial velocities self.kf.P *= 10. 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.class_id = bbox[-1] self.score = bbox[4] def update(self, bbox): """ Updates the state vector with observed bbox. """ self.time_since_update = 0 self.history = [] self.hits += 1 self.hit_streak += 1 self.kf.update(convert_bbox_to_z(bbox)) self.class_id = bbox[-1] self.score = bbox[4] def predict(self): """ Advances the state vector and returns the predicted bounding box estimate. """ if ((self.kf.x[6] + self.kf.x[2]) <= 0): self.kf.x[6] *= 0.0 self.kf.predict() self.age += 1 self.time_since_update += 1 self.history.append(convert_x_to_bbox(self.kf.x)) return self.history[-1] def get_state(self): """ Returns the current bounding box estimate. """ return convert_x_to_bbox(self.kf.x)
class KalmanBoxTracker(object): """ This class represents the internel state of individual tracked objects observed as bbox. """ count = 0 # How many object been tracked def __init__(self, bbox, feature, trk_id=None): """ Initialises a tracker using initial bounding box. """ #define constant velocity model self.kf = KalmanFilter(dim_x=7, dim_z=4) # State transition matrix 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]]) # Measurement function 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]]) # Measuerment uncertainty (dim_z, dim_z), default I self.kf.R[2:, 2:] *= 10. # Covariance matrix (dim_x, dim_x), default I self.kf.P[ 4:, 4:] *= 1000. #give high uncertainty to the unobservable initial velocities self.kf.P *= 10. # Process noise (dim_x, dim_x), default I self.kf.Q[-1, -1] *= 0.01 self.kf.Q[4:, 4:] *= 0.01 # Assign the initial value of the state self.kf.x[:4] = convert_bbox_to_z(bbox) self.time_since_update = 0 if trk_id is None: self.id = KalmanBoxTracker.count KalmanBoxTracker.count += 1 else: self.id = trk_id self.history = [] # bbox history between updates self.hits = 0 # total number of updates self.hit_streak = 0 # consective number of updates (probationary perirod) self.age = 0 self.features = [] if feature is not None: self.features.append(feature) self.predected_box = [] def update(self, bbox, feature): """ Updates the state vector with observed bbox. (Unnecessarily update in every frame) """ self.time_since_update = 0 self.history = [] self.hits += 1 self.hit_streak += 1 self.kf.update(convert_bbox_to_z(bbox)) self.features.append(feature) if len(self.features) > 100: self.features.pop(0) def predict(self): """ Advances the state vector and returns the predicted bounding box estimate. (Predict in every frame) """ if ((self.kf.x[6] + self.kf.x[2]) <= 0): self.kf.x[6] *= 0.0 self.kf.predict() self.age += 1 if (self.time_since_update > 0): self.hit_streak = 0 self.time_since_update += 1 self.history.append(convert_x_to_bbox(self.kf.x)) self.predected_box.append(self.history[-1][0]) return self.history[-1] def get_state(self): """ Returns the current bounding box estimate. """ return convert_x_to_bbox(self.kf.x) def get_velocity(self): """ Returns the current velocity estimate. """ u_dot = self.kf.x[4] v_dot = self.kf.x[5] s_dot = self.kf.x[6] return np.array([u_dot, v_dot, s_dot]).reshape((1, 3)) def get_predicted_bbox(self): """ Returns the current bounding box predicted. """ # print(self.history) if not self.predected_box: return self.get_state() predicted_box = self.predected_box self.predected_box = [] return predicted_box
def test_noisy_1d(): 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, 0]) measurements.append(z) # test mahalanobis a = np.zeros(f.y.shape) maha = scipy_mahalanobis(a, f.y, f.SI) assert f.mahalanobis == approx(maha) # 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. s = Saver(f) m, c, _, _ = f.batch_filter(zs, update_first=False, saver=s) s.to_array() assert len(s.x) == len(zs) assert len(s.x) == len(s) # 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 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)
class KalmanBoxTracker: """ This class represents the internel state of individual tracked objects observed as bbox. """ def __init__(self, bbox, tid): """ 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. self.kf.P[ 4:, 4:] *= 1000. #give high uncertainty to the unobservable initial velocities self.kf.P *= 10. self.kf.Q[-1, -1] *= 0.01 self.kf.Q[4:, 4:] *= 0.01 self._pre_z = KalmanBoxTracker._convert_bbox_to_z(bbox) self.kf.x[:4] = self._pre_z self.tid = tid self.predict_times_since_update = 0 self.update_times_since_predict = 0 def _convert_bbox_to_z(bbox): """ Takes a bounding box in the form [x1,y1,x2,y2] and returns z in the form [x,y,s,r] where x,y is the centre of the box and s is the scale/area and r is the aspect ratio """ w = bbox[2] - bbox[0] h = bbox[3] - bbox[1] x = bbox[0] + w / 2. y = bbox[1] + h / 2. s = w * h #scale is just area r = w / h return np.array([x, y, s, r]).reshape((4, 1)) def _convert_x_to_bbox(x): """ Takes a bounding box in the centre form [x,y,s,r] and returns it in the form [x1,y1,x2,y2] where x1,y1 is the top left and x2,y2 is the bottom right """ w = np.sqrt(x[2] * x[3]) h = x[2] / w return np.array( [x[0] - w / 2., x[1] - h / 2., x[0] + w / 2., x[1] + h / 2.]).reshape((1, 4)) def update(self, bbox): """ Updates the state vector with observed bbox. """ self.predict_times_since_update = 0 self.update_times_since_predict += 1 self._pre_z = KalmanBoxTracker._convert_bbox_to_z(bbox) self.kf.update(self._pre_z) return def predict(self): """ Advances the state vector and returns the predicted bounding box estimate. """ if (self.kf.x[6] + self.kf.x[2]) <= 0: self.kf.x[6] *= 0.0 self.kf.predict() self.update_times_since_predict = 0 self.predict_times_since_update += 1 return KalmanBoxTracker._convert_x_to_bbox(self.kf.x) def stay(self): self.kf.update(self._pre_z) return
class OneEyeTracker(): """One Eye Tracker using a Kalman Filter Parameters ---------- x_init : np.array Initial states of the the eye: [x, vx, ax, y, vy, ay, size] """ def __init__(self, x_init): # SETTINGS # Prediction Errors axdev = 10 # pixels/s^2 aydev = 10 # pixels/s^2 sdev = 10 # pixels # Measurement Errors xdev_measure = 5 ydev_measure = 5 sdev_measure = 2 # Initial Errors of States xdev_init = 240 ydev_init = 320 sdev_init = 10 # FPS fps = 30 # KALMAN # Initialize the filter's matrices self.my_filter = KalmanFilter(dim_x=7, dim_z=3) self.my_filter.F = np.array( [[1, 1 / float(fps), np.power(1 / float(fps), 2) / 2, 0, 0, 0, 0], [0, 1, 1 / float(fps), 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 1, 1 / float(fps), np.power(1 / float(fps), 2) / 2, 0], [0, 0, 0, 0, 1, 1 / float(fps), 0], [0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 1]]) # state transition matrix self.my_filter.Q = np.array( [[ np.power(axdev, 2) * np.power(1 / float(fps), 4) / 36, np.power(axdev, 2) * np.power(1 / float(fps), 3) / 12, np.power(axdev, 2) * np.power(1 / float(fps), 2) / 6, 0, 0, 0, 0 ], [ np.power(axdev, 2) * np.power(1 / float(fps), 3) / 12, np.power(axdev, 2) * np.power(1 / float(fps), 2) / 4, np.power(axdev, 2) * 1 / float(fps) * 1 / 2, 0, 0, 0, 0 ], [ np.power(axdev, 2) * np.power(1 / float(fps), 2) / 6, np.power(axdev, 2) * 1 / float(fps) * 1 / 2, np.power(axdev, 2), 0, 0, 0, 0 ], [ 0, 0, 0, np.power(aydev, 2) * np.power(1 / float(fps), 4) / 36, np.power(aydev, 2) * np.power(1 / float(fps), 3) / 12, np.power(aydev, 2) * np.power(1 / float(fps), 2) / 6, 0 ], [ 0, 0, 0, np.power(aydev, 2) * np.power(1 / float(fps), 3) / 12, np.power(aydev, 2) * np.power(1 / float(fps), 2) / 4, np.power(aydev, 2) * 1 / float(fps) * 1 / 2, 0 ], [ 0, 0, 0, np.power(aydev, 2) * np.power(1 / float(fps), 2) / 6, np.power(aydev, 2) * 1 / float(fps) * 1 / 2, np.power(aydev, 2), 0 ], [0, 0, 0, 0, 0, 0, np.power(sdev, 2)]]) # process uncertainty self.my_filter.H = np.array([[1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1]]) # Measurement function self.my_filter.R = np.array([[np.power(xdev_measure, 2), 0, 0], [0, np.power(ydev_measure, 2), 0], [0, 0, np.power(sdev_measure, 2)]]) # state uncertainty self.my_filter.P = np.array( [[np.power(xdev_init, 2), 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, np.power(ydev_init, 2), 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, pow(sdev_init, 2)]]) # coovariance initizialitation self.my_filter.x = x_init # initial state (location(x,y), velocity(x,y), acceleration(x,y), and size) def predict(self): """Predict the future states of the Kalman Filter Returns ------- x : np.array States of the of the eye after prediction [x, vx, ax, y, vy, ay, size] """ self.my_filter.predict() return self.my_filter.x def update(self, measurement): """Update the states of the Kalman Filter with a measurement Parameters ------- measurement : np.array Measurement of the states of the eye [x, y, size] Returns ------- x : np.array States of the of the eye after measurement [x, vx, ax, y, vy, ay, size] """ self.my_filter.update(measurement) return self.my_filter.x
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 = .0001 # 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 = UnscentedKalmanFilter(dim_x=2, dim_z=1, dt=dt, hx=o_func, fx=t_func, points=points) ukf.x = np.array([0., 1.]) ukf.R = np.copy(oc) ukf.Q = np.copy(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 = np.copy(oc) kf.Q = np.copy(tc) kf.H = np.copy(H) kf.F = np.copy(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 sp = JulierSigmaPoints(n=3, kappa=0.) f = UnscentedKalmanFilter(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) results = [] zs = [] kfxs = [] for t in range(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) 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')
class KalmanBoxTracker(object): """ This class represents the internel state of individual tracked objects observed as bbox. """ count = 0 def __init__(self, bbox): """ Initialises a tracker using initial bounding box. """ #define constant velocity model self.kf = KalmanFilter(dim_x=8, dim_z=4) self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0.5, 0], [0, 1, 0, 0, 0, 1, 0, 0.5], [0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 1, 0], [0, 0, 0, 0, 0, 1, 0, 1], [0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 1]]) self.kf.H = np.array([[1, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0]]) self.kf.R[:, :] *= 25.0 # set measurement uncertainty for positions self.kf.Q[:2, : 2] = 0.0 # process uncertainty for positions is zero - only moves due to velocity, leave process for width height as 1 to account for turning self.kf.Q[ 2:4, 2: 4] *= 0.1 # process uncertainty for width/height for turning self.kf.Q[ 4:6, 4: 6] = 0.0 # process uncertainty for velocities is zeros - only accelerates due to accelerations self.kf.Q[6:, 6:] *= 0.01 # process uncertainty for acceleration self.kf.P[4:, 4:] *= 5.0 # maximum speed z = mothe.yolo.convert_bbox_to_kfx(bbox) self.kf.x[:4] = z self.time_since_update = 0 self.id = mothe.yolo.KalmanBoxTracker.count mothe.yolo.KalmanBoxTracker.count += 1 self.hits = 1 self.hit_streak = 1 self.age = 1 self.score = bbox[4] def update(self, bbox): """ Updates the state vector with observed bbox. """ self.time_since_update = 0 self.hits += 1 self.hit_streak += 1 self.score = (self.score * (self.hits - 1.0) / float(self.hits)) + (bbox[4] / float(self.hits)) z = mothe.yolo.convert_bbox_to_kfx(bbox) self.kf.update(z) def predict(self): """ Advances the state vector and returns the predicted bounding box estimate. """ self.kf.predict() self.age += 1 if (self.time_since_update > 0): self.hit_streak = 0 self.time_since_update += 1 def get_state(self): """ Returns the current bounding box estimate. """ return convert_kfx_to_bbox(self.kf.x) def get_distance(self, y): """ Returns the mahalanobis distance to the given point. """ b1 = convert_kfx_to_bbox(self.kf.x[:4])[0] return (bbox_iou(b1, y))
class KalmanBoxTracker(object): """ This class represents the internal state of individual tracked objects observed as bbox. """ count = 1 # benchmark starts with 1 def __init__(self, bbox): """ Initialises a tracker using initial bounding box. """ self.original_id = bbox[5] # <--- add to keep track of original IDs #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. self.kf.P[ 4:, 4:] *= 1000. #give high uncertainty to the unobservable initial velocities self.kf.P *= 10. self.kf.Q[-1, -1] *= 0.01 self.kf.Q[4:, 4:] *= 0.01 z = convert_bbox_to_z(bbox) self.kf.x[:4] = z self.original_center = z # <--- keep track of where this object first appeared 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.speed = 0 def update(self, bbox): """ Updates the state vector with observed bbox. """ self.time_since_update = 0 self.history = [] self.hits += 1 self.hit_streak += 1 self.original_id = bbox[5] # <--- update to keep track of original IDs z = convert_bbox_to_z(bbox) self.kf.update(z) self.speed = np.linalg.norm(z[:2] - self.original_center[:2]) / self.age def predict(self): """ Advances the state vector and returns the predicted bounding box estimate. """ if ((self.kf.x[6] + self.kf.x[2]) <= 0): self.kf.x[6] *= 0.0 self.kf.predict() self.age += 1 if (self.time_since_update > 0): self.hit_streak = 0 self.time_since_update += 1 self.history.append(convert_x_to_bbox(self.kf.x)) return self.history[-1] def get_state(self): """ Returns the current bounding box estimate. """ return convert_x_to_bbox(self.kf.x)
def test_z_checks(): kf = KalmanFilter(dim_x=3, dim_z=1) kf.update(3.) kf.update([3]) kf.update((3)) kf.update([[3]]) kf.update(np.array([[3]])) try: kf.update([[3, 3]]) assert False, "accepted bad z shape" except ValueError: pass kf = KalmanFilter(dim_x=3, dim_z=2) kf.update([3, 4]) kf.update([[3, 4]]) kf.update(np.array([[3, 4]])) kf.update(np.array([[3, 4]]).T)
def test_1d_0P(): global inf 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) f.F = (np.array([[1., 1.], [0., 1.]])) # state transition matrix f.H = np.array([[1., 0.]]) # Measurement function f.R = np.array([[5.]]) # state uncertainty f.Q = np.eye(2) * 0.0001 # process uncertainty f.P = np.diag([20., 20.]) inf.x = f.x.copy() inf.F = f.F.copy() inf.H = np.array([[1., 0.]]) # Measurement function inf.R_inv *= 1. / 5 # state uncertainty inf.Q = np.eye(2) * 0.0001 inf.P_inv = 0.000000000000000000001 #inf.P_inv = inv(f.P) m = [] r = [] r2 = [] zs = [] for t in range(50): # create measurement = t plus white noise z = t + random.randn() * np.sqrt(5) zs.append(z) # perform kalman filtering f.predict() f.update(z) inf.predict() inf.update(z) try: print(t, inf.P) except: pass # save data r.append(f.x[0, 0]) r2.append(inf.x[0, 0]) m.append(z) #assert np.allclose(f.x, inf.x), f'{t}: {f.x.T} {inf.x.T}' if DO_PLOT: plt.plot(m) plt.plot(r) plt.plot(r2)
def test_procedural_batch_filter(): f = KalmanFilter(dim_x=2, dim_z=1) f.x = np.array([2., 0]) f.F = np.array([[1., 1.], [0., 1.]]) f.H = np.array([[1., 0.]]) f.P = np.eye(2) * 1000. f.R = np.eye(1) * 5 f.Q = Q_discrete_white_noise(2, 1., 0.0001) f.test_matrix_dimensions() x = np.array([2., 0]) F = np.array([[1., 1.], [0., 1.]]) H = np.array([[1., 0.]]) P = np.eye(2) * 1000. R = np.eye(1) * 5 Q = Q_discrete_white_noise(2, 1., 0.0001) zs = [13., None, 1., 2.] * 10 m, c, _, _ = f.batch_filter(zs, update_first=False) n = len(zs) mp, cp, _, _ = batch_filter(x, P, zs, [F] * n, [Q] * n, [H] * n, [R] * n) for x1, x2 in zip(m, mp): assert np.allclose(x1, x2) for p1, p2 in zip(c, cp): assert np.allclose(p1, p2)
def __init__(self, x_init): # SETTINGS # Prediction Errors axdev = 10 # pixels/s^2 aydev = 10 # pixels/s^2 sdev = 10 # pixels # Measurement Errors xdev_measure = 5 ydev_measure = 5 sdev_measure = 2 # Initial Errors of States xdev_init = 240 ydev_init = 320 sdev_init = 10 # FPS fps = 30 # KALMAN # Initialize the filter's matrices self.my_filter = KalmanFilter(dim_x=7, dim_z=3) self.my_filter.F = np.array( [[1, 1 / float(fps), np.power(1 / float(fps), 2) / 2, 0, 0, 0, 0], [0, 1, 1 / float(fps), 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 1, 1 / float(fps), np.power(1 / float(fps), 2) / 2, 0], [0, 0, 0, 0, 1, 1 / float(fps), 0], [0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 1]]) # state transition matrix self.my_filter.Q = np.array( [[ np.power(axdev, 2) * np.power(1 / float(fps), 4) / 36, np.power(axdev, 2) * np.power(1 / float(fps), 3) / 12, np.power(axdev, 2) * np.power(1 / float(fps), 2) / 6, 0, 0, 0, 0 ], [ np.power(axdev, 2) * np.power(1 / float(fps), 3) / 12, np.power(axdev, 2) * np.power(1 / float(fps), 2) / 4, np.power(axdev, 2) * 1 / float(fps) * 1 / 2, 0, 0, 0, 0 ], [ np.power(axdev, 2) * np.power(1 / float(fps), 2) / 6, np.power(axdev, 2) * 1 / float(fps) * 1 / 2, np.power(axdev, 2), 0, 0, 0, 0 ], [ 0, 0, 0, np.power(aydev, 2) * np.power(1 / float(fps), 4) / 36, np.power(aydev, 2) * np.power(1 / float(fps), 3) / 12, np.power(aydev, 2) * np.power(1 / float(fps), 2) / 6, 0 ], [ 0, 0, 0, np.power(aydev, 2) * np.power(1 / float(fps), 3) / 12, np.power(aydev, 2) * np.power(1 / float(fps), 2) / 4, np.power(aydev, 2) * 1 / float(fps) * 1 / 2, 0 ], [ 0, 0, 0, np.power(aydev, 2) * np.power(1 / float(fps), 2) / 6, np.power(aydev, 2) * 1 / float(fps) * 1 / 2, np.power(aydev, 2), 0 ], [0, 0, 0, 0, 0, 0, np.power(sdev, 2)]]) # process uncertainty self.my_filter.H = np.array([[1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1]]) # Measurement function self.my_filter.R = np.array([[np.power(xdev_measure, 2), 0, 0], [0, np.power(ydev_measure, 2), 0], [0, 0, np.power(sdev_measure, 2)]]) # state uncertainty self.my_filter.P = np.array( [[np.power(xdev_init, 2), 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, np.power(ydev_init, 2), 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, pow(sdev_init, 2)]]) # coovariance initizialitation self.my_filter.x = x_init # initial state (location(x,y), velocity(x,y), acceleration(x,y), and size)
import numpy as np import matplotlib.pyplot as plt from kalman import KLFilter from filterpy.kalman import KalmanFilter from filterpy.common import Q_discrete_white_noise kl2 = KalmanFilter(dim_x=2, dim_z=2, dim_u=1) N = 200 def train_noise(var): """ This is the noise that hits the train... """ return np.random.normal(0, scale=np.sqrt(var), size=N) def get_true_force(): """ The true force driving the train: """ return np.cos(np.linspace(0, np.pi * 2, N)) * F def train_measurement_noise(var): """ This is the noise due to measurement error """ return np.random.normal(0, scale=np.sqrt(var), size=N)
class Localization: """ localization module for Boat-Udrone system """ rospy.init_node('LocalizationUdrone', anonymous=True) last_t = rospy.Time.now() def __init__(self): rospy.Subscriber('/aruco_single/pose', PoseStamped, callback=self.aruco_pose_subscriber) self.pub_states = rospy.Publisher('udroneboat', UdroneStates) self.observation = None self.listener = tf.TransformListener() # Kalman filter definition self.kf = KalmanFilter(dim_x=6, dim_z=3) self.dt = 0.01 self.kf.x = np.array([[0.5], [0.5], [0.5], [0.5], [5], [0.5]]) self.kf.P = np.array([[2, 0, 0, 0, 0, 0], [0, 2, 0, 0, 0, 0], [0, 0, 2, 0, 0, 0], [0, 0, 0, 2, 0, 0], [0, 0, 0, 0, 2, 0], [0, 0, 0, 0, 0, 2]]) self.kf.H = np.array([[1., 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 1, 0]]) self.kf.R = np.array([[2, 0, 0], [0, 2, 0], [0, 0, 2]]) self.kf.Q = np.array([[0.002, 0, 0, 0, 0, 0], [0, 0.02, 0, 0, 0, 0], [0, 0, 0.002, 0, 0, 0], [0, 0, 0, 0.02, 0, 0], [0, 0, 0, 0, 0.002, 0], [0, 0, 0, 0, 0, 0.02]]) self.kf.F = np.array([[1., self.dt, 0, 0, 0, 0], [0, 1., 0, 0, 0, 0], [0, 0, 1., self.dt, 0, 0], [0, 0, 0, 1., 0, 0], [0, 0, 0, 0, 1., self.dt], [0, 0, 0, 0, 0, 1.]]) # Variables to maintain kalman update only on new aruco observation self.aruco_previous_timestamp = rospy.Time.now() self.aruco_pose_timestamp = rospy.Time.now() self.is_observation = False # Plots to stop_count iterations self.plot = True self.stop_count = 12000 # 6000, means 60 sec if dt is 0.01 self.on_vehicle = True self.count = 0 if self.plot: self.save_time = np.empty((0, 1)) # n x timestamps self.save_observation = np.empty( (0, 7)) # n x 7, where 7 values are x, y, z and quaternion self.save_ground_truth = np.empty( (0, 7)) # n x 7, where 7 values are x, y, z and quaternion self.save_estimate = np.empty(( 0, 6)) # n x 2, where 2 values are the state estimates x and xdot self.save_observation_time = np.empty((0, 1)) # n x timestamps self.run() # self.run_filter() # self.save_data_to_file() # self.plot_fusion(dim=3, axis=0) def run(self): t1 = threading.Thread(target=self.plot_fusion) t2 = threading.Thread(target=self.run_filter) t1.start() t2.start() def save_data_to_file(self): np.save('logs/kf_with_boat_ground_truth.npy', self.save_ground_truth) np.save('logs/kf_with_boat_observations.npy', self.save_observation) np.save('logs/kf_with_boat_observation_time.npy', self.save_observation_time) np.save('logs/kf_with_boat_estimates.npy', self.save_estimate) def aruco_pose_subscriber(self, msg): """ This subscriber publishes the timestamp of the aruco detection. This will make sure the rate of Kalman update is synchronized to image detections """ self.aruco_pose_timestamp = rospy.Time(secs=msg.header.stamp.secs, nsecs=msg.header.stamp.nsecs) def run_filter(self): rate = rospy.Rate(100.0) # print(kf.x) while not rospy.is_shutdown(): try: # (trans, rot) = listener.lookupTransform('to', 'from', rospy.Time(0)) # timestamp is used to make sure update happens only after an observation if (self.aruco_pose_timestamp - self.aruco_previous_timestamp) > rospy.Duration(0): self.is_observation = True (trans, rot) = self.listener.lookupTransform( '/udrone', '/world', self.aruco_pose_timestamp) self.aruco_previous_timestamp = self.aruco_pose_timestamp print("Aruco Observation: ", trans) if self.plot: (gtrans, grot) = self.listener.lookupTransform( '/ground_truth_udrone', '/ground_truth_heron', rospy.Time(0)) print("Ground Truth: ", gtrans) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as Ex: print(Ex) rate.sleep() continue self.kf.predict() print("Predict :", self.kf.x.T.flatten()[0], self.kf.x.T.flatten()[2], self.kf.x.T.flatten()[4]) # Do kalman update call only when the camera detects an aruco marker if self.is_observation: self.kf.update(np.array([trans])) self.is_observation = False if self.plot: self.save_observation_time = np.append( self.save_observation_time, np.array([[self.count]]), axis=0) self.save_observation = np.append(self.save_observation, np.array([trans + rot]), axis=0) print("Update :", self.kf.x.T.flatten()[0], self.kf.x.T.flatten()[2], self.kf.x.T.flatten()[4]) if self.plot: self.count += 1 self.save_time = np.append(self.save_time, np.array([[self.count]]), axis=0) self.save_estimate = np.append(self.save_estimate, np.array( [self.kf.x.T.flatten()]), axis=0) self.save_ground_truth = np.append(self.save_ground_truth, np.array([gtrans + grot]), axis=0) print(self.count) # if you are running this filter on vehicle, disable plot and stop count if self.count > self.stop_count and not self.on_vehicle: break # Publish message for the controller msg = UdroneStates() state = self.kf.x.T.flatten() msg.x = state[0] msg.xvel = state[1] msg.y = state[2] msg.yvel = state[3] msg.z = state[4] msg.zvel = state[5] msg.boatxvel = 0 msg.boatyvel = 0 self.pub_states.publish(msg) rate.sleep() def plot_fusion(self, dim=2, axis=0): if dim == 2: # 2D plots ax = plt.subplot(111) if self.save_observation_time.shape[0] > 0: ax.scatter(self.save_observation_time, self.save_observation[:, axis], marker='o', c="green", label='Sensor Observation') # 0 is x-axis, 1 is x-axis vel # 2 is y-axis, 3 is y-axis vel # 4 is z-axis, 5 is z-axis vel ax.plot(self.save_time, self.save_estimate[:, axis * 2], ls='-', label='State Estimate') ax.plot(self.save_time, self.save_ground_truth[:, axis], ls='-.', label='Ground Truth') plt.title("Sensor observation v/s State estimate v/s Ground truth") ax.set_xlabel("Time") ax.set_ylabel("Position") ax.legend() plt.legend() plt.show() elif dim == 3: # 3D plots # x-axis is the time # y-axis and z-axis will show positions fig = plt.figure() ax = plt.axes(projection='3d') # Plot estimates x = self.save_time y = self.save_estimate[:, axis] z = self.save_estimate[:, axis + 2] # x, x_dot, y, y_dot, z, z_dot ax.plot3D(x, y, z, 'blue') # Plot ground truth x = self.save_time y = self.save_ground_truth[:, axis] z = self.save_ground_truth[:, axis + 1] # x, y, z ax.plot3D(x, y, z, 'orange') # Plot observations x = self.save_observation_time y = self.save_observation[:, axis] z = self.save_observation[:, axis + 1] # x, y, z ax.scatter(x, y, z, color='r') ax.set_xlabel('Time') ax.set_ylabel('X-axis position') ax.set_zlabel('Y-axis position') ax.set_title( 'Sensor observation v/s State estimate v/s Ground truth') plt.show() def animate(self): x = np.random.normal(size=(100, 3)) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') gs = ax.scatter([], [], [], c='red') es = ax.scatter([], [], [], c='blue') def update(i): # print(self.save_time) length = self.save_time.shape[0] - 100 # print(length) if length > 5: time = self.save_time[length:length + 101].flatten() ground_truth = self.save_ground_truth[length:length + 101, :2] ax.set_xlim3d([time[0], time[len(time) - 1]]) gs._offsets3d = (time[:i], ground_truth[:i, 0].flatten(), ground_truth[:i, 1].flatten()) estimate = self.save_estimate[length:length + 101, :2] es._offsets3d = (time[:i], estimate[:i, 0].flatten(), estimate[:i, 1].flatten()) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') ax.set_xlim(-100, 100) ax.set_ylim(-5, 5) ax.set_zlim(-5, 5) ani = matplotlib.animation.FuncAnimation(fig, update, frames=100, interval=10) plt.tight_layout() plt.show()
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 test_default_dims(): kf = KalmanFilter(dim_x=3, dim_z=1) kf.predict() kf.update(np.array([[1.]]).T)
class KalmanBoxTracker(object): """ This class represents the internal state of individual tracked objects observed as bbox. """ count = 0 def __init__(self, bbox): """ Initialize a tracker using initial bounding box Parameter 'bbox' must have 'detected class' int number at the -1 position. """ 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.P[4:, 4:] *= 1000. # give high uncertainty to the unobservable initial velocities self.kf.P *= 10. self.kf.Q[-1, -1] *= 0.5 self.kf.Q[4:, 4:] *= 0.5 self.kf.x[:4] = convert_bbox_to_z(bbox) # STATE VECTOR self.time_since_update = 0 self.id = KalmanBoxTracker.count KalmanBoxTracker.count += 1 self.history = [] self.hits = 0 self.hit_streak = 0 self.age = 0 # keep yolov5 detected class information self.detclass = bbox[5] def update(self, bbox): """ Updates the state vector with observed bbox """ self.time_since_update = 0 self.history = [] self.hits += 1 self.hit_streak += 1 self.kf.update(convert_bbox_to_z(bbox)) def predict(self): """ Advances the state vector and returns the predicted bounding box estimate """ if ((self.kf.x[6] + self.kf.x[2]) <= 0): self.kf.x[6] *= 0.0 self.kf.predict() self.age += 1 if (self.time_since_update > 0): self.hit_streak = 0 self.time_since_update += 1 self.history.append(convert_x_to_bbox(self.kf.x)) return self.history[-1] def get_state(self): """ Returns the current bounding box estimate # test arr1 = np.array([[1,2,3,4]]) arr2 = np.array([0]) arr3 = np.expand_dims(arr2, 0) np.concatenate((arr1,arr3), axis=1) """ arr_detclass = np.expand_dims(np.array([self.detclass]), 0) arr_u_dot = np.expand_dims(self.kf.x[4], 0) arr_v_dot = np.expand_dims(self.kf.x[5], 0) arr_s_dot = np.expand_dims(self.kf.x[6], 0) return np.concatenate((convert_x_to_bbox(self.kf.x), arr_detclass, arr_u_dot, arr_v_dot, arr_s_dot), axis=1)
class Flodometry(object): """Main class of Node flodometry Attributes: kf (KalmanFilter): A kalman filter to filter sensor values and estimate odometry pub (rospy.Publisher): Publisher to push to odom """ def __init__(self): """Initialize the node set up the Kalman filter and set up subscriber and publisher. """ self.setup_kalman() # Subscribe to the adns topic if __name__ == '__main__': self.setup_ros() def setup_ros(self): self.tf_odom = tf.TransformBroadcaster() rospy.init_node('flodometry') rospy.Subscriber("/optical_flow", motion_read, self.update_flow) rospy.Subscriber("/encoder_values", rpm, self.update_encoders) rospy.Subscriber("/gyro", gyro, self.update_gyro) # Initialize the publisher self.pub = rospy.Publisher("/odom", Odometry, queue_size=10) self.vel_pub = rospy.Publisher("/vel", vel_update, queue_size=10) self.rate = rospy.Rate(60) # 60hz def setup_kalman(self): # Optical Flow velocity in the x direction self.flow_x = KalmanFilter(dim_x=cfg.flow.dim_x, dim_z=cfg.flow.dim_z) self._setup_kalman(self.flow_x, cfg.flow) # OPtical Flow velocity in the y self.flow_y = KalmanFilter(dim_x=cfg.flow.dim_x, dim_z=cfg.flow.dim_z) self._setup_kalman(self.flow_y, cfg.flow) # Velocity of left wheels from encoder self.vel_left = KalmanFilter(dim_x=cfg.vel_left.dim_x, dim_z=cfg.vel_left.dim_z) self._setup_kalman(self.vel_left, cfg.vel_left) # Velocity of the right wheels from encoder self.vel_right = KalmanFilter(dim_x=cfg.vel_right.dim_x, dim_z=cfg.vel_right.dim_z) self._setup_kalman(self.vel_right, cfg.vel_right) # Rotational velocity from gyroscope self.rotation = KalmanFilter(dim_x=cfg.rotation.dim_x, dim_z=cfg.rotation.dim_z) self._setup_kalman(self.rotation, cfg.rotation) # Overall motion with all sensors included self.odometry = KalmanFilter(dim_x=cfg.odometry.dim_x, dim_z=cfg.odometry.dim_z) self._setup_kalman(self.odometry, cfg.odometry) # Rotational Velocity self.rot_vel = KalmanFilter(dim_x=cfg.rotation.dim_x, dim_z=cfg.rotation.dim_z) self._setup_kalman(self.rot_vel, cfg.rotation) def _setup_kalman(self, kf, config): """Sets all of the kalman filter constants see config.kalman_config for more details on each variable Returns: None """ kf.x = config.x kf.F = config.F kf.H = config.H kf.P = config.P kf.Q = config.Q kf.R = config.R def update_flow(self, motion): """Call back to subscriber on /optical flow updates the Kalman filter with the velocity value then calls publish_updates Args: motion (motion_read): Message published to /optical_flow Returns: None """ self.flow_x.predict() self.flow_x.update(motion.dx) self.flow_y.update(motion.dy) def update_gyro(self, data): self.rotation.predict() self.rotation.update(data.theta) self.rot_vel.predict() self.rot_vel.update(data.omega) def update_encoders(self, enc): l_speed = enc.left_rpm self.vel_left.predict() self.vel_left.update(l_speed) r_speed = enc.right_rpm self.vel_right.predict() self.vel_right.update(r_speed) def update(self): flow_x = self.flow_x.x[0] flow_y = self.flow_y.x[0] l_speed = self.vel_left.x[0] r_speed = self.vel_right.x[0] rotation = self.rotation.x[0] # rospy.loginfo('Rotation: {}'.format(rotation)) avg = float(l_speed+r_speed)/2 diff = r_speed-avg # rospy.loginfo('left:{} right:{} avg:{} diff:{}'.format(l_speed, r_speed, avg, diff)) H = cfg.odometry.get_h(theta=self.rotation.x[0]) z = np.array([flow_x, flow_y, avg, rotation]) # x1 = self.odometry.x # rospy.loginfo('Theta: {}, theta_dot: {}'.format(x1[4], x1[5])) self.odometry.predict() # x2 = self.odometry.x # rospy.loginfo('Predicted Theta: {}, theta_dot: {}'.format(x2[4], x2[5])) self.odometry.update(z, H=H) # x3 = self.odometry.x # rospy.loginfo('Updated Theta: {}, theta_dot: {}'.format(x3[4], x3[5])) def publish_updates(self): """Publishes kalman filter data to odometry Args: x (List): List containing current state estimates p (List): List containing current variance estimates Returns: None """ if __name__ == '__main__': while not rospy.is_shutdown(): update = vel_update() update.header.stamp = rospy.Time.now() update.header.frame_id = '/base_link' update.linear_x = self.flow_x.x[0] update.angular_z = self.rot_vel.x[0] self.vel_pub.publish(update) odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = '/odom' self.update() x = self.odometry.x odom.pose.pose.position.x = x[0] odom.pose.pose.position.y = x[2] # odom.pose.covariance[0] = p[0][0] odom.twist.twist.linear.x = x[1] odom.twist.twist.linear.y = x[3] odom.twist.twist.angular.z = x[5] # odom.twist.covariance[0] = p[1][1] odom.pose.pose.orientation.x # rospy.loginfo('[pos_x: {}, vel_x: {}, pos_y: {}, vel_y: {}, theta: {}, vel_theta: {}]'.format(*x)) # quaternion_from_euler will operate on whatever object is passed # to it, that is why I am passing a copy of x[4] because it was # changing it's value! quaternion = quaternion_from_euler(0.0, 0.0, copy.copy(self.rotation.x[0])) odom.pose.pose.orientation.x = quaternion[0] odom.pose.pose.orientation.y = quaternion[1] odom.pose.pose.orientation.z = quaternion[2] odom.pose.pose.orientation.w = quaternion[3] self.pub.publish(odom) self.tf_odom.sendTransform( (x[0], x[2], 0.0), (quaternion[0], quaternion[1], quaternion[2], quaternion[3]), odom.header.stamp, "base_link", "odom") self.rate.sleep() else: pass
import numpy as np from filterpy.kalman import KalmanFilter from filterpy.common import Q_discrete_white_noise import pdb my_filter = KalmanFilter(dim_x=2, dim_z=1) my_filter.x = np.array([[2.], [0.]]) # initial state (location and velocity) my_filter.F = np.array([[1., 1.], [0., 1.]]) # state transition matrix my_filter.H = np.array([[1., 0.]]) # Measurement function my_filter.P *= 1000. # covariance matrix my_filter.R = 5 # state uncertainty pdb.set_trace() my_filter.Q = Q_discrete_white_noise(2, 0.1, .1) # process uncertainty while True: my_filter.predict() my_filter.update(get_some_measurement()) # do something with the output x = my_filter.x do_something_amazing(x)
def __init__(self, bbox3D, info, covariance_id=0, track_score=None, tracking_name='car', use_angular_velocity=False): """ Initialises a tracker using initial bounding box. """ #define constant velocity model if not use_angular_velocity: self.kf = KalmanFilter(dim_x=10, dim_z=7) self.kf.F = np.array([ [1, 0, 0, 0, 0, 0, 0, 1, 0, 0], # state transition matrix [0, 1, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1] ]) self.kf.H = np.array([ [1, 0, 0, 0, 0, 0, 0, 0, 0, 0], # measurement function, [0, 1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0] ]) else: # with angular velocity self.kf = KalmanFilter(dim_x=11, dim_z=7) self.kf.F = np.array([ [1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0], # state transition matrix [0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1] ]) self.kf.H = np.array([ [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], # measurement function, [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0] ]) # Initialize the covariance matrix, see covariance.py for more details self.kf.P, self.kf.Q, self.kf.R = covariance(covariance_id, tracking_name) if not use_angular_velocity: self.kf.P = self.kf.P[:-1, :-1] self.kf.Q = self.kf.Q[:-1, :-1] 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 self.track_score = track_score self.tracking_name = tracking_name self.use_angular_velocity = use_angular_velocity
class VelocityEstimator: """Estimates base velocity of A1 robot. The velocity estimator consists of 2 parts: 1) A state estimator for CoM velocity. Two sources of information are used: The integrated reading of accelerometer and the velocity estimation from contact legs. The readings are fused together using a Kalman Filter. 2) A moving average filter to smooth out velocity readings """ def __init__(self, robot, accelerometer_variance=0.1, sensor_variance=0.1, initial_variance=0.1, moving_window_filter_size=120): """Initiates the velocity estimator. See filterpy documentation in the link below for more details. https://filterpy.readthedocs.io/en/latest/kalman/KalmanFilter.html Args: robot: the robot class for velocity estimation. accelerometer_variance: noise estimation for accelerometer reading. sensor_variance: noise estimation for motor velocity reading. initial_covariance: covariance estimation of initial state. """ self.robot = robot self.filter = KalmanFilter(dim_x=3, dim_z=3, dim_u=3) self.filter.x = np.zeros(3) self._initial_variance = initial_variance self.filter.P = np.eye(3) * self._initial_variance # State covariance self.filter.Q = np.eye(3) * accelerometer_variance self.filter.R = np.eye(3) * sensor_variance self.filter.H = np.eye(3) # measurement function (y=H*x) self.filter.F = np.eye(3) # state transition matrix self.filter.B = np.eye(3) self._window_size = moving_window_filter_size self.moving_window_filter_x = MovingWindowFilter( window_size=self._window_size) self.moving_window_filter_y = MovingWindowFilter( window_size=self._window_size) self.moving_window_filter_z = MovingWindowFilter( window_size=self._window_size) self._estimated_velocity = np.zeros(3) self._last_timestamp = 0 def reset(self): self.filter.x = np.zeros(3) self.filter.P = np.eye(3) * self._initial_variance self.moving_window_filter_x = MovingWindowFilter( window_size=self._window_size) self.moving_window_filter_y = MovingWindowFilter( window_size=self._window_size) self.moving_window_filter_z = MovingWindowFilter( window_size=self._window_size) self._last_timestamp = 0 def _compute_delta_time(self, robot_state): if self._last_timestamp == 0.: # First timestamp received, return an estimated delta_time. delta_time_s = self.robot.time_step else: delta_time_s = (robot_state.tick - self._last_timestamp) / 1000. self._last_timestamp = robot_state.tick return delta_time_s def update(self, robot_state): """Propagate current state estimate with new accelerometer reading.""" delta_time_s = self._compute_delta_time(robot_state) sensor_acc = np.array(robot_state.imu.accelerometer) base_orientation = self.robot.GetBaseOrientation() rot_mat = self.robot.pybullet_client.getMatrixFromQuaternion( base_orientation) rot_mat = np.array(rot_mat).reshape((3, 3)) calibrated_acc = rot_mat.dot(sensor_acc) + np.array([0., 0., -9.8]) self.filter.predict(u=calibrated_acc * delta_time_s) # Correct estimation using contact legs observed_velocities = [] foot_contact = self.robot.GetFootContacts() for leg_id in range(4): if foot_contact[leg_id]: jacobian = self.robot.ComputeJacobian(leg_id) # Only pick the jacobian related to joint motors joint_velocities = self.robot.motor_velocities[leg_id * 3:(leg_id + 1) * 3] leg_velocity_in_base_frame = jacobian.dot(joint_velocities) base_velocity_in_base_frame = -leg_velocity_in_base_frame[:3] observed_velocities.append( rot_mat.dot(base_velocity_in_base_frame)) if observed_velocities: observed_velocities = np.mean(observed_velocities, axis=0) self.filter.update(observed_velocities) vel_x = self.moving_window_filter_x.calculate_average(self.filter.x[0]) vel_y = self.moving_window_filter_y.calculate_average(self.filter.x[1]) vel_z = self.moving_window_filter_z.calculate_average(self.filter.x[2]) self._estimated_velocity = np.array([vel_x, vel_y, vel_z]) @property def estimated_velocity(self): return self._estimated_velocity.copy()
class Kalman: """ This is just a wrapper around the filterpy.kalman.KalmanFilter class to be used in our experiments, adding the likelihood functionality """ def __init__(self, model, measurements, print_results=False): """ model: SimpleModel or StochasticModelTauLeaping measurements: the precomputed measurements of the model, for e.g. [model.measure(t) for t in range(time)] (optional) print_results: useful for debugging purposes """ self.kf = KF(dim_x=model.state_dim, dim_z=model.measurement_dim) # instantiating the underlying KF from filterpy self.measurements = measurements self.kf.x = model.states[0] self.kf.F = model.F self.kf.H = model.H self.kf.Q = model.Q self.kf.R = model.R self.model = model self.print_res = print_results def run(self): """ Running the KF and computing the likelihood. Returns the posterior means, variances and overall likelihood """ # Actually running the KF post_means, post_covs, prior_means, prior_covs = self.kf.batch_filter(self.measurements) # Computing the likelihood, according to Wikipedia's Kalman Filter > Marginal Likelihood equations and following their notations # https://en.wikipedia.org/wiki/Kalman_filter#Marginal_likelihood loglikelihood = 0 for t, z in enumerate(self.measurements): Sk = self.model.H @ prior_covs[t] @ self.model.H.T + self.model.R yk = z - self.model.H @ prior_means[t] sign, logdet = np.linalg.slogdet(Sk) logdet *= sign loglikelihood -= 0.5 * (yk.T @ np.linalg.inv(Sk) @ yk + logdet + self.model.measurement_dim * np.log(2*np.pi)) means = np.array(post_means) variances = np.array(list(map(lambda x: np.sqrt([x[i][i] for i in range(self.model.state_dim)]), post_covs))) if self.print_res: # Printing some results, highly customizable print("Loglikelihood: ", loglikelihood) fig = plt.figure() # plt.title('Kalman Filter Results', fontsize=16, y=1.08) measurements = np.array(self.measurements) times = range(len(self.measurements)) for i in range(self.model.state_dim): ax = fig.add_subplot(self.model.state_dim, 1, i+1) # ax.set_title(f"dimension {i}") ax.plot(times, means[:, i], 'm--', color='red', label='KF means') # ax.plot(times, means[:, i] + variances[:, i], 'c--', label='KF variance') # ax.plot(times, means[:, i] - variances[:, i], 'c--') ax.fill_between(times, means[:, i] + variances[:, i], means[:, i] - variances[:, i], alpha=0.2) ax.plot(times, measurements[:, i], 'ko', label='measurements', markersize=3) ax.set_xlabel("timestep") ax.set_ylabel("value") ax.legend() fig.tight_layout() plt.savefig('KF_1D_run.pdf') plt.show() print("KF ran successfully") return means, variances, loglikelihood
class KalmanBoxTracker(object): """ This class represents the internel state of individual tracked objects observed as bbox. """ count = 0 def __init__(self, bbox): """ 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. self.kf.P[4:, 4:] *= 1000. # give high uncertainty to the unobservable initial velocities self.kf.P *= 10. self.kf.Q[-1, -1] *= 0.01 self.kf.Q[4:, 4:] *= 0.01 self.kf.x[:4] = convert_bbox_to_z(bbox[:4]) # convert_bbox_to_z(bbox) self.time_since_update = 0 self.history = [] self.hits = 0 self.hit_streak = 0 self.age = 0 self.objConfidence = bbox[4] self.objclass = bbox[5] self.Region_path = [] self.Frame_count_path = [] self.id = KalmanBoxTracker.count KalmanBoxTracker.count += 1 """ Updates the state vector with observed bbox. """ def update(self, bbox): self.time_since_update = 0 self.history = [] self.hits += 1 self.hit_streak += 1 self.kf.update(convert_bbox_to_z(bbox)) """ Advances the state vector and returns the predicted bounding box estimate. """ def predict(self): """ Realiza la prediccion de la siguiente posicion del filtro. """ if ((self.kf.x[6] + self.kf.x[2]) <= 0): self.kf.x[6] *= 0.0 self.kf.predict() self.age += 1 if (self.time_since_update > 0): self.hit_streak = 0 self.time_since_update += 1 self.history.append(convert_x_to_bbox(self.kf.x)) return self.history[-1] def get_state(self): """ retorna la caja de deteccion estimada. """ return convert_x_to_bbox(self.kf.x) def get_point_of_interest(self): """ Retorna el centroide de la deteccion, la cual se define como el punto de interes. """ return [round(float(i[0]), 2) for i in self.kf.x[:2]] def get_last_region(self): return self.Region_path[-1] def get_path(self): return self.Region_path def get_frame_count(self): return self.Frame_count_path def get_id(self): return self.id def set_path(self,path): self.Region_path = path def set_frame_count(self,frame_count): self.Frame_count_path = frame_count def add_regions_name_to_path(self,region_name): self.Region_path.append(region_name) def eliminate_None_from_path_and_frame_count(self): path = [] frames = [] for (p,f) in zip(self.get_path(),self.get_frame_count()): if not p is None: path.append(p) frames.append(f) self.set_path(path) self.set_frame_count(frames) def print(self): print("###########################") print("state:", self.get_state()) print("path:", self.get_path()) print("fcount:", self.get_path()) print("hits:", self.hits) print("hit_streak:", self.hit_streak) print("age:", self.age) print("time_since_update:", self.time_since_update) print("###########################")