def _get_innovation(self, meas, C):
expected_meas = None
if meas.is_linear_meas:
expected_meas = C.dot(self.x_hat)
else:
expected_meas = get_nonlinear_expected_meas(
meas, self.x_hat, self.world_dim, self.num_ownship_states)
# if not meas.is_linear_meas:
# print("expected meas " + meas.__class__.__name__ + " : " + str(expected_meas))
# print("---> actual meas: " + str(meas.data))
if meas.is_angle_meas:
return normalize_angle(meas.data - expected_meas)
else:
return meas.data - expected_meas
def _get_implicit_predata(self, C, R, x_hat_start, P_start, meas):
x_ref = self._get_common_filter_states(meas.src_id).x_hat
if meas.is_linear_meas:
mu = C.dot(self.x_hat - x_hat_start)
Qe = C.dot(P_start.dot(C.T)) + R
alpha = C.dot(x_ref - x_hat_start)
else: # Nonlinear Measurement
mu0 = get_nonlinear_expected_meas(meas, self.x_hat, self.world_dim,
self.num_ownship_states)
mu1 = get_nonlinear_expected_meas(meas, x_hat_start,
self.world_dim,
self.num_ownship_states)
mu = mu0 - mu1
Qe = np.abs(C.dot(P_start.dot(C.T)) + R)
alpha0 = get_nonlinear_expected_meas(meas, x_ref, self.world_dim,
self.num_ownship_states)
alpha1 = get_nonlinear_expected_meas(meas, x_hat_start,
self.world_dim,
self.num_ownship_states)
alpha = alpha0 - alpha1
if meas.is_angle_meas:
mu = normalize_angle(mu)
alpha = normalize_angle(alpha)
return mu, Qe, alpha