def init_filter( S0: float = 5.74, s_variance: float = 1, Q00_s_noise: float = 0.01, R: float = 1, h: Callable = hx, f: Callable = abc, params: Dict = parameters, alpha: float = 1e-3, beta: float = 2, kappa: float = 1, ) -> UKF.UnscentedKalmanFilter: """Init the ABC model kalman filter X (state mean): [S0]^T P (state uncertainty): [s_variance] ABC Model F (process transition matrix): H (measurement function): Args: S0 (float, optional): [description]. Defaults to 5.74. s_variance (float, optional): P_t=0. Defaults to 1. Q00_s_noise (float, optional): [description]. Defaults to 0.01. R (float, optional): [description]. Defaults to 1. h (Callable, optional): [description]. Defaults to hx. f (Callable, optional): [description]. Defaults to abc. params (Dict, optional): [description]. Defaults to parameters. alpha (float, optional): [description]. Defaults to 1e-3. beta (float, optional): [description]. Defaults to 2. kappa (float, optional): [description]. Defaults to 1. Returns: [UKF.UnscentedKalmanFilter]: Initialised Unscented Kalman Filter """ assert all(np.isin(["a", "b", "c"], [k for k in params.keys()])) a, b, c = params["a"], params["b"], params["c"] # generate sigma points - van der Merwe method # n = number of dimensions; alpha = how spread out; # beta = prior knowledge about distribution, 2 == gaussian; # kappa = scaling parameter, either 0 or 3-n; points = MerweScaledSigmaPoints(n=1, alpha=alpha, beta=beta, kappa=kappa) ## TODO: dt = 86400s (1day) (??) abc_filter = UKF.UnscentedKalmanFilter( dim_x=1, dim_z=1, dt=1, hx=h, fx=f, points=points ) # INIT FILTER # ------- Predict Variables ------- # State Vector (X): storage and rainfall # (2, 1) = column vector abc_filter.x = np.array([S0]) # State Covariance (P) initial estimate # (2, 2) = square matrix abc_filter.P[:] = np.diag([s_variance]) # Process noise (Q) # (2, 2) = square matrix abc_filter.Q = np.diag([Q00_s_noise]) # ------- Update Variables ------- # 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
x_ukf = [] x_sensor = [] sp = JulierSigmaPoints(Ns,5) def state_tran(x, dt): A = np.eye(12) for i in range(3): A[i,i+3] = dt A[i+6,i+9] = dt return np.dot(A,x) def obs_state(x): A = np.zeros((6,12)) for i in range(3): A[i,i+3] = 1 A[i+3, i + 9] = 1 return np.dot(A,x) ukf_filter = UKF.UnscentedKalmanFilter(dim_x=Ns,dim_z=6, dt=0.01, hx = obs_state, fx = state_tran, points = sp) ukf_filter.Q = Q ukf_filter.R = R ukf_filter.P = P for i, state in enumerate(sim): ukf_filter.predict() Rot = np.reshape(state[6:15],(-1,9)) Rot_e = rot_eul(Rot) noise = np.zeros(Ns) noise[:3] = r*(0.5-np.random.random(3)) x_obs = np.concatenate((state[:6],np.reshape(Rot_e,(3,)),state[-3:])) # + noise x_obs[:3] += r*(0.5 - np.random.random(3)) x_sensor.append(x_obs) ukf_filter.update(obs_state(x_obs)) x = ukf_filter.x x_ukf.append(x)
return x def hx2d(prior_sigma, P_matrix=False): a, b, c = params["a"], params["b"], params["c"] H = np.array([[c, (1 - a - b)], [0, 1]]) if P_matrix: z_sigma = (H @ prior_sigma) @ np.transpose( H ) # np.dot(np.dot(H, prior_sigma), np.transpose(H)) else: # default z_sigma = np.dot(H, prior_sigma) return z_sigma ## TODO: dt = 86400s (1day) (??) abc_filter = UKF.UnscentedKalmanFilter( dim_x=2, dim_z=2, dt=1, hx=hx2d, fx=fx2d, points=points ) # INIT FILTER # ------- Predict Variables ------- # State Vector (X): storage and rainfall # (2, 1) = column vector abc_filter.x = np.array([S0, r0]).T # State Covariance (P) initial estimate # (2, 2) = square matrix abc_filter.P[:] = np.diag([s_variance, r_variance]) # Process noise (Q) # (2, 2) = square matrix abc_filter.Q = np.diag([Q00_s_noise, Q11_r_noise])
def __init__(self, Z, rbirth=1.): ''' Initialize a track for an unassociated measurement. Inputs: z is the dictinary consisting of Z['state'] is the [x, y, z, w, h, r] measurement vector from measurement fusion. Z['cls_prob'] is the classification probability from video detector. Z['conf_prob] is the confidence probability of radar detection. Z['object_class'] is the predicted class of this object from video object detector. rbirth is the target birth probability from the given Z. ''' # class/type of the track self.obj_class = Z['object_class'] if (self.obj_class == 'Pedestrian'): motion_noise = np.tile([person_white_noise_acc_var_x, person_white_noise_acc_var_y, person_white_noise_acc_var_z,],2) else: motion_noise = np.tile([vehicle_white_noise_acc_var_x, vehicle_white_noise_acc_var_y, vehicle_white_noise_acc_var_z,],2) self.motion_model = CVModel(motion_noise, step_dt) self.measurement_model = measModel(detect_var_x, detect_var_y, detect_var_z, radar_var_r) self.cam_R, self.radar_R = self.measurement_model.get_R() # measurement noise cov matrix self.R = np.concatenate((self.cam_R, self.radar_R)) # measurement vector z = Z['state'] # initial state vector using video detection x0 = np.array([*z[0:3], *unobserved_states_est],dtype=np.float32) # set the initial system covariance matrix P0 P0 = np.zeros(shape=(state_dim, state_dim), dtype=np.float32) P0[0:3, 0:3] = cam_cov_coeff_xyz * self.cam_R[0:3, 0:3] P0[3:, 3:] = cam_cov_coeff_vel * self.cam_R[0:3, 0:3] # set a unique track Id self.trackId = track.id_counter # increase the track Id by 1 track.id_counter += 1 # update initial state by associated radar detection if (z.shape[0] == meas_dim): self.filt = ukf.UnscentedKalmanFilter(dt=step_dt, dim_x=state_dim, dim_z=1, points=points, fx=None, hx=self.measurement_model.g) # a priori states and its covariance self.filt.x = x0 self.filt.P = P0 # calculate sigma points for given mean and covariance self.filt.sigmas_f = self.filt.points_fn.sigma_points(x0, P0) # range update via UKF self.filt.update(z[-1].reshape(1)) # updated states and its covariance x0 = self.filt.x P0 = self.filt.P logllk = self.filt.log_likelihood # compute <lambda(x),g*pD(x)> where lambda(x)=cls_prob*fx and pD(x)=pD*conf_prob pD_pConf_llk = Z['cls_prob']*Z['conf_prob']*pD*exp(logllk) # update of the detected Poisson target weight = cI + pD_pConf_llk exist_prob = pD_pConf_llk / weight if ( rbirth > pexist): self.status = track_status.EXISTING else: self.status = track_status.NEWBORN else: #no radar detection weight = 1. # cls_prob = E[lambda(x)] exist_prob = Z['cls_prob'] self.status = track_status.NEWBORN self.filt = ukf.UnscentedKalmanFilter(dt=step_dt, dim_x=state_dim, dim_z=meas_dim, points=points, fx=None, hx=self.measurement_model.h) # set measurement covariance matrix self.filt.R = self.R self.target = target_hypothesis(x0, Z['bbox_dims'], P0, exist_prob, weight) if (self.obj_class == 'Pedestrian'): self.gating_threshold = gate_distance_pedestrian else: self.gating_threshold = gate_distance_vehicle