def correct(self):
"""Runs correction step on the filter
"""
if not self.meas_queue:
# print("meas_queue is empty!")
return
x_hat_start = deepcopy(self.x_hat)
P_start = deepcopy(self.P)
for meas in self.meas_queue:
if DEBUG:
print("Fusing " + meas.__class__.__name__ + " of " +
str(meas.src_id) + " w/ data: " + str(meas.data))
print("State of Filter:")
print("x_hat")
print(self.x_hat)
print("P")
print(self.P)
R = meas.R
if isinstance(meas, Explicit):
C = get_measurement_jacobian(meas, self.x_hat, self.num_states,
self.world_dim,
self.num_ownship_states)
K = self._get_kalman_gain(C, R)
innovation = self._get_innovation(meas, C).reshape(1, 1)
self.x_hat += np.dot(K, innovation)
tmp = np.eye(self.num_states) - np.dot(K, C)
self.P = tmp.dot(self.P)
# self.P = np.dot( np.dot(tmp, self.P), tmp.T) + K.dot(R_mat.dot(K.T))
else: # Implicit Update
C = get_measurement_jacobian(meas, self.x_hat, self.num_states,
self.world_dim,
self.num_ownship_states)
mu, Qe, alpha = self._get_implicit_predata(
C, R, x_hat_start, P_start, meas)
if Qe < 0:
print(C)
print(P_start.dot(C.T))
print(x_hat_start)
raise Exception("Problematic Qe!")
z_bar, curly_theta = self._get_implicit_data(
meas.et_delta, mu, Qe, alpha)
if meas.et_delta == 12:
print("PROBLEMATIC MULTIPLIER")
print(meas)
K = self._get_kalman_gain(C, R)
if meas.is_angle_meas:
z_bar = normalize_angle(z_bar)
self.x_hat += np.dot(K, z_bar)
self.P = self.P - curly_theta * K.dot(C.dot(self.P))
self.x_hat = normalize_all_angles(self.x_hat,
self.num_ownship_states,
self.num_assets, self.world_dim)
# Clear measurement queue
self.meas_queue = []
def check_implicit(self, meas):
"""Checks if a measurement can be fused implicitly
Arguments:
meas {etddf.measurements.Explicit} -- Measurement to be checked
Raises:
TypeError: meas is not of type Explicit
Returns:
bool -- True for implicit / False for explicit
"""
# TODO add support for scaling innovation check by the uncertainty
if not isinstance(meas, Explicit):
raise TypeError("meas must of type Explicit")
if meas.is_angle_meas:
meas.data = normalize_angle(meas.data)
C = get_measurement_jacobian(meas, self.x_hat, self.num_states,
self.world_dim, self.num_ownship_states)
innovation = self._get_innovation(meas, C)
return np.abs(innovation) <= meas.et_delta, innovation