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_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 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 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 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 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 setup_KF(x, y, z=None): flag_2d = z is None if flag_2d: kf = KalmanFilter(dim_x=4, dim_z=2) 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.x[0] = x kf.x[1] = y else: kf = KalmanFilter(dim_x=6, dim_z=3) kf.F = np.array([[1, 0, 0, 1, 0, 0], [0, 1, 0, 0, 1, 0], [0, 0, 1, 0, 0, 1], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]]) kf.H = np.array([ [1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], ]) kf.x[0] = x kf.x[1] = y kf.x[2] = z # kf.R[2:, 2:] *= 10. # kf.P[4:, 4:] *= 1000. # give high uncertainty to the unobservable initial velocities # kf.P *= 10. # kf.Q[-1, -1] *= 0.01 # kf.Q[4:, 4:] *= 0.01 kf.B = np.eye(kf.dim_u) kf.get_pred = lambda: np.dot(kf.F, kf.x) return kf
def main(): [t, dt, s, v, a, theta, omega, alpha] = build_real_values() zs = build_measurement_values(t, [a, omega]) u = build_control_values(t, v) [F, B, H, Q, R] = init_kalman(t, dt) kf = KalmanFilter(9, 3, 2) kf.x = np.array([[0., 0., 0., 0., 0., 0., 0., 0., 0.]]).T kf.B = B kf.R = R kf.F = F kf.H = H kf.Q = Q xs, cov = [], [] for zk, uk in zip(zs, u): kf.predict(u=uk) kf.update(z=zk) xs.append(kf.x) cov.append(kf.P) xs, cov = np.array(xs), np.array(cov) xground = construct_xground(s, v, a, theta, omega, alpha, xs.shape) nees = NEES(xground, xs, cov) print(np.mean(nees)) plot_results(t, xs, xground, zs, nees)
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 getKalmanFilter(m): kalman = KalmanFilter(len(m) * 2, len(m)) kalman.x = np.hstack((m, [0.0, 0.0])).astype(np.float) kalman.F = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]]) kalman.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) kalman.P *= 1000 kalman.R = 0.00001 kalman.Q = Q_discrete_white_noise(dim=4, dt=1, var=5) kalman.B = 0 return kalman
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 create_kf_and_assign_predict_update(dim_z, X, P, A, Q, dt, R, H, B, U): ''' Function creates a Kalman Filter object and assigns all the instance variables :return: Kalman Filter ''' kf = KalmanFilter(dim_x=X.shape[0], dim_z=dim_z) kf.x = X kf.P = P kf.F = A print('=======================') kf.Q = Q kf.B = B kf.U = U kf.R = R kf.H = H return kf
def 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, dim_state, dim_control, dim_measurement, initial_state_mean, initial_state_covariance, matrix_F, matrix_B, process_noise_Q, matrix_H, measurement_noise_R): filter = KalmanFilter(dim_x=dim_state, dim_u=dim_control, dim_z=dim_measurement) filter.x = initial_state_mean filter.P = initial_state_covariance filter.Q = process_noise_Q filter.F = matrix_F filter.B = matrix_B filter.H = matrix_H filter.R = measurement_noise_R # covariance matrix super().__init__(filter)
def create_kf_and_assign_predict_update(dim_z, X, P, A, Q, dt, R, H, B, U): ''' :param configs tuple: all the values to define the kalman filter :return: Kalman Filter ''' kf = KalmanFilter(dim_x=X.shape[0], dim_z=dim_z) kf.x = X kf.P = P kf.F = A print('=======================') kf.Q = Q kf.B = B kf.U = U kf.R = R kf.H = H return kf
def make_filter(start): f = KalmanFilter(dim_x=6, dim_z=3) f.x = start f.F = np.eye(6) f.F[:3, 3:6] = np.eye(3) * dt f.B = np.array([0, dt**2 / 2, 0, 0, dt, 0]) g = -9.81 f.H = np.zeros((3, 6)) f.H[:, :3] = np.eye(3) f.Q *= 0.5 f.R *= 2 f.P *= 1 return f
def tof_filter_init(self): '''ToF Filter''' dt = 0.01 # Just random assumation tof_filter = KalmanFilter(dim_x=2, dim_z=2, dim_u=1) # Set up the ToF filter tof_filter.F = np.array([ [1, dt], # The Sensor Model [0, 1] ]) tof_filter.P = np.diag([0.1, 0.1]) # covariance matrix tof_filter.B = np.array([ [0], # Control Matrix [dt] ]) tof_filter.H = np.diag([1., 1.]) # Measurement Matrix tof_filter.Q = np.diag([0.9, 0.4]) # Process covariance tof_filter.R = np.diag([ 0.02**2, 0.05**2 ]) # Measurement covariance # Noise of he sensor ~0.01m (1cm) return tof_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 createTracker(iniVector): 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, 0, 0], [0, 0, 0, 0]]) 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.]) q = Q_discrete_white_noise(dim=2, dt=dt, var=0.04**2) tracker.Q = block_diag(q, q) tracker.x = iniVector tracker.P = np.diag([3,3,3,3]) return tracker
return self.attack_sequence, Gamma if __name__ == "__main__": # ================== Filter and system generation ======================== kf = KalmanFilter(dim_x=2,dim_z=2) kf.x = [0.,0.] # Initial values state-space is [ x xdot ]' kf.H = np.array([[1.,0.], # Observation matrix [0.,1.]]) kf.F = np.array([[1., 1.], [0., 1.]]) # State transition matrix kf.R = np.eye(2) kf.Q = np.eye(2) kf.P = np.array([[1., 0.], [0., 1.]]) kf.B = np.array([[0.5, 1.]]).T xs = kf.x # Initial values for the data generation zs = [[kf.x[0],kf.x[1]]] # We are measuring both the position and velocity pos = [kf.x[0]] # Initialization of the true position values vel = [kf.x[1]] # Initialization of the true velocity values noise_std = 1. # ========================================================================== # ======== Noisy position measurements generation for 30 samples =========== for _ in range(30): last_pos = xs[0] last_vel = xs[1] new_vel = last_vel new_pos = last_pos + last_vel xs = [new_pos, new_vel] z = [new_pos + (randn()*noise_std), new_vel + (randn()*noise_std)]
plt.xlabel('x') plt.ylabel('y') f2.show() # Basic Kalman filter # https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python # 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)
def kalclip(bbox, gt, model, clippath, stats, ofpoint=0, R=500, Q=500, P=0): #define start(no need to initiate before detections) start = h.getstartindex(bbox) clipname = h.extract(clippath) #for 30fps --> 1/fps #irrelevant in my case --> set 1 (i dont rly make a reference to reality here) deltat = 1 #define what model has what representation for retransformation after: correp = [] cenrep = ["cenof", "simplecen"] asprep = ["aspof", "aspofwidth", "aspofwidthman"] std = 70 #define dimensions and model(depends on representation) if model == "simplecen": #transform representation bbox = h.corcen(bbox) gt = h.corcen(gt) #set parameters n_x, n_z = 4, 4 Kal = KalmanFilter(dim_x=n_x, dim_z=n_z) Kal.F = np.eye(n_x) Kal.H = np.zeros((n_z, n_x)) Kal.H[0][0] = Kal.H[1][1] = Kal.H[2][2] = Kal.H[3][3] = 1 Kal.Q = np.eye(n_x) * std**2 for i in range(len(stats)): Kal.R[i, i] = stats[0, 1, i]**2 #init init = np.zeros(n_x) init[:] = gt[0, :] memp = np.zeros((bbox.shape[0], n_x)) memk = np.zeros((bbox.shape[0], n_x)) elif model == "cenof": #transform representation bbox = h.corcen(bbox) gt = h.corcen(gt) #set parameters n_x, n_z, n_u = 4, 4, 2 Kal = KalmanFilter(dim_x=n_x, dim_z=n_z, dim_u=n_u) Kal.R = np.eye(n_z) * R for i in range(len(stats)): Kal.R[i, i] = stats[1, 1, i]**2 Kal.Q = np.eye(n_x) * std**2 Kal.Q[0, 0], Kal.Q[1, 1] = stats[3, 1, 0], stats[3, 1, 1] Kal.P *= P Kal.F = np.eye(n_x) Kal.H = np.zeros((n_z, n_x)) Kal.H[0, 0] = Kal.H[1, 1] = Kal.H[2, 2] = Kal.H[3, 3] = 1 Kal.B = np.eye(n_x, n_u) #init init = np.zeros(n_x) init[:4] = gt[0, :] memp = np.zeros((bbox.shape[0], n_x)) memk = np.zeros((bbox.shape[0], n_x)) elif model == "aspof": #transform representation bbox = h.corasp(bbox) gt = h.corasp(gt) #set parameters n_x, n_z, n_u = 4, 4, 2 Kal = KalmanFilter(dim_x=n_x, dim_z=n_z, dim_u=n_u) Kal.R = np.eye(n_z) * R for i in range(len(stats)): Kal.R[i, i] = stats[2, 1, i]**2 Kal.Q = np.eye(n_x) * std**2 Kal.Q[0, 0], Kal.Q[1, 1], Kal.Q[3, 3] = stats[3, 1, 0]**2, stats[3, 1, 1]**2, 0.0001 Kal.P *= P Kal.F = np.eye(n_x) Kal.B = np.eye(n_x, n_u) Kal.H = np.zeros((n_z, n_x)) Kal.H[0, 0] = Kal.H[1, 1] = Kal.H[2, 2] = Kal.H[3, 3] = 1 #init init = np.zeros(n_x) init[:4] = gt[0, :] memp = np.zeros((bbox.shape[0], n_x)) memk = np.zeros((bbox.shape[0], n_x)) elif model == "aspofwidth": #transform representation bbox = h.corasp(bbox) gt = h.corasp(gt) #set parameters n_x, n_z, n_u = 4, 4, 3 Kal = KalmanFilter(dim_x=n_x, dim_z=n_z, dim_u=n_u) Kal.R = np.eye(n_z) * R for i in range(len(stats)): Kal.R[i, i] = stats[2, 1, i]**2 Kal.Q = np.eye(n_x) * std**2 #TODO put in stats for std width from of Kal.Q[0, 0], Kal.Q[1, 1], Kal.Q[2, 2], Kal.Q[3, 3] = stats[ 3, 1, 0]**2, stats[3, 1, 1]**2, stats[3, 1, 2]**2, 0.0001 Kal.P *= P Kal.F = np.eye(n_x) Kal.B = np.eye(n_x, n_u) Kal.H = np.zeros((n_z, n_x)) Kal.H[0, 0] = Kal.H[1, 1] = Kal.H[2, 2] = Kal.H[3, 3] = 1 #init init = np.zeros(n_x) init[:4] = gt[0, :] memp = np.zeros((bbox.shape[0], n_x)) memk = np.zeros((bbox.shape[0], n_x)) elif model == "aspofwidthman": #transform representation bbox = h.corasp(bbox) gt = h.corasp(gt) #set parameters n_x, n_z, n_u = 4, 4, 3 Kal = KalmanFilter(dim_x=n_x, dim_z=n_z, dim_u=n_u) Kal.R = np.eye(n_z) * R for i in range(len(stats)): Kal.R[i, i] = stats[2, 1, i]**2 Kal.Q = np.eye(n_x) * std**2 Kal.Q[0, 0], Kal.Q[1, 1], Kal.Q[2, 2], Kal.Q[3, 3] = stats[ 3, 1, 0], stats[3, 1, 1], 1**2, 0.0001 Kal.P *= P Kal.F = np.eye(n_x) Kal.B = np.eye(n_x, n_u) Kal.H = np.zeros((n_z, n_x)) Kal.H[0, 0] = Kal.H[1, 1] = Kal.H[2, 2] = Kal.H[3, 3] = 1 #init init = np.zeros(n_x) init[:4] = gt[0, :] memp = np.zeros((bbox.shape[0], n_x)) memk = np.zeros((bbox.shape[0], n_x)) #parameters to define(try these on all representations) Kal.x = init #create new array for results of the algorithm mem = ma.array(np.zeros_like(bbox), mask=True) mem[0, :] = init for j in range(memp.shape[1]): memp[0, j] = Kal.P[j, j] for j in range(memk.shape[1]): memk[0, j] = Kal.K[j, j] for i in range(1, bbox.shape[0], 1): z = bbox[i, :] if bbox.mask[i, 0] == True: z = None if model == "cenof": vx = ofpoint[i, 0] - ofpoint[i - 1, 0] vy = ofpoint[i, 1] - ofpoint[i - 1, 1] u = np.array([vx, vy]) Kal.predict(u) elif model == "aspof": vx = ofpoint[i, 0] - ofpoint[i - 1, 0] vy = ofpoint[i, 1] - ofpoint[i - 1, 1] u = np.array([vx, vy]) Kal.predict(u) elif model == "aspofwidth": vx = ofpoint[i, 0] - ofpoint[i - 1, 0] vy = ofpoint[i, 1] - ofpoint[i - 1, 1] vwidth = ofpoint[i, 2] - ofpoint[i - 1, 2] u = np.array([vx, vy, vwidth]) Kal.predict(u) elif model == "aspofwidthman": vx = ofpoint[i, 0] - ofpoint[i - 1, 0] vy = ofpoint[i, 1] - ofpoint[i - 1, 1] vwidth = ofpoint[i, 2] - ofpoint[i - 1, 2] u = np.array([vx, vy, vwidth]) Kal.predict(u) else: Kal.predict() Kal.update(z) x = Kal.x y = x[:4] #write into resulting array mem[i, :] = y[:4] for j in range(memp.shape[1]): memp[i, j] = Kal.P[j, j] for j in range(memk.shape[1]): memk[i, j] = Kal.K[j, j] #just return result --> do back-transformation into other representations outside of this function(as well as creating visuals) if model in correp: mem = mem elif model in cenrep: mem = h.cencor(mem) elif model in asprep: mem = h.aspcor(mem) return mem, memp, memk
# Process noise (Q) # (2, 2) = square matrix abc_filter.Q = np.diag([s_noise, r_noise]) # ------- Update Variables ------- # Measurement function (H) (how do we go from state -> observed?) # (1, 2) = row vector abc_filter.H = np.array([[c, (1 - a - b)], [0, 1]]) # measurement uncertainty # (2, 2) = square matrix OR is it just uncertainty on discharge (q) abc_filter.R *= R # Control inputs (defaults) abc_filter.B = None # np.ndarray([a]) abc_filter.dim_u = 0 return abc_filter def simulate_data( original_data: pd.DataFrame, q_obs_noise: float, r_obs_noise: float ) -> pd.DataFrame: assert all(np.isin(["precipitation", "discharge_spec"], original_data.columns)) data = original_data.copy() # simulate using the ABC model true_q, true_S = abc_simulate(data["precipitation"]) data["q_true"] = true_q data["S_true"] = true_S
def kf(self,x_obj,y_obj): #list posisi objek pos_x = x_obj pos_y = y_obj kf_res_x = [] kf_res_y = [] kf_res_vx = [] kf_res_vy = [] def vel(alist): vel_list = [0.] for i in range(len(alist)): j = i+1 # print(i) # print(alist[i]) try: vel_list.append(alist[j] - alist[i]) except: pass return vel_list #list acceleration; input=vel_list def accel(alist): accel_list = [0., 0.] try: if not alist[2]: pass else: for i in range(len(alist)): j = i+1 k = j+1 accel_list.append(alist[k]-alist[j]) except: pass return accel_list #list error def sqrt_alist(alist): #input alist= _min_avg yang mau di kuadrat sqrt_list=[] for i in range(len(alist)): sqrt_list.append(round((alist[i]**2), 2)) return sqrt_list def avgpos(alist): average = sum(alist) / len(alist) average = round(average, 2) return average def avgvel(alist): average = sum(alist) / (len(alist)-1) average = round(average, 2) return average def pos_min_avg(alist): min_avg = [] try: for i in range(len(alist)): min_aver = alist[i] - avgpos(alist) min_avg.append(round(min_aver, 2)) except: pass return min_avg def vel_min_avg(alist): min_avg = [0.] for i in range(len(alist)): j = i+1 try: min_avg.append(round((alist[j] - avgvel(alist)), 2)) except: pass return min_avg #standar deviasi def stdpos(alist): #input=list yang udh di kuadrat n = len(alist) std = math.sqrt( (1/(n-1)) * sum(alist) ) return std def stdvel(alist): #input=list yang udh di kuadrat; -2 soalnya isi mulai dr index 1 n = len(alist) std = math.sqrt( (1/(n-2)) * sum(alist) ) return std vel_x = vel(pos_x) vel_y = vel(pos_y) accelx = accel(pos_x) accely = accel(pos_y) var_x = stdpos( sqrt_alist( pos_min_avg(pos_x) ) ) var_y = stdpos( sqrt_alist( pos_min_avg(pos_y) ) ) var_vx = stdvel( sqrt_alist( vel_min_avg(vel_x) ) ) var_vy = stdvel( sqrt_alist( vel_min_avg(vel_y) ) ) ### TRACKING POSITION AND VELOCITY FROM POSITIONS ### # CONSTRUCT OBJECTS DIMENSIONALITY (4x1 dengan 4x4) f = KalmanFilter( dim_x = 4, dim_z = 4 ) # ASSIGN INIT VALUES (proses mengkuti contoh soal) f.x = np.array([ pos_x[2], pos_y[2], #position vel_x[2], vel_y[2] ]) #velocity # DEF STATE TRANSITION MATRIX f.F = np.array([ [1., 0., 1., 0.], [0., 1., 0., 1.], [0., 0., 1., 0.], [0., 0., 0., 1.] ]) f.B = np.array([ [.5, 0.], [0., .5], [1., 0.], [0., 1.] ]) f.u = np.array([ accelx[2], accely[2] ]) # DEF MEASUREMENT FUNCTION f.H = np.array([ [1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.] ]) # DEF COVARIANCE MATRIX f.P = np.array([ [.9*((var_x)**2), 0., 1., 0.], [0., .9*((var_y)**2), 0., 1.], [0., 0., .9*((var_vx)**2), 0.], [0., 0., 0., .9*((var_vy)**2)] ]) # ASSIGN MEASUREMENT NOISE f.R = np.array([ [(var_x)**2, 0., 1., 0.], [0., (var_y)**2, 0., 1.], [0., 0., (var_vx)**2, 0.], [0., 0., 0., (var_vy)**2] ]) # ASSIGN PROCESS NOISE f.Q = np.zeros((4,4)) #sensor input # def sensor_read(): # a =np.array([ [231.], # [77.], #position # [ np.negative(26.)], # [4.] ]) #velocity # return a # PREDICT UPDATE LOOP for i in range(len(accelx)-3): #masi kebanyakan len nya mknya out of range z = np.array([ pos_x[i+3], pos_y[i+3], #position vel_x[i+3], vel_y[i+3] ]) #velocity try: f.u = np.array([ accelx(i+3), accely(i+3) ]) except: pass f.predict() f.update(z) kf_res_x.append(np.round(f.x[0])) kf_res_y.append(np.round(f.x[1])) kf_res_vx.append(np.round(f.x[2])) kf_res_vy.append(np.round(f.x[3])) # print('input z:') # print(z) # print('===========KF============') # print("predict: ",f.x_prior) # print("update: ",f.x) # print('=========================') print('kf_x: {}'.format(kf_res_x)) print('kf_y: {}'.format(kf_res_y)) guess_noise=.9*((var_vy)**2) # print(guess_noise) # print(var_x,var_y,var_vx,var_vy) # print(f.R) out = { 'f_R': f.R.tolist(), 'kf_x': kf_res_x, 'kf_y': kf_res_y, 'kf_vx': kf_res_vx, 'kf_vy': kf_res_vy, 'guess_noise': .9 } return out
from filterpy.kalman import KalmanFilter import numpy as np from filterpy.common import Q_discrete_white_noise import matplotlib.pyplot as plt from kf_book.book_plots import plot_measurements from kf_book.book_plots import plot_filter from scipy.linalg import block_diag dt = 1.0 R = 3 kf = KalmanFilter(dim_x=2, dim_z=1, dim_u=1) kf.P *= 10 kf.R *= R kf.Q = Q_discrete_white_noise(2, dt, 0.1) kf.F = np.array([[1, 0], [0, 1]]) kf.B = np.array([[dt], [1]]) kf.H = np.array([[1, 0]]) zs = [i + np.random.randn() * R for i in range(1, 1000)] xs = [] cmd_velocity = 1 for z in zs: kf.predict(u=cmd_velocity) kf.update(z) kf.append(kf.x[0])
"""To save some time just run the part you want""" run_example = False run_real_cases = True """ Case from: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/08-Designing-Kalman-Filters.ipynb """ if run_example: dt = 1. R = 3. kf = KalmanFilter(dim_x=2, dim_z=1, dim_u=1) kf.P *= 10 kf.R *= R kf.Q = Q_discrete_white_noise(2, dt, 0.1) kf.F = np.array([[1., 0], [0., 0.]]) kf.B = np.array([[dt], [ 1.]]) kf.H = np.array([[1., 0]]) print(kf.P) zs = [i + randn()*R for i in range(1, 100)] xs = [] cmd_velocity = 1. for z in zs: kf.predict(u=cmd_velocity) kf.update(z) xs.append(kf.x[0]) plt.clf() plt.plot(xs, label='Kalman Filter') plot_measurements(zs) plt.xlabel('time') plt.legend(loc=4) plt.ylabel('distance')
bike.update_C(C) sys = bike.discrete_ss(dt) print(sys.A) print(sys.B) print(sys.C) print(sys.D) #dt = 1/60 #t_sim, x_sim = bike.continuous_response(time, np.zeros(time.size), # x0=np.array([np.deg2rad(6), 0, 0, 0])) #measurements = x_sim[:,2] + np.random.randn(t_sim.size)*r_var #print(measurements) kf = KalmanFilter(dim_x=4, dim_z=1) kf.F = sys.A kf.B = sys.B kf.H = sys.C kf.R *= r_var kf.Q = bike.calc_process_noise(q_var, dt) kf.P *= 1 saver = common.Saver(kf) for idx in range(time.size): kf.predict() kf.update(measurements[idx]) saver.save() saver.to_array() plt.figure() plt.subplot(2, 2, 1) plt.plot(time, np.rad2deg(saver.x[:, 0, 0]))
# Generate groundtruth and measurement sequence for i in range(1, N): x_pre = xt_gt[..., i - 1].reshape(4, 1) x = (F @ x_pre + B * u).flatten() xt_gt[..., i] = x n_z = np.random.multivariate_normal([0, 0], R) zt[..., i] = x[[0, 2]] + n_z # Init Kalman filter kf = KalmanFilter(dim_x=4, dim_z=2, dim_u=1) # Assign transition matrices kf.F = F kf.H = H kf.B = B # Assgin covariance matrices kf.Q = np.zeros_like(F) kf.R = R kf.P = np.diag([16.0, 16.0, 16.0, 16.0]) # Assign init state kf.x = x_init + np.random.randn(4, 1) * 4 # Run Kalman filter xt_est = np.empty_like(xt_gt) for i in range(N): kf.update(zt[..., i].reshape(2, 1)) kf.predict(u)
kl2_predict = np.zeros((N, 2)) kl2_estimate = np.zeros((N, 2)) kl_predict = np.zeros((N, 2)) kl_estimate = np.zeros((N, 2)) # Initialize: kl2_predict[0] = init_vec kl_predict[0] = init_vec kl2.x = init_vec kl2.F = F kl2.H = np.eye(2) kl2.P = P kl2.R = R kl2.Q = Q kl2.B = B.ravel() print(kl2.Q) for i in range(1, N): kl2.predict(input_f[i - 1] / m_train) kl2_predict[i] = kl2.x.ravel() kl2.update(train_measured[i]) kl2_estimate[i] = kl2.x.ravel() kl_predict[i] = kl.predict(input_f[i - 1] / m_train) kl_estimate[i] = kl.update(train_measured[i]) _, ax = plt.subplots(figsize=(12, 9)) ax.scatter(times, train_measured_x, marker='+') ax.plot(times, x_train, lw=1, c='black') ax.plot(times, kl2_predict[:, 0], label='predict')
x_est = [] # pos x y_est = [] # pos y vx_est = [] # vel x vy_est = [] # vel y x_est.append(0) y_est.append(0) vx_est.append(0) vy_est.append(0) my_filter = KalmanFilter(dim_x=4, dim_z=2) my_filter.x = np.array([[0.], [0.], [0.], [0.]]) my_filter.B = np.array([[(delta_t**2) / 2, 0], [0, (delta_t**2) / 2], [delta_t, 0], [0, delta_t]]) my_filter.F = np.array([[1., 0., delta_t, 0.], [0., 1., 0., delta_t], [0., 0., 1., 0.], [0., 0., 0., 1.]]) # state transition matrix my_filter.H = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.]]) # Measurement function my_filter.P = np.array( [ [100., 0., 0., 0.], [0., 100., 0., 0.], [0., 0., 100., 0.], [0., 0., 0., 100.], ]
powerrec = np.array([]) u_array = np.array([[0], [0], [0], [0]]) ref_array = np.array([[0], [0], [0], [0]]) integral_array = np.array([[0], [0], [0], [0]]) ucontrolrec = np.array([]) roll_ref = 0 pitch_ref = 0 gpsd = None ## Kalman Filter Initialization kf = KalmanFilter(dim_x=4, dim_z=4) kf.x = np.array([[0], [0], [0], [0]]) kf.F = np.array([[1, 0, 0, 0], [DT, 1, 0, 0], [0, 0, 1, 0], [0, 0, DT, 1]]) kf.B = np.array([[0.0001, -0.0001, -0.0001, 0.0001], [0.0000007, -0.0000007, -0.0000007, 0.0000007], [0.0001, 0.0001, -0.0001, -0.0001], [0.0000007, 0.0000007, -0.0000007, -0.0000007]]) kf.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) kf.P = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) kf.Q = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) kf.R = np.array([[200, 0, 0, 0], [0, 1000, 0, 0], [0, 0, 200, 0], [0, 0, 0, 1000]]) # default is 1000, 10000 klqr1 = 20 # default 20 klqr2 = 40 # default 80 klqr = np.array([[klqr1, klqr2, klqr1, klqr2], [-klqr1, -klqr2, klqr1, klqr2], [-klqr1, -klqr2, -klqr1, -klqr2], [klqr1, klqr2, -klqr1, -klqr2]]) ## Functions & Classes
import psycopg2 import yaml conn = psycopg2.connect(database="track", user="******", password="******", host="172.17.0.3", port="5432") cur = conn.cursor() dt = 1 u_noise = 0.010081 #standard deviation my_filter = KalmanFilter(dim_x=4, dim_z=2) my_filter.x = np.array([[0.], [0.], [0.], [0.]]) my_filter.B = np.array([[(dt ** 2) / 2], [(dt ** 2) / 2], [dt], [dt]]) my_filter.F = np.array([[1., 0., dt, 0.], [0., 1., 0., dt], [0., 0., 1., 0.], [0., 0., 0., 1.]]) # state transition matrix my_filter.H = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.]]) # Measurement function my_filter.P = np.array([[3., 0., 0., 0.], [0., 3., 0., 0.], [0., 0., 3., 0.], [0., 0., 0., 3.]]) # covariance matrix. The terms along the main diagonal of P are the variances associated with the corresponding terms # in the state vector. The off-diagonal terms of P provide the covariances between terms in the state vector my_filter.R = [[1.*(3.**2), 0.],
from filterpy.kalman import KalmanFilter import numpy as np from filterpy.common import Q_discrete_white_noise import matplotlib.pyplot as plt from kf_book.book_plots import plot_measurements from kf_book.book_plots import plot_filter from scipy.linalg import block_diag import TEMP as tm dt = 2 kf = KalmanFilter(dim_x=2, dim_z=2, dim_u=1) kf.P = np.array([[1, 0], [0, 1]]) kf.R = 0.05 kf.Q = np.array([[0.001, 0], [0, 0.003]]) kf.F = np.array([[1, -1 * dt], [0, 1]]) kf.B = np.array([[dt], [0]]) kf.H = np.array([[1, 0], [0, 1]]) z_ang, z_vel, zs_ang, zs_vel = tm.Issue_1(50) zs = [] z = [] for i in range(len(z_ang)): zs.append([zs_ang[i], 0.01]) z.append([z_ang[i], 0.01]) ut = np.array(z_vel).reshape(-1, 1) zs = np.array(zs) z = np.array(z) result = [] for i in range(len(zs)):
plt.plot(xs, ys) plt.title('Measurements') plt.xlabel('x') plt.ylabel('y') f2.show() # Basic Kalman filter # https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python # 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')