def ball_filter4(dt,R=1., Q = 0.1): f1 = KalmanFilter(dim=4) g = 10 f1.F = np.mat ([[1., dt, 0, 0,], [0, 1., 0, 0], [0, 0, 1., dt], [0, 0, 0., 1.]]) f1.H = np.mat([[1,0,0,0], [0,0,0,0], [0,0,1,0], [0,0,0,0]]) f1.B = np.mat([[0,0,0,0], [0,0,0,0], [0,0,1.,0], [0,0,0,1.]]) f1.u = np.mat([[0], [0], [-0.5*g*dt**2], [-g*dt]]) f1.R = np.mat(np.eye(4)) * R f1.Q = np.zeros((4,4)) f1.Q[1,1] = Q f1.Q[3,3] = Q f1.x = np.mat([0, 0 , 0, 0]).T f1.P = np.eye(4) * 50. return f1
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 _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 ball_filter6(dt,R=1., Q = 0.1): f1 = KalmanFilter(dim=6) g = 10 f1.F = np.mat ([[1., dt, dt**2, 0, 0, 0], [0, 1., dt, 0, 0, 0], [0, 0, 1., 0, 0, 0], [0, 0, 0., 1., dt, -0.5*dt*dt*g], [0, 0, 0, 0, 1., -g*dt], [0, 0, 0, 0, 0., 1.]]) f1.H = np.mat([[1,0,0,0,0,0], [0,0,0,0,0,0], [0,0,0,0,0,0], [0,0,0,1,0,0], [0,0,0,0,0,0], [0,0,0,0,0,0]]) f1.R = np.mat(np.eye(6)) * R f1.Q = np.zeros((6,6)) f1.Q[2,2] = Q f1.Q[5,5] = Q f1.x = np.mat([0, 0 , 0, 0, 0, 0]).T f1.P = np.eye(6) * 50. f1.B = 0. f1.u = 0 return f1
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 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 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 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 test_noisy_1d(): f = KalmanFilter(dim_x=2, dim_z=1) f.x = np.array([[2.0], [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 fsq = SquareRootKalmanFilter(dim_x=2, dim_z=1) fsq.x = np.array([[2.0], [0.0]]) # initial state (location and velocity) fsq.F = np.array([[1.0, 1.0], [0.0, 1.0]]) # state transition matrix fsq.H = np.array([[1.0, 0.0]]) # Measurement function fsq.P = np.eye(2) * 1000.0 # covariance matrix fsq.R = 5 # state uncertainty fsq.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() fsq.update(z) fsq.predict() assert abs(f.x[0, 0] - fsq.x[0, 0]) < 1.0e-12 assert abs(f.x[1, 0] - fsq.x[1, 0]) < 1.0e-12 # save data results.append(f.x[0, 0]) measurements.append(z) p = f.P - fsq.P print(f.P) print(fsq.P) for i in range(f.P.shape[0]): assert abs(f.P[i, i] - fsq.P[i, i]) < 0.01 # 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, 0]]).T f.P = np.eye(2) * 100.0 m, c, _, _ = f.batch_filter(zs, update_first=False)
def sensor_fusion_test(wheel_sigma=2., gps_sigma=4.): dt = 0.1 kf2 = KalmanFilter(dim_x=2, dim_z=2) kf2.F = array ([[1., dt], [0., 1.]]) kf2.H = array ([[1., 0.], [1., 0.]]) kf2.x = array ([[0.], [0.]]) kf2.Q = array ([[dt**3/3, dt**2/2], [dt**2/2, dt]]) * 0.02 kf2.P *= 100 kf2.R[0,0] = wheel_sigma**2 kf2.R[1,1] = gps_sigma**2 random.seed(SEED) xs = [] zs = [] nom = [] for i in range(1, 100): m0 = i + randn()*wheel_sigma m1 = i + randn()*gps_sigma if gps_sigma >1e40: m1 = -1e40 z = array([[m0], [m1]]) 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('fusion std: {:.3f}'.format (np.std(res))) if DO_PLOT: plt.subplot(211) plt.plot(xs[:,0]) #plt.plot(zs[:,0]) #plt.plot(zs[:,1]) plt.subplot(212) plt.axhline(0) plt.plot(res) plt.show() print(kf2.Q) print(kf2.K) return std_dev
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 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 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_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 one_run_test_fls(): fls = FixedLagSmoother(dim_x=2, dim_z=1) fls.x = np.array([0., .5]) fls.F = np.array([[1.,1.], [0.,1.]]) fls.H = np.array([[1.,0.]]) fls.P *= 200 fls.R *= 5. fls.Q *= 0.001 kf = KalmanFilter(dim_x=2, dim_z=1) kf.x = np.array([0., .5]) kf.F = np.array([[1.,1.], [0.,1.]]) kf.H = np.array([[1.,0.]]) kf.P *= 2000 kf.R *= 1. kf.Q *= 0.001 N = 4 # size of lag nom = np.array([t/2. for t in range (0,40)]) zs = np.array([t + random.randn()*1.1 for t in nom]) xs, x = fls.smooth_batch(zs, N) M, P, *_ = kf.batch_filter(zs) rts_x, *_ = kf.rts_smoother(M, P) xfl = xs[:,0].T[0] xkf = M[:,0].T[0] fl_res = abs(xfl-nom) kf_res = abs(xkf-nom) if DO_PLOT: plt.cla() plt.plot(zs,'o', alpha=0.5, marker='o', label='zs') plt.plot(x[:,0], label='FLS') plt.plot(xfl, label='FLS S') plt.plot(xkf, label='KF') plt.plot(rts_x[:,0], label='RTS') plt.legend(loc=4) plt.show() print(fl_res) print(kf_res) print('std fixed lag:', np.mean(fl_res[N:])) print('std kalman:', np.mean(kf_res[N:])) return np.mean(fl_res) <= np.mean(kf_res)
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 makeLinearKF(A, B, C, P, F, x, R = None, dim_x = 4, dim_z = 4): kf = KalmanFilter(dim_x=dim_x, dim_z=dim_z) kf.x = x kf.P = P kf.F = F kf.H = C kf.R = R return kf
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 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 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 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 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_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) 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 *= 0.0001 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 kf(s, r, q): '''Perform Kalman Filter on pandas Series Inputs: s -- Pandas series with data to filter, index is datetime r -- measurement noise variance (use 10*quiet_period_variance) q -- process noise variance (use 20) Returns: f -- Kalman Filter model ''' estimate_columns = ['z', 'x', 'dx', 'r', 'v11', 'v21', 'v12', 'v22'] estimate_df = pd.DataFrame(index=s.index, columns=estimate_columns) estimate_df = estimate_df.fillna(0) # NCV model f = KalmanFilter(dim_x=2, dim_z=1) # Initial condition f.x = np.array([s[0], s.diff()[1:5].mean()]) # State transition matrix f.F = np.array([[ 1., 1., ], [ 0, 1., ]]) # Measurement Function f.H = np.array([[1., 0]]) # Covariance Matrix f.P *= 1 # Measurement Noise Covariance f.R = r # Add noise profile f.Q = Q_discrete_white_noise(dim=2, dt=1, var=q) for k in range(0, len(estimate_df)): z = s[k] f.predict() f.update(z) estimate_df.loc[estimate_df.index[k], 'z'] = z estimate_df.loc[estimate_df.index[k], ['x', 'dx']] = list(f.x) estimate_df.loc[estimate_df.index[k], 'r'] = f.y**2 estimate_df.loc[estimate_df.index[k], ['v11', 'v21', 'v12', 'v22']] = list(np.reshape( f.P, 4)) return (estimate_df, f)
def tracker1(): tracker = KalmanFilter(dim_x=4, dim_z=2) dt = 1.0 tracker.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) tracker.u = 0 tracker.H = np.array([[1 / 0.3048, 0, 0, 0], [0, 0, 1 / 0.3048, 0]]) tracker.R = np.eye(2) * R_std**2 q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2) tracker.Q = block_diag(q, q) tracker.x = np.array([[0, 0, 0, 0]]).T tracker.P = np.eye(4) * 500 return tracker
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, 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 _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 newTracker(self, dt, var=0.06): tracker = KalmanFilter(dim_x=4, dim_z=2) tracker.x = np.array([0., 0., 0., 0.]) tracker.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) tracker.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) tracker.P = np.eye(4) * 500. tracker.R = np.array([[5., 0.], [0., 5.]]) tracker.u = 0. Q = Q_discrete_white_noise(dim=2, dt=dt, var=var) Q = block_diag(Q, Q) tracker.Q = Q return tracker
def tracker1(x_initial, R_std, Q_std): tracker = KalmanFilter(dim_x=1, dim_z=1) tracker.F = np.array([1]) tracker.u = 0. tracker.H = np.array([1]) tracker.R = R_std tracker.Q = Q_std tracker.x = np.array([x_initial]).T tracker.P = 50 return tracker
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 init_tracker(): tracker = KalmanFilter(dim_x=4, dim_z=2) dt = 1.0 # time step tracker.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) tracker.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) tracker.R = np.eye(2) * 1000 q = Q_discrete_white_noise(dim=2, dt=dt, var=1) tracker.Q = block_diag(q, q) tracker.x = np.array([[2000, 0, 700, 0]]).T tracker.P = np.eye(4) * 50.0 return tracker
def filter(data, var): rk = KalmanFilter(dim_x=2, dim_z=1) rk.F = np.array([[1., 1.], [0., 1.]]) rk.H = np.array([[1., 0.]]) initValue = data[0] # 初始位置 先设置为0 rk.x = np.array([initValue, 0]).T # 测量误差 rk.R *= var rk.P *= 10 rk.Q *= 0.001 mu, cov, _, _ = rk.batch_filter(data) M, P, C_ = rk.rts_smoother(mu, cov) return M[:, 0]
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 = [] results = [] xest = [] ks = [] 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 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 = [] results = [] xest = [] ks = [] 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 get_kalman(): kf = KalmanFilter(dim_x=2, dim_z=1) dz = 0.3 kf.x = np.array([138.0, -0.1]) kf.R = 2 kf.F = np.array([[1., dz], [0., 1.]]) kf.H = np.array([[1., 0.]]) kf.P *= [5., 2.] kf.Q = Q_discrete_white_noise(2, dz, 2.) return kf
def FilteringData(self,DataArray): #First construct the object with the required dimensionality. f = KalmanFilter(dim_x=len(DataArray), dim_z=3) f.x = np.array(DataArray)# Real data f.F = np.array([[1., 1.],[0., 1.]])#Define the state transition matrix f.H = np.array([[1., 0.]])#Define the measurement function f.P = np.array([[1000., 0.],[0., 1000.]])#Define the covariance matrix f.R = 5 #Assign the measurement noise. from filterpy.common import Q_discrete_white_noise f.Q = Q_discrete_white_noise(dim=len(DataArray), dt=0.1, var=0.13) #Assign the process noise for i in range(100): #IterationNum f.predict() f.update() return f.x
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 make_ca_filter(self, dt, R_std): cafilter = KalmanFilter(dim_x=6, dim_z=2) cafilter.x = np.array([0., 0., 0., 0., 0., 0.]) cafilter.P *= 3 cafilter.R *= np.eye(2) * R_std**2 cafilter.F = np.array( [[1, dt, 0.5 * dt * dt, 0, 0, 0], [0, 1, dt, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, dt, 0.5 * dt * dt], [0, 0, 0, 0, 1, dt], [0, 0, 0, 0, 0, 1]], dtype=float) cafilter.H = np.array([[1., 0, 0, 0, 0, 0], [0, 0, 0, 1., 0, 0]], dtype=float) q = Q_discrete_white_noise(dim=3, dt=dt, var=0.01) cafilter.Q = block_diag(q, q) return cafilter
def setup(): F = generate_F_matrix(velocity=0.001) H = np.array([[0, 1]]) sim = PlaybackSensor("data/vehicle_state.json", ["fVx", "fYawrate", "fStwAng"]) # set up kalman filter tracker = KalmanFilter(dim_x=2, dim_z=1) tracker.F = F tracker.Q = np.eye(2) * 0.001 tracker.H = H tracker.R = measurement_var tracker.x = np.array([[0, 0]]).T tracker.P = np.eye(2) * 500 return sim, tracker
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 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 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