def one_dim_kalman_filter(z_seq, measure_std_seq, proc_std, dim_x=3, x0=None, P0=None, dt=1): # NOTE: we do not specify velocity and acceleration in predict function, # since the filter will track it assert dim_x == 2 or dim_x == 3 kf = KalmanFilter(dim_x=dim_x, dim_z=1) if dim_x == 2: # x is the state vector if x0 is None: kf.x = np.array([z_seq[0], 0]) else: assert x0.shape[0] == dim_x kf.x = x0 # P is the covariance matrix of state if P0 is None: kf.P = np.array([[1, 0], [0, 1]]) else: assert P0.shape[0] == dim_x and P0.shape[1] == dim_x kf.P = P0 # F is the state transmition function kf.F = np.array([[1, dt], [0, 1]]) # H is the measurement function kf.H = np.array([[1, 0]]) else: if x0 is None: kf.x = np.array([z_seq[0], 0, 0]) else: assert x0.shape[0] == dim_x kf.x = x0 if P0 is None: kf.P = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) else: assert P0.shape[0] == dim_x and P0.shape[1] == dim_x kf.P = P0 kf.F = np.array([[1, dt, 0.5 * dt**2], [0, 1, dt], [0, 0, 1]]) kf.H = np.array([[1, 0, 0]]) # Q is the covariance matrix of process noise kf.Q = Q_discrete_white_noise(dim=dim_x, dt=dt, var=proc_std**2) # TODO: change the scale of R?? std_scale = 1 R_seq = (measure_std_seq * std_scale)**2 x_seq, cov_seq = [kf.x], [kf.P] # z is the measurement vector # R is the covariance matrix of measurement noise for z, R in zip(z_seq[1:], R_seq[1:]): kf.predict() kf.update(z, R=np.array([[R]])) x_seq.append(kf.x) cov_seq.append(kf.P) x_seq, cov_seq = np.array(x_seq), np.array(cov_seq) return x_seq[:, 0]
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(self, x, dt): ''' dt is time difference betweeen two frames ''' kf = KalmanFilter(dim_x=4, dim_z=2) kf.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) kf.u = 0. kf.H = np.array([ [ 1, 0, 0, 0 ], # for initialization of R we should take the measurement errors for position [0, 0, 1, 0] ]) kf.R = np.diag( [0.01, 0.01] ) # for initialization of R we should take the measurement errors for position vel_max = 10 # maximum velocity of a human q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01) # here var is changable kf.Q = block_diag(q, q) kf.x = x variance_of_vel = np.diag([vel_max**2, vel_max**2]) kf.P = block_diag( kf.R[0, 0], variance_of_vel[0, 0], kf.R[1, 1], variance_of_vel[ 1, 1]) #initialize P with R and square of maximum velocity return kf
def kfilter(self): tracker = KalmanFilter(dim_x=8, dim_z=8) dt = 1.0 # time step d3 = 0.0 #d-triangle, correlation between VXs dc = 0.0 #d-circle, correlation between VYs a = 1 #cx1=1,cx2=0.5,cx3=0.25 tracker.F = np.array([ #x1 vx y1 vy x2 vx2 y2 vy2 [1, dt, 0, 0, a, 0, 0, 0], [0, 1, 0, 0, 0, a, 0, 0], [0, 0, 1, dt, 0, 0, a, 0], [0, 0, 0, 1, 0, 0, 0, a], [0, 0, 0, 0, 1, dt, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, dt], [0, 0, 0, 0, 0, 0, 0, 1] ]) tracker.u = 0. tracker.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], [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]]) tracker.R = np.eye(8) * self.R_std**2 q = Q_discrete_white_noise(dim=2, dt=dt, var=self.Q_std**2) tracker.Q = block_diag(q, q) #tracker.Q.dim = 4 tracker.Q = block_diag(tracker.Q, tracker.Q) #tracker.Q.dim = 8 v = 0.00001 tracker.x = np.array([[0, v, 0, v, 0, v, 0, v]]).T tracker.P = np.eye( 8 ) * 225. # (3*5)^2, 25,35,75,85 araliginda 5er li dagilim in 3 sigma variansi. return tracker
def initKalman(self, X, Y, dt_average): ''' Description : Input : Output : ''' k_filter = KalmanFilter(dim_x=4, dim_z=2) dt = dt_average #time step in seconds k_filter.x = np.array([X[0], X[1] - X[0], Y[0], Y[1] - Y[0] ]).T # initial state (x and y position) k_filter.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) # state transition matrix q = Q_discrete_white_noise(dim=2, dt=dt, var=np.cov(X, Y)[0][0]) # process uncertainty k_filter.Q = block_diag(q, q) k_filter.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) # Measurement function k_filter.R = np.cov(X, Y) # state uncertainty P = np.eye(4) covar = np.cov(X, Y) P[0][0] = covar[0][0] P[1][1] = covar[1][0] P[2][2] = covar[0][1] P[3][3] = covar[1][1] k_filter.P = P # covariance matrix return k_filter
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 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 createTracker(iniVector): """ Creates the Kalman filter matrices to track and predict the Thymio's trajectory :param iniVector: initial position and velocity state of the robot :return: object tracker of class KalmanFilter with all Kalman matrices """ tracker = KalmanFilter(dim_x=4, dim_z=4) dt = 0.1 # time step tracker.F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) tracker.B = np.array([[dt, 0], [0, dt], [1, 0], [0, 1]]) tracker.u = np.array([[1.],[1.]]) tracker.H = np.diag([1.,1.,1.,1.]) R = np.diag([5.,5.,3.,3.]) tracker.Q = np.diag([1.3,1.3,3.5,3.5]) tracker.x = iniVector tracker.P = np.diag([1,1,1,1]) return tracker
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 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 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 pva_kalman_filter(): kf = KalmanFilter(dim_x=3, dim_z=1) # Time Step dt = 0.1 # Control Inputs kf.u = 0 # Measurement Function (we are only recording position) kf.H = np.array([[1, 0, 0]]) # State Variable kf.x = np.array([[0, 0, 0]]).T # Measurement Noise kf.R = 20 # Process/Motion Noise kf.Q = np.eye(3) * .1 # Covariance Matrix kf.P = np.eye(3) * 1 # State Transition Function kf.F = np.array([[1., dt, .5 * dt * dt], [0., 1., dt], [0., 0., 1.]]) ## --------------- PREDICT / UPDATE -----------## for z in mag_compass: kf.predict() kf.update(z) pys.append(kf.x[0])
def xy_of_filter_init(self): '''XY Filter''' KFXY = KalmanFilter(dim_x=4, dim_z=2, dim_u=1) # Set up the XY filter KFXY.x = np.array( [ [0], #dx(pitch) [0], #dy(roll) [0], #vx(pitch) [0] ], #vy(roll) dtype=float) KFXY.F = np.diag([1., 1., 1., 1.]) KFXY.P = np.diag([.9, .9, 1., 1.]) KFXY.B = np.diag([1., 1., 1., 1.]) KFXY.H = np.array([[0, 0, 1., 0], [0, 0, 0, 1.]]) KFXY.Q *= 0.1**2 KFXY.R *= 0.01**2 KFXY_z = np.array( [ [0.], # Update value of the XY filter [0.] ], dtype=float) KFXY_u = np.array( [ [0.], # Control input for XY filter [0.], [0.], [0.] ], dtype=float) return KFXY, KFXY_z, KFXY_u # Filter, Filter_Sensor_varible, Control_input
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_filter(dt): # Establish Kalman Filter with 4 dimensions (x, xdot, y, ydot) and 2 measurements (x, y) f = KalmanFilter(dim_x=4, dim_z=2) # set state transition matrix (our best linear model for the system) f.F = np.array([[1., 0., dt, 0.], [0., 1., 0., dt], [0., 0., 1., 0.], [0., 0., 0., 1.]]) # set control transition matrix f.B = np.array([[0], [-.5 * pow(dt, 2)], [0], [-dt]]) # define the measurement function (what our measurement, z measures) f.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) # define initial covariance. We are not confident in our initial state, so we make it relatively high # The initial position x,y have a much lower variance then the initial velocity # no covariance between x and y directions sigma_sq_cov = 0.1 f.P = sigma_sq_cov * np.array([[.1, 0, 0.4472, 0], [0, .1, 0, 0.4472], [0.4472, 0, 2, 0], [0, 0.4472, 0, 2]]) # define process covariance matrix (covariance of the error in our model) # Set covariance between x and y dimensional variables to 0. Calculate other covariances as normal f.Q = Q_discrete_white_noise(dim=4, dt=dt, var=0.1) # define measurement covariance matrix (covariance of the error in our measurement) sigma_sq_measurement = .0001 f.R = sigma_sq_measurement * np.array([[1, 0], [0, 1]]) return f
def get_linear_tracker(): # KF related dt = IMSHOW_SLEEP_TIME / 1000 # time step R_std = 0.35 Q_std = 0.04 M_TO_FT = 1 / 0.3048 tracker = KalmanFilter(dim_x=4, dim_z=2) tracker.F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) tracker.H = np.array([[M_TO_FT, 0, 0, 0], [0, M_TO_FT, 0, 0]]) tracker.R = np.eye(2) * R_std**2 q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2) tracker.Q[0, 0] = q[0, 0] tracker.Q[1, 1] = q[0, 0] tracker.Q[2, 2] = q[1, 1] tracker.Q[3, 3] = q[1, 1] tracker.Q[0, 2] = q[0, 1] tracker.Q[2, 0] = q[0, 1] tracker.Q[1, 3] = q[0, 1] tracker.Q[3, 1] = q[0, 1] tracker.x = np.array([[0, 0, 0, 0]]).T tracker.P = np.eye(4) * 500. return tracker
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 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 kalman_filter_clean_algorithm(iq_mat): plt.figure() plt.imshow(iq_mat) plt.savefig("before_kalman_filter") Filter = KalmanFilter(dim_x=iq_mat.shape[0], dim_z=iq_mat.shape[0]) Filter.x = np.copy(iq_mat[:, 0]) Filter.H = np.eye(iq_mat.shape[0]) Filter.P = np.eye(iq_mat.shape[0]) * 0.1 Filter.Q = np.eye(iq_mat.shape[0]) * 0.1 for slow_axis_index in range(1, iq_mat.shape[1]): plt.figure() plt.plot(Filter.x) plt.savefig("kalman_filter_estimation") Filter.predict() Filter.update(z=np.copy(iq_mat[:, slow_axis_index])) for slow_axis_index in range(0, iq_mat.shape[1]): iq_mat[:, slow_axis_index] = iq_mat[:, slow_axis_index] - Filter.x plt.figure() plt.imshow(iq_mat) plt.savefig("after_kalman_filter") return iq_mat
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 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_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 initialize_filter(self, x_inicial): filtro = KalmanFilter(dim_x=2, dim_z=1) filtro.x = np.array([[x_inicial],[0.]]) filtro.F = np.array([[1.,1.],[0.,1.]]) filtro.H = np.array([[1.,0.]]) filtro.P = np.array([[10000.,0.],[0.,100.]]) filtro.R = 5.**2 filtro.Q = np.array([[0.0001,0.],[0.,0.00001]]) return filtro
def init_kalman_filter(initValue): my_filter = KalmanFilter(dim_x=2, dim_z=1) # initial state (location and velocity) my_filter.x = np.array([[initValue], [0.]]) my_filter.F = np.array([[1., 1.], [0., 1.]]) # state transition matrix my_filter.H = np.array([[1., 0.]]) # Measurement function my_filter.P = np.array([[1, 0.], [0., 1]]) # covariance matrix my_filter.R = np.array([[1000.]]) # Measurement noise my_filter.Q = np.array([[1, 1], [1, 1]]) # process noise return my_filter
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_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 kalman_initialise(self, dim_x=3, dim_z=2, dt=0.5): kfilter = KalmanFilter(dim_x, dim_z) kfilter.x = np.array([88., 40., 0.]) kfilter.F = np.array([[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]], dtype=float) kfilter.H = np.array([[1, 0, 0], [0, 1, 0]], dtype=float) kfilter.P = Q_discrete_white_noise(dim_x, 1, 1) kfilter.Q = 0.01 * np.array([[0.1, 0, 0], [0, 10, 0], [0, 0, 100]], dtype=float) kfilter.R = np.array([[1, 0], [0, 1]], dtype=float) return kfilter
def run_tracker(): # parameters filter_misestimation_factor = 1.0 sample_size = 100 used_taps = int(sample_size * 0.5) measurement_std = 3.5 # set up sensor simulator dt = 0.1 measurement_std_list = np.asarray([measurement_std] * sample_size) sim = SensorSim(0, 0.1, measurement_std_list, 1, timestep=dt) # set up kalman filter tracker = KalmanFilter(dim_x=2, dim_z=1) tracker.F = np.array([[1, dt], [0, 1]]) q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01) tracker.Q = q tracker.H = np.array([[1, 0]]) tracker.R = measurement_std**2 * filter_misestimation_factor tracker.x = np.array([[0, 0]]).T tracker.P = np.eye(2) * 500 # perform sensor simulation and filtering readings = [] truths = [] mu = [] residuals = [] for _ in measurement_std_list: reading, truth = sim.read() readings.extend(reading.flatten()) truths.extend(truth.flatten()) tracker.predict() tracker.update(reading) mu.extend(tracker.x[0]) residual_posterior = reading - np.dot(tracker.H, tracker.x) residuals.extend(residual_posterior[0]) # error = np.asarray(truths) - mu # plot_results(readings, mu, error, residuals) # perform estimation cor = Correlator(residuals) correlation = cor.autocorrelation(used_taps) R_approx = estimate_noise_approx(correlation[0], tracker.H, tracker.P, 'posterior') abs_err = measurement_std**2 - R_approx rel_err = abs_err / measurement_std**2 print("True: %.3f" % measurement_std**2) print("Filter: %.3f" % tracker.R) print("Estimated (approximation): %.3f" % R_approx) print("Absolute error: %.3f" % abs_err) print("Relative error: %.3f %%" % (rel_err * 100)) print("-" * 15) return rel_err
def create_linear_kalman_filter_1d(dt, discrete_white_noise_var, r, x, p): f = np.array([[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]]) q = Q_discrete_white_noise(dim=3, dt=dt, var=discrete_white_noise_var) tracker = KalmanFilter(dim_x=3, dim_z=1) tracker.F = f tracker.Q = q tracker.x = np.array([x]).T tracker.P = p tracker.R = np.eye(1) * r tracker.H = np.array([[1.0, 0.0, 0.0]]) 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 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 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 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(): 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 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 FirstOrderKF(R, Q, dt): """ Create first order Kalman filter. Specify R and Q as floats.""" kf = KalmanFilter(dim_x=4, dim_z=4) kf.x = np.array([[0, 0, 0, 0]]).T kf.P = np.eye(4) * 0.0001 kf.R = [[0.04017680, 0., 0., 0.], [0., 0.04082702, 0., 0.], [0., 0., 0.0002017680, 0.], [0., 0., 0., 0.0002017680]] #np.eye(3) * R kf.Q = np.eye(4) * Q kf.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) kf.H = np.array([[0, 1., 0, 0], [0, 0, 0, 1.], [1., 0, 0, 0], [0, 0, 1., 0]]) return kf
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 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 box_kalman_filter(initial_state, Q_std, R_std): """ Tracks a 2D rectangular object (e.g. a bounding box) whose state includes position, velocity, and dimensions. Parameters ---------- initial_state : sequence of floats [x, vx, y, vy, w, h] 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=6, dim_z=4) 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. Set coefficients for x,y,w,h to 1.0. kf.H = np.zeros([kf.dim_z, kf.dim_x]) kf.H[0, 0] = kf.H[1, 2] = kf.H[2, 4] = kf.H[3, 5] = 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) # assume width and height are uncorrelated q_wh = np.diag([Q_std**2, Q_std**2]) kf.Q = block_diag(q, q, q_wh) return kf
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 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(self): 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.u = 0. tracker.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) tracker.R = np.eye(2) * self.R_std**2 q = Q_discrete_white_noise(dim=2, dt=dt, var=self.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 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 test_noisy_11d(): 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 measurements = [] results = [] zs = [] for t in range(100): # create measurement = t plus white noise z = t + random.randn() * 20 zs.append(z) # perform kalman filtering f.update(z) f.predict() # save data results.append(f.x[0]) measurements.append(z) # now do a batch run with the stored z values so we can test that # it is working the same as the recursive implementation. # give slightly different P so result is slightly different f.x = np.array([[2.0, 0]]).T f.P = np.eye(2) * 100.0 m, c, _, _ = f.batch_filter(zs, update_first=False) # plot data if DO_PLOT: p1, = plt.plot(measurements, "r", alpha=0.5) p2, = plt.plot(results, "b") p4, = plt.plot(m[:, 0], "m") p3, = plt.plot([0, 100], [0, 100], "g") # perfect result plt.legend([p1, p2, p3, p4], ["noisy measurement", "KF output", "ideal", "batch"], loc=4) plt.show()
def const_vel_filter(dt, x0=0, x_ndim=1, P_diag=(1., 1.), R_std=1., Q_var=0.0001): """ helper, constructs 1d, constant velocity filter""" f = KalmanFilter(dim_x=2, dim_z=1) if x_ndim == 1: f.x = np.array([x0, 0.]) else: f.x = np.array([[x0, 0.]]).T f.F = np.array([[1., dt], [0., 1.]]) f.H = np.array([[1., 0.]]) f.P = np.diag(P_diag) f.R = np.eye(1) * (R_std**2) f.Q = Q_discrete_white_noise(2, dt, Q_var) return f
def point_2d_kalman_filter(initial_state, Q_std, R_std): """ Parameters ---------- initial_state : sequence of floats [x0, vx0, y0, vy0] 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=4, dim_z=2) dt = 1.0 # time step # state mean (x, vx, y, vy) 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.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) # measurement matrix - maps from state space to observation space kf.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) # measurement noise covariance kf.R = np.eye(kf.dim_z) * R_std**2 # process noise covariance q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2) kf.Q = block_diag(q, q) return kf
def init_filter(dt): states = 18 # number of states, first and second order derivatives obs = 6 # observations filter = KalmanFilter(dim_x=states,dim_z=obs) filter.x = np.zeros([1,states]).T transition_mat = np.eye(9) for i in range(0,9): id_vel = i + 3 id_acc = i + 6 if id_vel < 9: transition_mat[i,id_vel] = dt if id_acc < 9: transition_mat[i,id_acc] = 0.5 * dt * dt zero_mat = np.zeros([9,9]) tmp1 = np.hstack([transition_mat,zero_mat]) tmp2 = np.hstack([zero_mat,transition_mat]) filter.F = np.vstack([tmp1,tmp2]) filter.H = np.zeros([obs,states]) filter.H[0,0] = 1 filter.H[1,1] = 1 filter.H[2,2] = 1 filter.H[3,9] = 1 filter.H[4,10] = 1 filter.H[5,11] = 1 filter.Q = np.eye(states) * 1e-4; # process noise filter.R = np.eye(obs) * 0.01 # measurement noise filter.P = np.eye(states) * 1e-4 # covariance post filter.u = 0. return filter
def init_kalman(): filter = KalmanFilter(dim_x=4,dim_z=2) dt = 0.01 filter.x = np.array([[0,0,0,0]]).T filter.F = np.array([[1,0,dt,0], [0,1,0,dt], [0,0,1,0], [0,0,0,1]]) #q = Q_discrete_white_noise(dim=2, dt=dt, var=0.001) #filter.Q = block_diag(q, q) filter.Q = np.eye(4) * 1e-4; # process noise print(filter.Q) filter.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) filter.R = np.array([[0.1, 0],[0, 0.1]]) # measurement noise filter.P = np.eye(4) * 1e-4 # covariance post filter.u = 0. return filter
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)
import cv2 from filterpy.kalman import KalmanFilter from filterpy.common import Q_discrete_white_noise #################################### x_kalman = KalmanFilter(dim_x=2,dim_z=1) x_kalman.x = np.array([[0.], # position [0.]]) # velocity x_kalman.F = np.array([[1.,1.], [0.,1.]]) x_kalman.H = np.array([[1.,0.]]) x_kalman.P = x_kalman.P*1000.0 #.array([[1000., 0.], # [ 0., 1000.] ]) x_kalman.R = np.array([[5.]]) x_kalman.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13) ################################### y_kalman = KalmanFilter(dim_x=2,dim_z=1) y_kalman.x = np.array([[0.], # position [0.]]) # velocity y_kalman.F = np.array([[1.,1.], [0.,1.]])
# See Chapter 08 tracker = KalmanFilter(dim_x=4, dim_z=2) dt = times[1] - times[0] tracker.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) q = Q_discrete_white_noise(dim=2, dt=dt, var=0.001) tracker.Q = block_diag(q, q) tracker.B = 0 tracker.H = np.array([[1.0, 0, 0, 0], [0 , 0, 1, 0]]) tracker.R = np.array([[0.1, 0], [0, 0.1]]) tracker.x = np.array([[x, y, 0, 0]]).T tracker.P = np.eye(4)*10. # Convert measurements to expected form # Note: n x 4 x 1 np array, measurements must be a col vector measurements = np.array([np.array([pos]).T for pos in positions]) mu, cov, _, _ = tracker.batch_filter(measurements) f3 = plt.figure() plt.scatter(xs, ys) plt.plot(mu[:,0], mu[:,2]) plt.title('Comparison of Measurements and Kalman Filter') plt.xlabel('x') plt.ylabel('y') f3.show()
def getPos(self): return [self.pos[0], self.pos[1]] tracker = KalmanFilter(dim_x=4, dim_z=2) dt = 1. 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.3084, 0, 0, 0], [0, 0, 1/0.3084, 0]]) tracker.R = np.array([[5,0], [0,5]]) tracker.Q = np.eye(4) * 0.1 tracker.P = np.eye(4) * 500 count = 80 tx, ty = [],[] xs, ys = [],[] pxs, pys = [],[] sensor = PosSensor1([0,0], (2,2), 1) for i in range(count): tp = sensor.getPos() tx.append(tp[0]*.3084) ty.append(tp[1]*.3084) pos = sensor.read() z = np.array([[pos[0]], [pos[1]]]) tracker.predict() tracker.update(z)
if __name__ == '__main__': rospy.init_node('particle_filter', anonymous=True) rospy.Subscriber("/poi", PointStamped, observation_callback) rospy.Subscriber("/ardrone/bottom/image_raw", Image, img_callback, queue_size=5) error_pub = rospy.Publisher("/error", PointStamped, queue_size=5) frame = None observation = None f = KalmanFilter(dim_x=2, dim_z=2) f.x = np.array([[0.], [0.]]) # initial state (location and velocity) f.F = np.array([[1.0002,0.], [0.,1.0002]]) # state transition matrix f.P = np.array([[10.0, 0.0], [0.0, 10.0]]) f.R = np.array([[0.0001, 0.0], [0.0, 0.0001]]) f.H = np.array([[1.0, 0.0], [0.0, 1.0]]) f.Q = Q_discrete_white_noise(dim = 2, dt = 100.0, var = 5000.0) rate = rospy.Rate(100) # 10hz cv2.namedWindow("Kalman Filter Output",1) while not rospy.is_shutdown(): print "Prior Belief :", f.x f.predict() x_bar, covar = f.get_prediction()
from numpy import dot, zeros, eye, isscalar def CO2_kf_filter(CO2, param): """filter matrices initialization for KF""" try: theta = param.theta Qparam = param.Qparam except AttributeError, e: raise e x_dim = CO2.x_dim z_dim = CO2.H_mtx.shape[0] # theta = (1.14,1e-5) kf = KalmanFilter(x_dim, z_dim) kf.P = np.zeros((x_dim, x_dim)) kf.x = np.zeros((x_dim, 1)) kf.H = CO2.H_mtx kf.F = np.identity(x_dim) kf.R = theta[1] * kf.R grid = CO2.grid # create Q Q = common.getGCF(grid, Qparam[0], Qparam[1]) kf.Q = theta[0] * theta[1] * Q return kf def CO2_hikf_filter(CO2, param): """HiKF filter matrices initialization""" try: theta = param.theta
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 fsq = SquareRootKalmanFilter (dim_x=2, dim_z=1) fsq.x = np.array([[2.], [0.]]) # initial state (location and velocity) fsq.F = np.array([[1.,1.], [0.,1.]]) # state transition matrix fsq.H = np.array([[1.,0.]]) # Measurement function fsq.P = np.eye(2) * 1000. # 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.e-12 assert abs(f.x[1,0] - fsq.x[1,0]) < 1.e-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]]).T f.P = np.eye(2)*100. m,c,_,_ = f.batch_filter(zs,update_first=False) # plot data if DO_PLOT: p1, = plt.plot(measurements,'r', alpha=0.5) p2, = plt.plot (results,'b') p4, = plt.plot(m[:,0], 'm') p3, = plt.plot ([0,100],[0,100], 'g') # perfect result plt.legend([p1,p2, p3, p4], ["noisy measurement", "KF output", "ideal", "batch"], 4) plt.show()
from numpy import random import matplotlib.pyplot as plt from filterpy.kalman import KalmanFilter if __name__ == '__main__': 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,C = fk.rts_smoother(mu, cov) # plot data p1, = plt.plot(zs,'cyan', alpha=0.5)
[0, 0, 1, 0]]) # Measurement function f.H = np.array([[1., 1., 0., 0.]]) # Measurement noise f.R = np.array([[sigma_epsilon **2]]) # Measurement noise covariance f.Q = np.diag([sigma_xi ** 2, sigma_omega ** 2, 0.0, 0.0]) # Run the recursive flow # initial state, period 1 f.x = np.array([18.0, 5, -12, -8]) # initial covariance has a nice identity default f.P = np.diag([1, 1, 1, 1]) * 1. # Here's how to match the simdkalman, which updates the initial state first! # Echo initial states f.x f.P f.K f.update(series[0]) # has to be a measurement from the right period! f.x # a posteriori estimate of x f.P # a posteriori estimate of P f.K # Now there is a K # Go to the next time point f.predict()
# -*- coding: utf-8 -*- """ Created on Thu Jun 18 09:16:54 2015 @author: rlabbe """ from filterpy.kalman import KalmanFilter kf = KalmanFilter(1, 1) kf.x[0,0] = 1. kf.P = np.ones((1,1)) kf.H = np.array([[2.]]) kf.F = np.ones((1,1)) kf.R = 1 kf.Q = 0 kf.predict() kf.update(2)