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